我的arduino两轮小车下位机 - 小R科技-WIFI机器人网·机器人创意工作室

/*
* 云台伺服电机设定
* 现有两个伺服电机做为云台
* 因应云台两侧及后面有东西会卡到，所以有预设转动极限角度
*/
#include <Servo.h>
Servo horiServo; // 云台水平伺服电机
const static int HORIZONTAL_SERVO_PIN = 9; // 接到PIN 9
const static int MAX_HORIZONTAL_ANGLE = 120; // 最大旋转角度 120
const static int MIN_HORIZONTAL_ANGLE = 40; // 最小旋转角度 40
const static int CENTER_HORIZONTAL_ANGLE = 80; // 初始角度 80
Servo vertServo; // Vertical camera servo
const static int VERTICAL_SERVO_PIN = 10; // 接到PIN 10
const static int MAX_VERTICAL_ANGLE = 130; // 最大仰角 130
const static int MIN_VERTICAL_ANGLE = 40; // 最小俯角 40
const static int CENTER_VERTICAL_ANGLE = 85; // 初始角度 85
/*
* 车轮直流电机设定
* 一般L298模块一个电机有三个输入，ENA、IN1、IN2，这模块只用EN与IN2，IN2直接反转進IN1
*/
const static int RIGHT_DIRECT_PIN = 7;
const static int RIGHT_POWER_PIN = 6; // Pin 5 PWM
const static int LEFT_DIRECT_PIN = 4;
const static int LEFT_POWER_PIN = 5; // Pin 6 PWM
/*
* 车灯设定
*/
const static int HEADLIGHT_PIN = 13; // 车灯接到PIN 13
/*
* 小车控制程序
* 输入0时电机不转，正负值控制电机正反转.
*/
static void carMove(int leftPower = 0, int rightPower = 0){
boolean leftDirect = HIGH,rightDirect = HIGH;
if(leftPower < 0) { // 左轮负值，取正值并反转电机方向
leftPower *= -1;
leftDirect = LOW;
}
if(rightPower < 0) { // 右轮负值，取正值并反转右轮电机方向
rightPower *= -1;
rightDirect = LOW;
}
digitalWrite(RIGHT_DIRECT_PIN, rightDirect);
digitalWrite(LEFT_DIRECT_PIN, leftDirect);
analogWrite(RIGHT_POWER_PIN, rightPower);
analogWrite(LEFT_POWER_PIN, leftPower);
}
/*
* 解译通讯协议
* 格式: FFxxxxxxFF，已定义内容见论坛
* http://www.wifi-robots.com/forum.php?mod=viewthread&tid=3546
*/
void decodeCommand(int *buffer){
Serial.print("Command: ");
Serial.print(buffer[0],HEX);
Serial.print(buffer[1],HEX);
Serial.println(buffer[2],HEX);
/*
* FF00xxxxFF 控制小车移动
* 输入1: 移动方向 2:未使用
*/
if(buffer[0] == 0x00){
switch(buffer[1]){
case 0x00: // FF0000xxFF: 停车
carMove(0,0); return;
case 0x01: // FF0001xxFF: 前进
carMove(200,200); return;
case 0x02: // FF0002xxFF: 后退
carMove(-150,-150); return;
case 0x03: // FF0003xxFF: 左旋转
carMove(-150,150); return;
case 0x04: // FF0004xxFF: 右旋转
carMove(150,-150); return;
case 0x05: // FF0005xxFF: 左前
carMove(150,200); return;
case 0x06: // FF0006xxFF: 左后
carMove(-100,-150); return;
case 0x07: // FF0007xxFF: 右前
carMove(200,150); return;
case 0x08: // FF0008xxFF: 右后
carMove(-150,-100); return;
}
}
/*
* FF01xxxxFF 控制伺服电机
* 传入1:电机编号 2:转动角度
* 上位机预设角度0至180，在此处直接转换为限制角度
*/
else if(buffer[0] == 0x01){
switch(buffer[1]){
case 0x01: // FF0101xxFF: 一号电机: 云台水平
if(buffer[2] <= 90)
horiServo.write(map(buffer[2],0,90,MIN_HORIZONTAL_ANGLE,CENTER_HORIZONTAL_ANGLE));
else
horiServo.write(map(buffer[2],90,180,CENTER_HORIZONTAL_ANGLE,MAX_HORIZONTAL_ANGLE));
break;
case 0x02: // FF0102xxFF: 八号电机: 云台垂直
if(buffer[2] <= 90)
vertServo.write(map(buffer[2],0,90,MIN_VERTICAL_ANGLE,CENTER_VERTICAL_ANGLE));
else
vertServo.write(map(buffer[2],90,180,CENTER_VERTICAL_ANGLE,MAX_VERTICAL_ANGLE));
break;
}
}
/*
* FF04xxxxFF 控制车灯
*/
else if(buffer[0] == 0x04){
switch(buffer[1]){
case 0x00: // FF0400xxFF: 关灯
digitalWrite(HEADLIGHT_PIN,LOW); break;
case 0x01: // FF0401xxFF: 开灯
digitalWrite(HEADLIGHT_PIN,HIGH); break;
}
}
}
/*
* 初始设定
*/
void setup(){
Serial.begin(9600); // 不可删除，用于读取外部指令
pinMode(RIGHT_DIRECT_PIN,OUTPUT);
pinMode(RIGHT_POWER_PIN,OUTPUT);
pinMode(LEFT_DIRECT_PIN,OUTPUT);
pinMode(LEFT_POWER_PIN,OUTPUT);
pinMode(HEADLIGHT_PIN,OUTPUT);
digitalWrite(HEADLIGHT_PIN,LOW); // 先关灯
vertServo.attach(VERTICAL_SERVO_PIN);
vertServo.write(CENTER_VERTICAL_ANGLE); // 云台垂直定位
horiServo.attach(HORIZONTAL_SERVO_PIN);
horiServo.write(CENTER_HORIZONTAL_ANGLE); // 云台水平定位
}
/*
* 读取指令
*/
void loop()
{
timer.run();
int readValue;
boolean waitCommandHead = true;
int commandIndex=0; // 快取位置指标
int buffer[3]; // 输入协议快取，扣除包头包尾仅3字节
while(true) {
readValue = Serial.read(); // 读输入
if(readValue != -1) { // -1表示无输入
if(waitCommandHead){ // 等待协议包头
if(readValue == 0xff) waitCommandHead = false; // 读取协议包头
}
else { // 读取协议中
if(readValue == 0xff){ // 读取到协议包尾
waitCommandHead = true; // 结束读取协议，回到等待协议包头
decodeCommand(buffer); // 解码协议
commandIndex = 0; // 清除快取位置指标
}
else if(commandIndex < 3) buffer[commandIndex++] = readValue; // 推入快取，位置加一
}
}
}
}

复制代码