找回密码
 立即注册

QQ登录

只需一步,快速开始

搜索
查看: 5913|回复: 3
收起左侧

Arduino+MPU6050平衡小车 含源码 图片

[复制链接]
ID:670179 发表于 2019-12-21 23:31 | 显示全部楼层 |阅读模式
采用arduino单片机,MPU6050模块,L298n电机驱动模块,HC06蓝牙模块进行参数调节,
一个KEY键连IO3,用于保存当前参数与读取参数。
代码下载后可直接使用,可根据你机器工况调整PID即可。

blanCar.jpg

单片机源程序如下:
  1. /****************************************************************************
  2. Minibalance For Arduino
  3. ****************************************************************************/
  4. #include <DATASCOPE.h>      //这是PC端上位机的库文件
  5. #include <PinChangeInt.h>    //外部中断
  6. #include <MsTimer2.h>        //定时中断
  7. #include <KalmanFilter.h>    //卡尔曼滤波
  8. #include "I2Cdev.h"        
  9. #include "MPU6050_6Axis_MotionApps20.h"//MPU6050库文件
  10. #include "Wire.h"  
  11. #include <EEPROM.h>         
  12. MPU6050 Mpu6050; //实例化一个 MPU6050 对象,对象名称为 Mpu6050
  13. DATASCOPE data;//实例化一个 上位机 对象,对象名称为 data
  14. KalmanFilter KalFilter;//实例化一个卡尔曼滤波器对象,对象名称为 KalFilter
  15. int16_t ax, ay, az, gx, gy, gz;  //MPU6050的三轴加速度和三轴陀螺仪数据
  16. #define KEY 3     //按键引脚
  17. #define IN1 11   //TB6612FNG驱动模块控制信号 共6个
  18. #define IN2 12
  19. #define IN3 7
  20. #define IN4 6
  21. #define PWMA 10
  22. #define PWMB 9
  23. #define ENCODER_L 2  //编码器采集引脚 每路2个 共4个
  24. #define DIRECTION_L 5
  25. #define ENCODER_R 4
  26. #define DIRECTION_R 8
  27. #define ZHONGZHI -4.2//小车的机械中值  DIFFERENCE
  28. #define DIFFERENCE 2
  29. int Balance_Pwm, Velocity_Pwm, Turn_Pwm;   //直立 速度 转向环的PWM
  30. int Motor1, Motor2;      //电机叠加之后的PWM
  31. float Battery_Voltage;   //电池电压 单位是V
  32. volatile long Velocity_L, Velocity_R = 0;   //左右轮编码器数据
  33. int Velocity_Left, Velocity_Right = 0;     //左右轮速度
  34. int Flag_Qian, Flag_Hou, Flag_Left, Flag_Right; //遥控相关变量
  35. int Angle, Show_Data,PID_Send;  //用于显示的角度和临时变量
  36. unsigned char Flag_Stop = 1,Send_Count,Flash_Send;  //停止标志位和上位机相关变量
  37. float Balance_Kp=15,Balance_Kd=0.4,Velocity_Kp=2,Velocity_Ki=0.01;
  38. //***************下面是卡尔曼滤波相关变量***************//
  39. float K1 = 0.05; // 对加速度计取值的权重
  40. float Q_angle = 0.001, Q_gyro = 0.005;
  41. float R_angle = 0.5 , C_0 = 1;
  42. float dt = 0.005; //注意:dt的取值为滤波器采样时间 5ms
  43. int addr = 0;
  44. /**************************************************************************
  45. 函数功能:检测小车是否被拿起
  46. 入口参数:Z轴加速度 平衡倾角 左轮编码器 右轮编码器
  47. 返回  值:0:无事件 1:小车被拿起
  48. **************************************************************************/
  49. int Pick_Up(float Acceleration, float Angle, int encoder_left, int encoder_right){
  50.   static unsigned int flag, count0, count1, count2;
  51.   if (flag == 0) //第一步
  52.   {
  53.     if (abs(encoder_left) + abs(encoder_right) < 15)         count0++;  //条件1,小车接近静止
  54.     else       count0 = 0;
  55.     if (count0 > 10)      flag = 1, count0 = 0;
  56.   }
  57.   if (flag == 1) //进入第二步
  58.   {
  59.     if (++count1 > 400)       count1 = 0, flag = 0;                         //超时不再等待2000ms
  60.     if (Acceleration > 27000 && (Angle > (-14 + ZHONGZHI)) && (Angle < (14 + ZHONGZHI)))  flag = 2; //条件2,小车是在0度附近被拿起
  61.   }
  62.   if (flag == 2)  //第三步
  63.   {
  64.     if (++count2 > 200)       count2 = 0, flag = 0;       //超时不再等待1000ms
  65.     if (abs(encoder_left + encoder_right) > 300)           //条件3,小车的轮胎因为正反馈达到最大的转速      
  66.      {
  67.         flag = 0;  return 1;
  68.       }                                          
  69.   }
  70.   return 0;
  71. }
  72. /**************************************************************************
  73. 函数功能:检测小车是否被放下 作者:平衡小车之家
  74. 入口参数: 平衡倾角 左轮编码器 右轮编码器
  75. 返回  值:0:无事件 1:小车放置并启动
  76. **************************************************************************/
  77. int Put_Down(float Angle, int encoder_left, int encoder_right){
  78.   static u16 flag, count;
  79.   if (Flag_Stop == 0)         return 0;                   //防止误检
  80.   if (flag == 0)
  81.   {
  82.     if (Angle > (-10 + ZHONGZHI) && Angle < (10 + ZHONGZHI) && encoder_left == 0 && encoder_right == 0)      flag = 1; //条件1,小车是在0度附近的
  83.   }
  84.   if (flag == 1)
  85.   {
  86.     if (++count > 100)       count = 0, flag = 0;  //超时不再等待 500ms
  87.     if (encoder_left > 12 && encoder_right > 12 && encoder_left < 80 && encoder_right < 80) //条件2,小车的轮胎在未上电的时候被人为转动
  88.     {
  89.       flag = 0;
  90.       flag = 0;
  91.       return 1;    //检测到小车被放下
  92.     }
  93.   }
  94.   return 0;
  95. }
  96. /**************************************************************************
  97. 函数功能:异常关闭电机 作者:平衡小车之家
  98. 入口参数:倾角和电池电压
  99. 返回  值:1:异常  0:正常
  100. **************************************************************************/
  101. unsigned char Turn_Off(float angle, float voltage)
  102. {
  103.   unsigned char temp;
  104.   if (angle < -40 || angle > 40 || 1 == Flag_Stop || voltage < 11.1) //电池电压低于11.1V关闭电机 //===倾角大于40度关闭电机//===Flag_Stop置1关闭电机
  105.   {                                                                        
  106.     temp = 1;                                          
  107.     analogWrite(PWMA, 0);  //PWM输出为0
  108.     analogWrite(PWMB, 0); //PWM输出为0
  109.   }
  110.   else    temp = 0;   //不存在异常,返回0
  111.   return temp;
  112. }
  113. /**************************************************************************
  114. 函数功能:虚拟示波器往上位机发送数据 作者:平衡小车之家
  115. 入口参数:无
  116. 返回  值:无
  117. **************************************************************************/
  118. void DataScope(void)
  119. {
  120.   int i;
  121.   data.DataScope_Get_Channel_Data(Angle, 1);  //显示第一个数据,角度
  122.   data.DataScope_Get_Channel_Data(Velocity_Left, 2);//显示第二个数据,左轮速度  也就是每40ms输出的脉冲计数
  123.   data.DataScope_Get_Channel_Data(Velocity_Right, 3);//显示第三个数据,右轮速度 也就是每40ms输出的脉冲计数
  124.   data.DataScope_Get_Channel_Data(Battery_Voltage, 4);//显示第四个数据,电池电压,单位V
  125.   Send_Count = data.DataScope_Data_Generate(4);
  126.   for ( i = 0 ; i < Send_Count; i++)
  127.   {
  128.     Serial.write(DataScope_OutPut_Buffer[i]);  
  129.   }
  130.   delay(50);  //上位机必须严格控制发送时序
  131. }
  132. /**************************************************************************
  133. 函数功能:按键扫描  作者:平衡小车之家
  134. 入口参数:无
  135. 返回  值:按键状态,1:单击事件,0:无事件。
  136. **************************************************************************/
  137. unsigned char My_click(void){
  138.   static unsigned char flag_key = 1; //按键按松开标志
  139.   unsigned char Key;   
  140.   Key = digitalRead(KEY);   //读取按键状态
  141.   if (flag_key && Key == 0) //如果发生单击事件
  142.   {
  143.     flag_key = 0;
  144.     return 1;            // 单击事件
  145.   }
  146.   else if (1 == Key)     flag_key = 1;
  147.   return 0;//无按键按下
  148. }
  149. /**************************************************************************
  150. 函数功能:直立PD控制  作者:平衡小车之家
  151. 入口参数:角度、角速度
  152. 返回  值:直立控制PWM
  153. **************************************************************************/
  154. int balance(float Angle, float Gyro)
  155. {
  156.   float Bias;
  157.   int balance;
  158.   Bias = Angle -  ZHONGZHI;   //===求出平衡的角度中值 和机械相关
  159.   balance = Balance_Kp * Bias + Gyro * Balance_Kd; //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数
  160.   return balance;
  161. }
  162. /**************************************************************************
  163. 函数功能:速度PI控制 作者:平衡小车之家
  164. 入口参数:左轮编码器、右轮编码器
  165. 返回  值:速度控制PWM
  166. **************************************************************************/
  167. int velocity(int encoder_left, int encoder_right)
  168. {
  169.   static float Velocity, Encoder_Least, Encoder, Movement;
  170.   static float Encoder_Integral, Target_Velocity;
  171.   float kp = 2, ki = kp / 200;    //PI参数
  172.   if       ( Flag_Qian == 1)Movement = 600;
  173.   else   if ( Flag_Hou == 1)Movement = -600;
  174.   else    //这里是停止的时候反转,让小车尽快停下来
  175.   {
  176.     Movement = 0;
  177.     if (Encoder_Integral > 300)   Encoder_Integral -= 200;
  178.     if (Encoder_Integral < -300)  Encoder_Integral += 200;
  179.   }
  180.   //=============速度PI控制器=======================//
  181.   Encoder_Least = (encoder_left + encoder_right) - 0;               //===获取最新速度偏差==测量速度(左右编码器之和)-目标速度(此处为零)
  182.   Encoder *= 0.7;                                                   //===一阶低通滤波器
  183.   Encoder += Encoder_Least * 0.3;                                   //===一阶低通滤波器
  184.   Encoder_Integral += Encoder;                                      //===积分出位移 积分时间:40ms
  185.   Encoder_Integral = Encoder_Integral - Movement;                   //===接收遥控器数据,控制前进后退
  186.   if (Encoder_Integral > 21000)    Encoder_Integral = 21000;        //===积分限幅
  187.   if (Encoder_Integral < -21000) Encoder_Integral = -21000;         //===积分限幅
  188.   Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki;                  //===速度控制
  189.   if (Turn_Off(KalFilter.angle, Battery_Voltage) == 1 || Flag_Stop == 1)    Encoder_Integral = 0;//小车停止的时候积分清零
  190.   return Velocity;
  191. }
  192. /**************************************************************************
  193. 函数功能:转向控制 作者:平衡小车之家
  194. 入口参数:Z轴陀螺仪
  195. 返回  值:转向控制PWM
  196. **************************************************************************/
  197. int turn(float gyro)//转向控制
  198. {
  199.   static float Turn_Target, Turn, Turn_Convert = 3;
  200.   float Turn_Amplitude = 80, Kp = 2, Kd = 0.001;  //PD参数
  201.   if (1 == Flag_Left)             Turn_Target += Turn_Convert;  //根据遥控指令改变转向偏差
  202.   else if (1 == Flag_Right)       Turn_Target -= Turn_Convert;//根据遥控指令改变转向偏差
  203.   else Turn_Target = 0;
  204.   if (Turn_Target > Turn_Amplitude)  Turn_Target = Turn_Amplitude; //===转向速度限幅
  205.   if (Turn_Target < -Turn_Amplitude) Turn_Target = -Turn_Amplitude;
  206.   Turn = -Turn_Target * Kp + gyro * Kd;         //===结合Z轴陀螺仪进行PD控制
  207.   return Turn;
  208. }
  209. /**************************************************************************
  210. 函数功能:赋值给PWM寄存器 作者:平衡小车之家
  211. 入口参数:左轮PWM、右轮PWM
  212. 返回  值:无
  213. **************************************************************************/
  214. void Set_Pwm(int moto1, int moto2)
  215. {
  216.   if (moto1 > 0)     digitalWrite(IN1, HIGH),      digitalWrite(IN2, LOW);  //TB6612的电平控制
  217.   else             digitalWrite(IN1, LOW),       digitalWrite(IN2, HIGH); //TB6612的电平控制
  218.   analogWrite(PWMA, abs(moto1)); //赋值给PWM寄存器
  219.   if (moto2 < 0) digitalWrite(IN3, HIGH),     digitalWrite(IN4, LOW); //TB6612的电平控制
  220.   else        digitalWrite(IN3, LOW),      digitalWrite(IN4, HIGH); //TB6612的电平控制
  221.   analogWrite(PWMB, abs(moto2));//赋值给PWM寄存器
  222. }
  223. /**************************************************************************
  224. 函数功能:限制PWM赋值  作者:平衡小车之家
  225. 入口参数:无
  226. 返回  值:无
  227. **************************************************************************/
  228. void Xianfu_Pwm(void)
  229. {
  230.   int Amplitude = 250;  //===PWM满幅是255 限制在250
  231.   if(Flag_Qian==1)  Motor2-=DIFFERENCE;  //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出,让小车具有更好的一致性。
  232.   if(Flag_Hou==1)   Motor2-=DIFFERENCE-2;
  233.   if (Motor1 < -Amplitude) Motor1 = -Amplitude;
  234.   if (Motor1 > Amplitude)  Motor1 = Amplitude;
  235.   if (Motor2 < -Amplitude) Motor2 = -Amplitude;
  236.   if (Motor2 > Amplitude)  Motor2 = Amplitude;
  237. }
  238. /**************************************************************************
  239. 函数功能:5ms控制函数 核心代码 作者:平衡小车之家
  240. 入口参数:无
  241. 返回  值:无
  242. **************************************************************************/
  243. void control()
  244. {
  245.   static int Velocity_Count, Turn_Count, Encoder_Count;
  246.   static float Voltage_All,Voltage_Count;
  247.   int Temp;
  248.   sei();//全局中断开启
  249.   Mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  //获取MPU6050陀螺仪和加速度计的数据
  250.   KalFilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);          //通过卡尔曼滤波获取角度
  251.   Angle = KalFilter.angle;//Angle是一个用于显示的整形变量
  252.   Balance_Pwm = balance(KalFilter.angle, KalFilter.Gyro_x);//直立PD控制 控制周期5ms
  253.   if (++Velocity_Count >= 8) //速度控制,控制周期40ms
  254.   {
  255.     Velocity_Left = Velocity_L;    Velocity_L = 0;  //读取左轮编码器数据,并清零,这就是通过M法测速(单位时间内的脉冲数)得到速度。
  256.     Velocity_Right = Velocity_R;    Velocity_R = 0; //读取右轮编码器数据,并清零
  257.     Velocity_Pwm = velocity(Velocity_Left, Velocity_Right);//速度PI控制,控制周期40ms
  258.     Velocity_Count = 0;
  259.   }
  260.   if (++Turn_Count >= 4)//转向控制,控制周期20ms
  261.   {
  262.     Turn_Pwm = turn(gz);
  263.     Turn_Count = 0;
  264.   }
  265.   Motor1 = Balance_Pwm - Velocity_Pwm + Turn_Pwm;  //直立速度转向环的叠加
  266.   Motor2 = Balance_Pwm - Velocity_Pwm - Turn_Pwm; //直立速度转向环的叠加
  267.   Xianfu_Pwm();//限幅
  268.   if (Pick_Up(az, KalFilter.angle, Velocity_Left, Velocity_Right))   Flag_Stop = 1;  //===如果被拿起就关闭电机//===检查是否小车被那起
  269.   if (Put_Down(KalFilter.angle, Velocity_Left, Velocity_Right))      Flag_Stop = 0;              //===检查是否小车被放下
  270.   if (Turn_Off(KalFilter.angle, Battery_Voltage) == 0)        Set_Pwm(Motor1, Motor2);//如果不存在异常,赋值给PWM寄存器控制电机
  271.   if (My_click()) Flag_Stop = !Flag_Stop;   //中断剩余的时间扫描一下按键状态
  272.   Temp = analogRead(0);  //采集一下电池电压
  273.   
  274.   Voltage_Count++;       //平均值计数器
  275.   Voltage_All+=Temp;     //多次采样累积
  276.   if(Voltage_Count==200) Battery_Voltage=11.1,Voltage_All=0,Voltage_Count=0;//求平均值  =Voltage_All*0.05371/200
  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.    while (Serial.available()) {

  426.     Receive_Data=Serial.read();  
  427.     if(Receive_Data==0x7B) Flag_PID=1;  //参数指令起始位
  428.     if(Receive_Data==0x7D) Flag_PID=2;  //参数指令停止位
  429.     if(Flag_PID==1)
  430.      {
  431.       Receive[i]=Receive_Data;     //记录数据
  432.       i++;
  433.      }
  434.     else  if(Flag_PID==2)  //执行指令
  435.      {
  436.            if(Receive[3]==0x50)          PID_Send=1;   //获取PID参数
  437.            else  if(Receive[3]==0x57)    Flash_Send=1; //掉电保存参数
  438.            else  if(Receive[1]!=0x23)    //更新PID参数
  439.            {               
  440.             for(j=i;j>=4;j--)
  441.             {
  442.               Data+=(Receive[j-1]-48)*pow(10,i-j);   //通讯协议
  443.             }
  444.             switch(Receive[1])
  445.              {
  446.                case 0x30:  Balance_Kp=Data/100;break;
  447.                case 0x31:  Balance_Kd=Data/100;break;
  448.                case 0x32:  Velocity_Kp=Data/100;break;
  449.                case 0x33:  Velocity_Ki=Data/100;break;
  450.                case 0x34:  break; //9个通道,预留5个
  451.                case 0x35:  break;
  452.                case 0x36:  break;
  453.                case 0x37:  break;
  454.                case 0x38:  break;
  455.              }
  456.            }         
  457.            Flag_PID=0; //相关标志位清零
  458.            i=0;
  459.            j=0;
  460.            Data=0;
  461.      }
  462.        else  //蓝牙遥控指令
  463.        {
  464.               switch (Receive_Data)   {
  465.                  //这是MinibalanceV1.0的APP发送指令
  466.                   case 0x01: Flag_Qian = 1, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0;   break;              //前进
  467.                   case 0x02: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1;   break;              //右转
  468.                   case 0x03: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1;   break;              //右转
  469.                   case 0x04: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1;   break;              //右转
  470.                   case 0x05: Flag_Qian = 0, Flag_Hou = 1, Flag_Left = 0, Flag_Right = 0;   break;              //后退
  471.                   case 0x06: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0;   break;               //左转
  472.                   case 0x07: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0;   break;               //左转
  473.                   case 0x08: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0;   break;               //左转
  474.                   //这是MinibalanceV3.5的APP发送指令
  475.                   case 0x41: Flag_Qian = 1, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0;   break;              //前进
  476.                   case 0x42: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1;   break;             //右转
  477.                   case 0x43: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1;   break;             //右转
  478.                   case 0x44: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 1;   break;              //右转
  479.                   case 0x45:  Flag_Qian = 0, Flag_Hou = 1, Flag_Left = 0, Flag_Right = 0;   break;             //后退
  480.                   case 0x46: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0;    break;               //左转
  481.                   case 0x47: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0;   break;               //左转
  482.                   case 0x48: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 1, Flag_Right = 0;   break;             //左转
  483.                   default: Flag_Qian = 0, Flag_Hou = 0, Flag_Left = 0, Flag_Right = 0;    break;                //停止
  484.                 }
  485.         }
  486.    }
  487. }
复制代码

以上资料51hei提供下载:
Minibalance_for_Arduino_Final20181113.rar (6.24 KB, 下载次数: 68)

评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励!

查看全部评分

回复

使用道具 举报

ID:603612 发表于 2019-12-23 18:48 | 显示全部楼层
学习了,编译没通过
fatal error: DATASCOPE.h: No such file or directory少文件
回复

使用道具 举报

ID:171746 发表于 2019-12-30 19:27 | 显示全部楼层
请发详细点的资料吗?    我是小白啊
回复

使用道具 举报

ID:452800 发表于 2020-3-7 21:07 | 显示全部楼层
看不懂的样子啊
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表