测试代码，备用。
1 /**************************************************************************** 2 作者：平衡小车之家 3 产品名称：Minibalance For Arduino 4 ****************************************************************************/ 5 #include <DATASCOPE.h> //这是PC端上位机的库文件 6 #include <PinChangeInt.h> //外部中断 7 #include <MsTimer2.h> //定时中断 8 #include <KalmanFilter.h> //卡尔曼滤波 9 #include "I2Cdev.h" 10 #include "MPU6050_6Axis_MotionApps20.h"//MPU6050库文件 11 #include "Wire.h" 12 #include <EEPROM.h> 13 MPU6050 Mpu6050; //实例化一个 MPU6050 对象，对象名称为 Mpu6050 14 DATASCOPE data;//实例化一个 上位机 对象，对象名称为 data 15 KalmanFilter KalFilter;//实例化一个卡尔曼滤波器对象，对象名称为 KalFilter 16 int16_t ax, ay, az, gx, gy, gz; //MPU6050的三轴加速度和三轴陀螺仪数据 17 #define KEY 3 //按键引脚 18 #define IN1 12 //TB6612FNG驱动模块控制信号 共6个 19 #define IN2 13 20 #define IN3 7 21 #define IN4 6 22 #define PWMA 10 23 #define PWMB 9 24 #define ENCODER_L 2 //编码器采集引脚 每路2个 共4个 25 #define DIRECTION_L 5 26 #define ENCODER_R 4 27 #define DIRECTION_R 8 28 #define ZHONGZHI 0//小车的机械中值 DIFFERENCE 29 #define DIFFERENCE 2 30 int Balance_Pwm, Velocity_Pwm, Turn_Pwm; //直立 速度 转向环的PWM 31 int Motor1, Motor2; //电机叠加之后的PWM 32 float Battery_Voltage; //电池电压 单位是V 33 volatile long Velocity_L, Velocity_R = 0; //左右轮编码器数据 34 int Velocity_Left, Velocity_Right = 0; //左右轮速度 35 int Flag_Qian, Flag_Hou, Flag_Left, Flag_Right; //遥控相关变量 36 int Angle, Show_Data,PID_Send; //用于显示的角度和临时变量 37 unsigned char Flag_Stop = 1,Send_Count,Flash_Send; //停止标志位和上位机相关变量 38 float Balance_Kp=15,Balance_Kd=0.4,Velocity_Kp=2,Velocity_Ki=0.01; 39 //***************下面是卡尔曼滤波相关变量***************// 40 float K1 = 0.05; // 对加速度计取值的权重 41 float Q_angle = 0.001, Q_gyro = 0.005; 42 float R_angle = 0.5 , C_0 = 1; 43 float dt = 0.005; //注意：dt的取值为滤波器采样时间 5ms 44 int addr = 0; 45 /************************************************************************** 46 函数功能：检测小车是否被拿起 47 入口参数：Z轴加速度 平衡倾角 左轮编码器 右轮编码器 48 返回 值：0：无事件 1：小车被拿起 49 **************************************************************************/ 50 int Pick_Up(float Acceleration, float Angle, int encoder_left, int encoder_right){ 51 static unsigned int flag, count0, count1, count2; 52 if (flag == 0) //第一步 53 { 54 if (abs(encoder_left) + abs(encoder_right) < 15) count0++; //条件1，小车接近静止 55 else count0 = 0; 56 if (count0 > 10) flag = 1, count0 = 0; 57 } 58 if (flag == 1) //进入第二步 59 { 60 if (++count1 > 400) count1 = 0, flag = 0; //超时不再等待2000ms 61 if (Acceleration > 27000 && (Angle > (-14 + ZHONGZHI)) && (Angle < (14 + ZHONGZHI))) flag = 2; //条件2，小车是在0度附近被拿起 62 } 63 if (flag == 2) //第三步 64 { 65 if (++count2 > 200) count2 = 0, flag = 0; //超时不再等待1000ms 66 if (abs(encoder_left + encoder_right) > 300) //条件3，小车的轮胎因为正反馈达到最大的转速 67 { 68 flag = 0; return 1; 69 } 70 } 71 return 0; 72 } 73 /************************************************************************** 74 函数功能：检测小车是否被放下 作者：平衡小车之家 75 入口参数： 平衡倾角 左轮编码器 右轮编码器 76 返回 值：0：无事件 1：小车放置并启动 77 **************************************************************************/ 78 int Put_Down(float Angle, int encoder_left, int encoder_right){ 79 static u16 flag, count; 80 if (Flag_Stop == 0) return 0; //防止误检 81 if (flag == 0) 82 { 83 if (Angle > (-10 + ZHONGZHI) && Angle < (10 + ZHONGZHI) && encoder_left == 0 && encoder_right == 0) flag = 1; //条件1，小车是在0度附近的 84 } 85 if (flag == 1) 86 { 87 if (++count > 100) count = 0, flag = 0; //超时不再等待 500ms 88 if (encoder_left > 12 && encoder_right > 12 && encoder_left < 80 && encoder_right < 80) //条件2，小车的轮胎在未上电的时候被人为转动 89 { 90 flag = 0; 91 flag = 0; 92 return 1; //检测到小车被放下 93 } 94 } 95 return 0; 96 } 97 /************************************************************************** 98 函数功能：异常关闭电机 作者：平衡小车之家 99 入口参数：倾角和电池电压 100 返回 值：1：异常 0：正常 101 **************************************************************************/ 102 unsigned char Turn_Off(float angle, float voltage) 103 { 104 unsigned char temp; 105 if (angle < -40 || angle > 40 || 1 == Flag_Stop || voltage < 11.1) //电池电压低于11.1V关闭电机 //===倾角大于40度关闭电机//===Flag_Stop置1关闭电机 106 { 107 temp = 1; 108 analogWrite(PWMA, 0); //PWM输出为0 109 analogWrite(PWMB, 0); //PWM输出为0 110 } 111 else temp = 0; //不存在异常，返回0 112 return temp; 113 } 114 /************************************************************************** 115 函数功能：虚拟示波器往上位机发送数据 作者：平衡小车之家 116 入口参数：无 117 返回 值：无 118 **************************************************************************/ 119 void DataScope(void) 120 { 121 int i; 122 data.DataScope_Get_Channel_Data(Angle, 1); //显示第一个数据，角度 123 data.DataScope_Get_Channel_Data(Velocity_Left, 2);//显示第二个数据，左轮速度 也就是每40ms输出的脉冲计数 124 data.DataScope_Get_Channel_Data(Velocity_Right, 3);//显示第三个数据，右轮速度 也就是每40ms输出的脉冲计数 125 data.DataScope_Get_Channel_Data(Battery_Voltage, 4);//显示第四个数据，电池电压，单位V 126 Send_Count = data.DataScope_Data_Generate(4); 127 for ( i = 0 ; i < Send_Count; i++) 128 { 129 Serial.write(DataScope_OutPut_Buffer[i]); 130 } 131 delay(50); //上位机必须严格控制发送时序 132 } 133 /************************************************************************** 134 函数功能：按键扫描 作者：平衡小车之家 135 入口参数：无 136 返回 值：按键状态，1：单击事件，0：无事件。 137 **************************************************************************/ 138 unsigned char My_click(void){ 139 static unsigned char flag_key = 1; //按键按松开标志 140 unsigned char Key; 141 Key = digitalRead(KEY); //读取按键状态 142 if (flag_key && Key == 0) //如果发生单击事件 143 { 144 flag_key = 0; 145 return 1; // 单击事件 146 } 147 else if (1 == Key) flag_key = 1; 148 return 0;//无按键按下 149 } 150 /************************************************************************** 151 函数功能：直立PD控制 作者：平衡小车之家 152 入口参数：角度、角速度 153 返回 值：直立控制PWM 154 **************************************************************************/ 155 int balance(float Angle, float Gyro) 156 { 157 float Bias; 158 int balance; 159 Bias = Angle - 0; //===求出平衡的角度中值 和机械相关 160 balance = Balance_Kp * Bias + Gyro * Balance_Kd; //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数 161 return balance; 162 } 163 /************************************************************************** 164 函数功能：速度PI控制 作者：平衡小车之家 165 入口参数：左轮编码器、右轮编码器 166 返回 值：速度控制PWM 167 **************************************************************************/ 168 int velocity(int encoder_left, int encoder_right) 169 { 170 static float Velocity, Encoder_Least, Encoder, Movement; 171 static float Encoder_Integral, Target_Velocity; 172 float kp = 2, ki = kp / 200; //PI参数 173 if ( Flag_Qian == 1)Movement = 600; 174 else if ( Flag_Hou == 1)Movement = -600; 175 else //这里是停止的时候反转，让小车尽快停下来 176 { 177 Movement = 0; 178 if (Encoder_Integral > 300) Encoder_Integral -= 200; 179 if (Encoder_Integral < -300) Encoder_Integral += 200; 180 } 181 //=============速度PI控制器=======================// 182 Encoder_Least = (encoder_left + encoder_right) - 0; //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零） 183 Encoder *= 0.7; //===一阶低通滤波器 184 Encoder += Encoder_Least * 0.3; //===一阶低通滤波器 185 Encoder_Integral += Encoder; //===积分出位移 积分时间：40ms 186 Encoder_Integral = Encoder_Integral - Movement; //===接收遥控器数据，控制前进后退 187 if (Encoder_Integral > 21000) Encoder_Integral = 21000; //===积分限幅 188 if (Encoder_Integral < -21000) Encoder_Integral = -21000; //===积分限幅 189 Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki; //===速度控制 190 if (Turn_Off(KalFilter.angle, Battery_Voltage) == 1 || Flag_Stop == 1) Encoder_Integral = 0;//小车停止的时候积分清零 191 return Velocity; 192 } 193 /************************************************************************** 194 函数功能：转向控制 作者：平衡小车之家 195 入口参数：Z轴陀螺仪 196 返回 值：转向控制PWM 197 **************************************************************************/ 198 int turn(float gyro)//转向控制 199 { 200 static float Turn_Target, Turn, Turn_Convert = 3; 201 float Turn_Amplitude = 80, Kp = 2, Kd = 0.001; //PD参数 202 if (1 == Flag_Left) Turn_Target += Turn_Convert; //根据遥控指令改变转向偏差 203 else if (1 == Flag_Right) Turn_Target -= Turn_Convert;//根据遥控指令改变转向偏差 204 else Turn_Target = 0; 205 if (Turn_Target > Turn_Amplitude) Turn_Target = Turn_Amplitude; //===转向速度限幅 206 if (Turn_Target < -Turn_Amplitude) Turn_Target = -Turn_Amplitude; 207 Turn = -Turn_Target * Kp + gyro * Kd; //===结合Z轴陀螺仪进行PD控制 208 return Turn; 209 } 210 /************************************************************************** 211 函数功能：赋值给PWM寄存器 作者：平衡小车之家 212 入口参数：左轮PWM、右轮PWM 213 返回 值：无 214 **************************************************************************/ 215 void Set_Pwm(int moto1, int moto2) 216 { 217 if (moto1 > 0) digitalWrite(IN1, HIGH), digitalWrite(IN2, LOW); //TB6612的电平控制 218 else digitalWrite(IN1, LOW), digitalWrite(IN2, HIGH); //TB6612的电平控制 219 analogWrite(PWMA, abs(moto1)); //赋值给PWM寄存器 220 if (moto2 < 0) digitalWrite(IN3, HIGH), digitalWrite(IN4, LOW); //TB6612的电平控制 221 else digitalWrite(IN3, LOW), digitalWrite(IN4, HIGH); //TB6612的电平控制 222 analogWrite(PWMB, abs(moto2));//赋值给PWM寄存器 223 } 224 /************************************************************************** 225 函数功能：限制PWM赋值 作者：平衡小车之家 226 入口参数：无 227 返回 值：无 228 **************************************************************************/ 229 void Xianfu_Pwm(void) 230 { 231 int Amplitude = 250; //===PWM满幅是255 限制在250 232 if(Flag_Qian==1) Motor2-=DIFFERENCE; //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出，让小车具有更好的一致性。 233 if(Flag_Hou==1) Motor2-=DIFFERENCE-2; 234 if (Motor1 < -Amplitude) Motor1 = -Amplitude; 235 if (Motor1 > Amplitude) Motor1 = Amplitude; 236 if (Motor2 < -Amplitude) Motor2 = -Amplitude; 237 if (Motor2 > Amplitude) Motor2 = Amplitude; 238 } 239 /************************************************************************** 240 函数功能：5ms控制函数 核心代码 作者：平衡小车之家 241 入口参数：无 242 返回 值：无 243 **************************************************************************/ 244 void control() 245 { 246 static int Velocity_Count, Turn_Count, Encoder_Count; 247 static float Voltage_All,Voltage_Count; 248 int Temp; 249 sei();//全局中断开启 250 Mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); //获取MPU6050陀螺仪和加速度计的数据 251 KalFilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1); //通过卡尔曼滤波获取角度 252 Angle = KalFilter.angle;//Angle是一个用于显示的整形变量 253 Balance_Pwm = balance(KalFilter.angle, KalFilter.Gyro_x);//直立PD控制 控制周期5ms 254 if (++Velocity_Count >= 8) //速度控制，控制周期40ms 255 { 256 Velocity_Left = Velocity_L; Velocity_L = 0; //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度。 257 Velocity_Right = Velocity_R; Velocity_R = 0; //读取右轮编码器数据，并清零 258 Velocity_Pwm = velocity(Velocity_Left, Velocity_Right);//速度PI控制，控制周期40ms 259 Velocity_Count = 0; 260 } 261 if (++Turn_Count >= 4)//转向控制，控制周期20ms 262 { 263 Turn_Pwm = turn(gz); 264 Turn_Count = 0; 265 } 266 Motor1 = Balance_Pwm - Velocity_Pwm + Turn_Pwm; //直立速度转向环的叠加 267 Motor2 = Balance_Pwm - Velocity_Pwm - Turn_Pwm; //直立速度转向环的叠加 268 Xianfu_Pwm();//限幅 269 if (Pick_Up(az, KalFilter.angle, Velocity_Left, Velocity_Right)) Flag_Stop = 1; //===如果被拿起就关闭电机//===检查是否小车被那起 270 if (Put_Down(KalFilter.angle, Velocity_Left, Velocity_Right)) Flag_Stop = 0; //===检查是否小车被放下 271 if (Turn_Off(KalFilter.angle, Battery_Voltage) == 0) Set_Pwm(Motor1, Motor2);//如果不存在异常，赋值给PWM寄存器控制电机 272 if (My_click()) Flag_Stop = !Flag_Stop; //中断剩余的时间扫描一下按键状态 273 Temp = analogRead(0); //采集一下电池电压 274 Voltage_Count++; //平均值计数器 275 Voltage_All+=Temp; //多次采样累积 276 if(Voltage_Count==200) Battery_Voltage=Voltage_All*0.05371/200,Voltage_All=0,Voltage_Count=0;//求平均值 277 } 278 /************************************************************************** 279 函数功能：初始化 相当于STM32里面的Main函数 作者：平衡小车之家 280 入口参数：无 281 返回 值：无 282 **************************************************************************/ 283 void setup() { 284 pinMode(IN1, OUTPUT); //TB6612控制引脚，控制电机1的方向，01为正转，10为反转 285 pinMode(IN2, OUTPUT); //TB6612控制引脚， 286 pinMode(IN3, OUTPUT); //TB6612控制引脚，控制电机2的方向，01为正转，10为反转 287 pinMode(IN4, OUTPUT); //TB6612控制引脚， 288 pinMode(PWMA, OUTPUT); //TB6612控制引脚，电机PWM 289 pinMode(PWMB, OUTPUT); //TB6612控制引脚，电机PWM 290 digitalWrite(IN1, 0); //TB6612控制引脚拉低 291 digitalWrite(IN2, 0); //TB6612控制引脚拉低 292 digitalWrite(IN3, 0); //TB6612控制引脚拉低 293 digitalWrite(IN4, 0); //TB6612控制引脚拉低 294 analogWrite(PWMA, 0); //TB6612控制引脚拉低 295 analogWrite(PWMB, 0); //TB6612控制引脚拉低 296 pinMode(2, INPUT); //编码器引脚 297 pinMode(4, INPUT); //编码器引脚 298 pinMode(5, INPUT); //编码器引脚 299 pinMode(8, INPUT); //编码器引脚 300 pinMode(3, INPUT); //按键引脚 301 Wire.begin(); //加入 IIC 总线 302 Serial.begin(9600); //开启串口，设置波特率为 9600 303 delay(1500); //延时等待初始化完成 304 Mpu6050.initialize(); //初始化MPU6050 305 delay(20); 306 if(digitalRead(KEY)==0) { //读取EEPROM的参数 307 Balance_Kp = (float)((EEPROM.read(addr+0)*256)+EEPROM.read(addr+1) )/100; 308 Balance_Kd = (float)((EEPROM.read(addr+2)*256)+EEPROM.read(addr+3))/100; 309 Velocity_Kp = (float)((EEPROM.read(addr+4)*256)+EEPROM.read(addr+5))/100; 310 Velocity_Ki = (float)((EEPROM.read(addr+6)*256)+EEPROM.read(addr+7))/100; 311 } 312 MsTimer2::set(5, control); //使用Timer2设置5ms定时中断 313 MsTimer2::start(); //使用中断使能 314 attachInterrupt(0, READ_ENCODER_L, CHANGE); //开启外部中断 编码器接口1 315 attachPinChangeInterrupt(4, READ_ENCODER_R, CHANGE); //开启外部中断 编码器接口2 316 } 317 /************************************************************************** 318 函数功能：主循环程序体 319 入口参数：无 320 返回 值：无 321 **************************************************************************/ 322 void loop() { 323 int Voltage_Temp; 324 static unsigned char flag; 325 unsigned char Balance_Kp_Temp=0,Balance_Kd_Temp=0,Velocity_Kp_Temp=0,Velocity_Ki_Temp=0; 326 Voltage_Temp = (Battery_Voltage - 11.1) * 60; //根据APP的协议对电池电压变量进行处理 327 if (Voltage_Temp > 100)Voltage_Temp = 100; 328 if (Voltage_Temp < 0)Voltage_Temp = 0; 329 if (Flag_Stop == 0) 330 { 331 Serial.begin(9600); //开启串口，设置波特率为 9600 332 flag=!flag; 333 if(PID_Send==1)//发送PID参数 334 { 335 Serial.print("{C"); 336 Serial.print((int)(Balance_Kp*100)); //左轮编码器 337 Serial.print(":"); 338 Serial.print((int)(Balance_Kd*100)); //右轮编码器 339 Serial.print(":"); 340 Serial.print((int)(Velocity_Kp*100)); //电池电压 341 Serial.print(":"); 342 Serial.print((int)(Velocity_Ki*100)); //平衡倾角 343 Serial.print("}$"); 344 PID_Send=0; 345 } 346 else if(flag==0) 347 { 348 Serial.print("{A"); 349 Serial.print(abs(Velocity_Left / 2)); //左轮编码器 350 Serial.print(":"); 351 Serial.print(abs(Velocity_Right / 2)); //右轮编码器 352 Serial.print(":"); 353 Serial.print(Voltage_Temp); //电池电压 354 Serial.print(":"); 355 Serial.print(Angle); //平衡倾角 356 Serial.print("}$"); 357 } 358 else 359 { 360 Serial.print("{B"); 361 Serial.print(Angle); 362 Serial.print(":"); 363 Serial.print(Voltage_Temp); 364 Serial.print(":"); 365 Serial.print(Velocity_Left/2); 366 Serial.print(":"); 367 Serial.print(Velocity_Right/2); 368 Serial.print("}$"); 369 } 370 delay(50); 371 } 372 else Serial.begin(128000), DataScope(); //使用上位机时，波特率是128000 373 if(Flash_Send==1) //写入PID参数到EEPROM,由app控制该指令 374 { 375 EEPROM.write(addr, ((unsigned int)(Balance_Kp*100)&0xff00)>>8); 376 EEPROM.write(addr+1, (unsigned int)(Balance_Kp*100)&0xff); 377 EEPROM.write(addr+2, ((unsigned int)(Balance_Kd*100)&0xff00)>>8); 378 EEPROM.write(addr+3, (unsigned int)(Balance_Kd*100)&0xff); 379 EEPROM.write(addr+4, ((unsigned int)(Velocity_Kp*100)&0xff00)>>8); 380 EEPROM.write(addr+5, (unsigned int)(Velocity_Kp*100)&0xff); 381 EEPROM.write(addr+6, ((unsigned int)(Velocity_Ki*100)&0xff00)>>8); 382 EEPROM.write(addr+7, (unsigned int)(Velocity_Ki*100)&0xff); 383 Flash_Send=0; 384 } 385 } 386 /************************************************************************** 387 函数功能：外部中断读取编码器数据，具有二倍频功能 注意外部中断是跳变沿触发 388 入口参数：无 389 返回 值：无 390 **************************************************************************/ 391 void READ_ENCODER_L() { 392 if (digitalRead(ENCODER_L) == LOW) { //如果是下降沿触发的中断 393 if (digitalRead(DIRECTION_L) == LOW) Velocity_L--; //根据另外一相电平判定方向 394 else Velocity_L++; 395 } 396 else { //如果是上升沿触发的中断 397 if (digitalRead(DIRECTION_L) == LOW) Velocity_L++; //根据另外一相电平判定方向 398 else Velocity_L--; 399 } 400 } 401 /************************************************************************** 402 函数功能：外部中断读取编码器数据，具有二倍频功能 注意外部中断是跳变沿触发 403 入口参数：无 404 返回 值：无 405 **************************************************************************/ 406 void READ_ENCODER_R() { 407 if (digitalRead(ENCODER_R) == LOW) { //如果是下降沿触发的中断 408 if (digitalRead(DIRECTION_R) == LOW) Velocity_R--;//根据另外一相电平判定方向 409 else Velocity_R++; 410 } 411 else { //如果是上升沿触发的中断 412 if (digitalRead(DIRECTION_R) == LOW) Velocity_R++; //根据另外一相电平判定方向 413 else Velocity_R--; 414 } 415 } 416 /************************************************************************** 417 函数功能：串口接收中断 418 入口参数：无 419 返回 值：无 420 **************************************************************************/ 421 void serialEvent() 422 { 423 static unsigned char Flag_PID,Receive[10],Receive_Data,i,j; 424 static float Data; 425 426 while (Serial.available()) { 427 428 Receive_Data=Serial.read(); 429 if(Receive_Data==0x7B) Flag_PID=1; //参数指令起始位 430 if(Receive_Data==0x7D) Flag_PID=2; //参数指令停止位 431 if(Flag_PID==1) 432 { 433 Receive[i]=Receive_Data; //记录数据 434 i++; 435 } 436 else if(Flag_PID==2) //执行指令 437 { 438 if(Receive[3]==0x50) PID_Send=1; //获取PID参数 439 else if(Receive[3]==0x57) Flash_Send=1; //掉电保存参数 440 else if(Receive[1]!=0x23) //更新PID参数 441 { 442 for(j=i;j>=4;j--) 443 { 444 Data+=(Receive[j-1]-48)*pow(10,i-j); //通讯协议 445 } 446 switch(Receive[1]) 447 { 448 case 0x30: Balance_Kp=Data/100;break; 449 case 0x31: Balance_Kd=Data/100;break; 450 case 0x32: Velocity_Kp=Data/100;break; 451 case 0x33: Velocity_Ki=Data/100;break; 452 case 0x34: break; //9个通道，预留5个 453 case 0x35: break; 454 case 0x36: break; 455 case 0x37: break; 456 case 0x38: break; 457 } 458 } 459 Flag_PID=0; //相关标志位清零 460 i=0; 461 j=0; 462 Data=0; 463 } 464 else //蓝牙遥控指令 465 { 466 switch (Receive_Data) { 467 //这是MinibalanceV1.0的APP发送指令 468 case 0x01: Flag_Qian = 1, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0; break; //前进 469 case 0x02: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1; break; //右转 470 case 0x03: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1; break; //右转 471 case 0x04: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1; break; //右转 472 case 0x05: Flag_Qian = 0, Flag_Hou = 1, Flag_Left = 0, Flag_Right = 0; break; //后退 473 case 0x06: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0; break; //左转 474 case 0x07: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0; break; //左转 475 case 0x08: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0; break; //左转 476 //这是MinibalanceV3.5的APP发送指令 477 case 0x41: Flag_Qian = 1, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0; break; //前进 478 case 0x42: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1; break; //右转 479 case 0x43: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1; break; //右转 480 case 0x44: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1; break; //右转 481 case 0x45: Flag_Qian = 0, Flag_Hou = 1, Flag_Left = 0, Flag_Right = 0; break; //后退 482 case 0x46: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0; break; //左转 483 case 0x47: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0; break; //左转 484 case 0x48: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0; break; //左转 485 default: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0; break; //停止 486 } 487 } 488 } 489 }