与mRobots连接控制小车

1. include <Microduino_Protocol.h>
2. include <Microduino_Motor.h>

1. define _DEBUG //DEBUG调试

1. define BLE_SPEED 57600 //蓝牙接口速度
2. define CHANNEL_NUM 8
3. define SAFE_TIME_OUT 250 //失控保护时间
4. define MAX_THROTTLE 255 //最大油门 < 255
5. define MAX_STEERING 512 //最大转向 < 512
6. define CHANNEL_THROTTLE 1 //油门通道
7. define CHANNEL_STEERING 0 //转向通道

Motor MotorLeft(MOTOR0_PINA, MOTOR0_PINB); Motor MotorRight(MOTOR1_PINA, MOTOR1_PINB);

1. define ProSerial Serial

ProtocolSer bleProtocol(&ProSerial, 16); //采用ProSerial，数据长度为16个字节

/////////////////////////////////////////////////////////// uint16_t channalData[CHANNEL_NUM]; //8通道数据 int16_t throttle = 0; //油门 int16_t steering = 0; //转向 unsigned long safe_ms = millis();

uint8_t recCmd;

void setup() {

 bleProtocol.begin(BLE_SPEED);
 pinMode(D6,OUTPUT);
 MotorLeft.begin();   //电机MotorLeft初始化
 MotorRight.begin();  //电机MotorLeft初始化

}

void loop() {

 if (bleProtocol.available())
 {
   bleProtocol.readWords(&recCmd, channalData, 8);

   throttle = map(channalData[CHANNEL_THROTTLE], 1000, 2000, -MAX_THROTTLE, MAX_THROTTLE);
   steering = map(channalData[CHANNEL_STEERING], 1000, 2000, -MAX_STEERING, MAX_STEERING);

   MotorLeft.setSpeed((throttle + steering / 2));
   MotorRight.setSpeed(-(throttle - steering / 2));

1. ifdef _DEBUG

   Serial.print("DATA OK :[");
   for (int a = 0; a < CHANNEL_NUM; a++) {
     Serial.print(channalData[a]);
     Serial.print(" ");
   }
   Serial.print("],throttle:");
   Serial.print(throttle);  //+153
   Serial.print(",steering:");
   Serial.println(steering);
   if(throttle>128)
   {
     digitalWrite(D6,HIGH);
   }else
   {
     digitalWrite(D6,LOW);
   }
   if(steering>256)
   {
     digitalWrite(D8,HIGH);
   }else
   {
     digitalWrite(D8,LOW);
   }

1. endif

   safe_ms = millis();
 }

 if (safe_ms > millis()) safe_ms = millis();
 if (millis() - safe_ms > SAFE_TIME_OUT) {
   MotorLeft.setSpeed(FREE);  //设置电机MotorLeft为释放状态，即速度为0
   MotorRight.setSpeed(FREE);
 }

}