3PA基础套件

目录

1 功能介绍
2 STEP1:组装小车
3 STEP2:调试电机
4 STEP3:安装上层板
5 STEP4: 调试超声波和舵机
6 STEP5: 整机调试

功能介绍
以3PA小车为平台,用超声波作为距离检测装置,舵机作为前方扫描器,实现一个可自动蔽障小车。

STEP1:组装小车
参照安装说明。
注意事项:

  1. 在安装说明中没有说明电机电源线连接,建议安装前先将电机的电源线焊接好。便于之后安装。

3PA 1.png

  1. 安装完底盘之后,按下图连接电机线和电源线。

电机连接
实物图

STEP2:调试电机
下载电机调试代码

int speedPin_M1 = 5; //M1 Speed Control
int speedPin_M2 = 6; //M2 Speed Control
int directionPin_M1 = 4; //M1 Direction Control
int directionPin_M2 = 7; //M1 Direction Control

void setup(){

}

void loop(){
carAdvance(100,100);
delay(1000);
carBack(100,100);
delay(1000);
carTurnLeft(250,250);
delay(1000);
carTurnRight(250,250);
delay(1000);
}

void carStop(){ // Motor Stop
digitalWrite(speedPin_M2,0);
digitalWrite(directionPin_M1,LOW);
digitalWrite(speedPin_M1,0);
digitalWrite(directionPin_M2,LOW);
}

void carAdvance(int leftSpeed,int rightSpeed){ //Move forward
analogWrite (speedPin_M2,leftSpeed); //PWM Speed Control
digitalWrite(directionPin_M1,HIGH);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M2,HIGH);
}

void carBack(int leftSpeed,int rightSpeed){ //Move backward
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M1,LOW);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M2,LOW);
}

void carTurnRight(int leftSpeed,int rightSpeed){ //Turn Right
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M1,LOW);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M2,HIGH);
}
void carTurnLeft(int leftSpeed,int rightSpeed){ //Turn Left
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M1,HIGH);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M2,LOW);
}

上电后发现,如果发现效果与代码不匹配,可以对代码做一下微调整。直到匹配为止,才进行下一步。

STEP3:安装上层板

  1. 准备材料

Cherokey RomeoBLE3.png

  1. 固定超声波位置
    可以参看超声波扫描套件的安装手册

  2. 固定舵机位置

3PA 3.png

STEP4: 调试超声波和舵机

  1. 硬件连接

超声波、舵机控制

实物图

  1. 下载代码
    下载代码之前需要安装Metro libray
    如何加载库,可见链接

#include <Servo.h>
#include <Metro.h>
Metro measureDistance = Metro(50);
Metro sweepServo = Metro(20);

unsigned long actualDistance = 0;

Servo myservo; // create servo object to control a servo
int pos = 60;
int sweepFlag = 1;

int URPWM = 3; // PWM Output 0-25000US,Every 50US represent 1cm
int URTRIG= 10; // PWM trigger pin
uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01}; // distance measure command

void setup(){ // Serial initialization
myservo.attach(9);
Serial.begin(9600); // Sets the baud rate to 9600
SensorSetup();
}

void loop(){
if(measureDistance.check() == 1){
actualDistance = MeasureDistance();
// Serial.println(actualDistance);
// delay(100);
}

if(sweepServo.check() == 1){
servoSweep();
}

}

void SensorSetup(){
pinMode(URTRIG,OUTPUT); // A low pull on pin COMP/TRIG
digitalWrite(URTRIG,HIGH); // Set to HIGH
pinMode(URPWM, INPUT); // Sending Enable PWM mode command
for(int i=0;i<4;i++){
Serial.write(EnPwmCmd[i]);
}
}

int MeasureDistance(){ // a low pull on pin COMP/TRIG triggering a sensor reading
digitalWrite(URTRIG, LOW);
digitalWrite(URTRIG, HIGH); // reading Pin PWM will output pulses
unsigned long distance=pulseIn(URPWM,LOW);
if(distance==50000){ // the reading is invalid.
Serial.print("Invalid");
}else{
distance=distance/50; // every 50us low level stands for 1cm
}
return distance;
}

void servoSweep(){
if(sweepFlag ){
if(pos>=60 && pos<=120){
pos=pos+1; // in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable 'pos'
}
if(pos>119) sweepFlag = false; // assign the variable again
}else {
if(pos>=60 && pos<=120){
pos=pos-1;
myservo.write(pos);
}
if(pos<61) sweepFlag = true;
}
}

3. 调试舵机位置 方法一:可重新安装舵盘 方法二:代码中角度做相应修改

STEP5: 整机调试
下载整机调试代码

#include <Servo.h>
#include <Metro.h>
Metro measureDistance = Metro(50);
Metro sweepServo = Metro(20);

int speedPin_M1 = 5; //M1 Speed Control
int speedPin_M2 = 6; //M2 Speed Control
int directionPin_M1 = 4; //M1 Direction Control
int directionPin_M2 = 7; //M1 Direction Control
unsigned long actualDistance = 0;

Servo myservo; // create servo object to control a servo
int pos = 60;
int sweepFlag = 1;

int URPWM = 3; // PWM Output 0-25000US,Every 50US represent 1cm
int URTRIG= 10; // PWM trigger pin
uint8_t EnPwmCmd[4]={0x44,0x02,0xbb,0x01}; // distance measure command

void setup(){ // Serial initialization
myservo.attach(9);
Serial.begin(9600); // Sets the baud rate to 9600
SensorSetup();
}

void loop(){

if(measureDistance.check() == 1){
actualDistance = MeasureDistance();
// Serial.println(actualDistance);
// delay(100);
}

if(sweepServo.check() == 1){
servoSweep();
}

if(actualDistance <= 30){
myservo.write(90);
if(pos>=90){
// carBack(100,100);
//// Serial.println("carBack");
// delay(100);
carTurnRight(150,150);
// Serial.println("carTurnRight");
delay(100);
}else{
// carBack(100,100);
//// Serial.println("carBack");
// delay(100);
carTurnLeft(150,150);
// Serial.println("carTurnLeft");
delay(100);
}
}else{
carAdvance(70,70);
// Serial.println("carAdvance");
delay(100);
}
// carBack(150,150);
}

void SensorSetup(){
pinMode(URTRIG,OUTPUT); // A low pull on pin COMP/TRIG
digitalWrite(URTRIG,HIGH); // Set to HIGH
pinMode(URPWM, INPUT); // Sending Enable PWM mode command
for(int i=0;i<4;i++){
Serial.write(EnPwmCmd[i]);
}
}

int MeasureDistance(){ // a low pull on pin COMP/TRIG triggering a sensor reading
digitalWrite(URTRIG, LOW);
digitalWrite(URTRIG, HIGH); // reading Pin PWM will output pulses
unsigned long distance=pulseIn(URPWM,LOW);
if(distance==50000){ // the reading is invalid.
Serial.print("Invalid");
}else{
distance=distance/50; // every 50us low level stands for 1cm
}
return distance;
}

void carStop(){ // Motor Stop
digitalWrite(speedPin_M2,0);
digitalWrite(directionPin_M1,LOW);
digitalWrite(speedPin_M1,0);
digitalWrite(directionPin_M2,LOW);
}

void carAdvance(int leftSpeed,int rightSpeed){ //Move forward
analogWrite (speedPin_M2,leftSpeed); //PWM Speed Control
digitalWrite(directionPin_M1,HIGH);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M2,HIGH);
}

void carBack(int leftSpeed,int rightSpeed){ //Move backward
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M1,LOW);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M2,LOW);
}

void carTurnRight(int leftSpeed,int rightSpeed){ //Turn Right
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M1,LOW);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M2,HIGH);
}

void carTurnLeft(int leftSpeed,int rightSpeed){ //Turn Left
analogWrite (speedPin_M2,leftSpeed);
digitalWrite(directionPin_M1,HIGH);
analogWrite (speedPin_M1,rightSpeed);
digitalWrite(directionPin_M2,LOW);
}
void servoSweep(){
if(sweepFlag){
if(pos>=60 && pos<=120){
pos=pos+1; // in steps of 1 degree
myservo.write(pos); // tell servo to go to position in variable 'pos'
}
if(pos>119) sweepFlag = false; // assign the variable again
}
else {
if(pos>=60 && pos<=120){
pos=pos-1;
myservo.write(pos);
}
if(pos<61) sweepFlag = true;
}
}

你的专属小车就此诞生!

Nextredirectltr.png购买3PA基础套件 (SKU:ROB0118)

标签: Arduino传感器