Qian Yuyang / Mbed 2 deprecated Balance_Car_QianYuyang

QAQ ==!

Dependencies:   mbed QEI-1 nRF24L01P xiugai

main.cpp@15:934289377f7a, 2019-12-14 (annotated)

Committer:
AlexQian
Date:
Sat Dec 14 05:45:16 2019 +0000
Revision:
15:934289377f7a
Parent:
14:5bc7349d0e3b 
Balance_Car;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AlexQian 3:ee5e434e047e 1 #define HIGH 1
AlexQian 3:ee5e434e047e 2 #define LOW 0
glintligo 0:e63858fec119 3 #include "mbed.h"
AlexQian 3:ee5e434e047e 4 #include <string>
AlexQian 3:ee5e434e047e 5 typedef bool boolean;
AlexQian 3:ee5e434e047e 6 typedef std::string String;
AlexQian 15:934289377f7a 7 #include "nRF24L01P.h"
zhangyx 14:5bc7349d0e3b 8 #include "JY901.h"
AlexQian 15:934289377f7a 9 #include "QEI.h"
AlexQian 15:934289377f7a 10 #include "converters.h"
AlexQian 15:934289377f7a 11 float Ps,Ds,Is;
AlexQian 15:934289377f7a 12 float Pa,Da,Ia;
AlexQian 15:934289377f7a 13 float Error_S,Error_SI,Error_SD,Error_SLast;
AlexQian 15:934289377f7a 14 float Error_A,Error_AI,Error_AD,Error_ALast;
AlexQian 15:934289377f7a 15 float Target_A,Target_S;
AlexQian 15:934289377f7a 16 float Speed_Out,Angle_Out;
AlexQian 15:934289377f7a 17 float Roll,Yaw,Pitch;
AlexQian 15:934289377f7a 18 float Speed_L,Speed_R;
AlexQian 15:934289377f7a 19 float Gyo_x,Gyo_y,Gyo_z;
AlexQian 15:934289377f7a 20 float Left_Speed,Right_Speed;
AlexQian 15:934289377f7a 21
AlexQian 15:934289377f7a 22 Serial Serial_1(PA_2,PA_3);
AlexQian 15:934289377f7a 23 #define NRF24_TRANSFER_SIZE 32
AlexQian 15:934289377f7a 24 nRF24L01P nrf24_PB_15(PB_15,PB_14,PB_13,PA_8,PB_12,PA_11);
AlexQian 15:934289377f7a 25 JY901 jy901_Serial_3(PB_10,PB_11);
AlexQian 15:934289377f7a 26 PwmOut myServoPA_7(PA_7);
AlexQian 15:934289377f7a 27 PwmOut myServoPA_6(PA_6);
AlexQian 15:934289377f7a 28 PwmOut myServoPB_1(PB_1);
AlexQian 15:934289377f7a 29 PwmOut myServoPB_0(PB_0);
AlexQian 15:934289377f7a 30 QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
AlexQian 15:934289377f7a 31 QEI qei_PC_14(PC_14,PC_15,NC,13,QEI::X4_ENCODING);
AlexQian 15:934289377f7a 32 Ticker tick969801;
AlexQian 15:934289377f7a 33
AlexQian 15:934289377f7a 34 void init() {
AlexQian 15:934289377f7a 35 Pa=6;Ia=0;Da=1;
AlexQian 15:934289377f7a 36 Ps=0.1;Is=0;Ds=0;
AlexQian 15:934289377f7a 37 Target_A=0;
AlexQian 15:934289377f7a 38 Target_S=0;
AlexQian 15:934289377f7a 39 Error_SLast=Error_ALast=0;
AlexQian 15:934289377f7a 40 qei_PA_0.reset();
AlexQian 15:934289377f7a 41 qei_PC_14.reset();
AlexQian 15:934289377f7a 42 }
zhangyx 10:cda7ae2fc9de 43
AlexQian 15:934289377f7a 44 void Set_Pwm() {
AlexQian 15:934289377f7a 45 Left_Speed=Speed_Out+Angle_Out;
AlexQian 15:934289377f7a 46 Right_Speed=Speed_Out+Angle_Out;
AlexQian 15:934289377f7a 47 if (Left_Speed >=0)
AlexQian 15:934289377f7a 48 {
AlexQian 15:934289377f7a 49 myServoPA_7.period_ms(20);
AlexQian 15:934289377f7a 50 myServoPA_7.pulsewidth_us(0);
AlexQian 15:934289377f7a 51 myServoPA_6.period_ms(20);
AlexQian 15:934289377f7a 52 myServoPA_6.pulsewidth_us((Left_Speed * (200000 / 1000)));
AlexQian 15:934289377f7a 53 }
AlexQian 15:934289377f7a 54 else
AlexQian 15:934289377f7a 55 {
AlexQian 15:934289377f7a 56 myServoPA_7.period_ms(20);
AlexQian 15:934289377f7a 57 myServoPA_7.pulsewidth_us(((-Left_Speed) * (200000 / 1000)));
AlexQian 15:934289377f7a 58 myServoPA_6.period_ms(20);
AlexQian 15:934289377f7a 59 myServoPA_6.pulsewidth_us(0);
AlexQian 15:934289377f7a 60 }
AlexQian 15:934289377f7a 61 if (Right_Speed >=0)
AlexQian 15:934289377f7a 62 {
AlexQian 15:934289377f7a 63 myServoPB_1.period_ms(20);
AlexQian 15:934289377f7a 64 myServoPB_1.pulsewidth_us(0);
AlexQian 15:934289377f7a 65 myServoPB_0.period_ms(20);
AlexQian 15:934289377f7a 66 myServoPB_0.pulsewidth_us((Right_Speed * (200000 / 1000)));
AlexQian 15:934289377f7a 67 }
AlexQian 15:934289377f7a 68 else {
AlexQian 15:934289377f7a 69 myServoPB_1.period_ms(20);
AlexQian 15:934289377f7a 70 myServoPB_1.pulsewidth_us(((-Right_Speed) * (200000 / 1000)));
AlexQian 15:934289377f7a 71 myServoPB_0.period_ms(20);
AlexQian 15:934289377f7a 72 myServoPB_0.pulsewidth_us(0);
AlexQian 15:934289377f7a 73 }
AlexQian 15:934289377f7a 74 }
AlexQian 15:934289377f7a 75 void Speed_Control()
AlexQian 15:934289377f7a 76 {
AlexQian 15:934289377f7a 77 Target_S = 0;
AlexQian 15:934289377f7a 78 Error_S=Target_S-(-Speed_L+Speed_R)/2;
AlexQian 15:934289377f7a 79 Error_SD=Error_S-Error_SLast;
AlexQian 15:934289377f7a 80 Error_SLast=Error_S;
AlexQian 15:934289377f7a 81 Error_SI+=Error_S;
AlexQian 15:934289377f7a 82 if(Error_SI>500)
AlexQian 15:934289377f7a 83 Error_SI=500;
AlexQian 15:934289377f7a 84 else if(Error_SI<-500)
AlexQian 15:934289377f7a 85 Error_SI=-500;
AlexQian 15:934289377f7a 86 Target_A=Ps * Error_S + Is * Error_SI + Ds * Error_SD;
AlexQian 15:934289377f7a 87 if(Target_A>10) Target_A=10;
AlexQian 15:934289377f7a 88 else if(Target_A<-10) Target_A=-10;
AlexQian 15:934289377f7a 89 }
AlexQian 15:934289377f7a 90 void Angle_Control() {
AlexQian 15:934289377f7a 91 Error_A = Target_A - Roll;
AlexQian 15:934289377f7a 92 Error_AD = Error_A - Error_ALast;
AlexQian 15:934289377f7a 93 Error_ALast = Error_A;
AlexQian 15:934289377f7a 94 Error_AI += Error_A;
AlexQian 15:934289377f7a 95 if (Error_AI > 500)
AlexQian 15:934289377f7a 96 {
AlexQian 15:934289377f7a 97 Error_AI = 500;
AlexQian 15:934289377f7a 98 }
AlexQian 15:934289377f7a 99 else if (Error_AI < -500)
AlexQian 15:934289377f7a 100 {
AlexQian 15:934289377f7a 101 Error_AI = -500;
AlexQian 15:934289377f7a 102 }
AlexQian 15:934289377f7a 103 Angle_Out = Pa * Error_A + (Ia * Error_AI - Da * Gyo_x);
AlexQian 15:934289377f7a 104 if(Angle_Out>1000)
AlexQian 15:934289377f7a 105 Angle_Out=1000;
AlexQian 15:934289377f7a 106 else if(Angle_Out<-1000)
AlexQian 15:934289377f7a 107 Angle_Out=-1000;
AlexQian 15:934289377f7a 108 }
AlexQian 15:934289377f7a 109 void read_speed()
AlexQian 15:934289377f7a 110 {
AlexQian 15:934289377f7a 111 Speed_R = qei_PA_0.getPulses();
AlexQian 15:934289377f7a 112 Speed_L = qei_PC_14.getPulses();
AlexQian 15:934289377f7a 113 qei_PA_0.reset();
AlexQian 15:934289377f7a 114 qei_PC_14.reset();
AlexQian 15:934289377f7a 115 }
zhangyx 14:5bc7349d0e3b 116
AlexQian 15:934289377f7a 117 void tick969801_handle() {
AlexQian 15:934289377f7a 118 read_speed();
AlexQian 15:934289377f7a 119 // Serial_1.printf("%.2f %.2f\n",_p(Speed_L),_p(Speed_R));
AlexQian 15:934289377f7a 120 if(Roll>180)
AlexQian 15:934289377f7a 121 Roll-=360;
AlexQian 15:934289377f7a 122 if(Gyo_x>2000)
AlexQian 15:934289377f7a 123 Gyo_x-=4000;
AlexQian 15:934289377f7a 124 Roll-=3;
AlexQian 15:934289377f7a 125 // Serial_1.printf("%.2f %.2f\n",_p(Roll),_p(Gyo_x));
AlexQian 15:934289377f7a 126 Speed_Control();
AlexQian 15:934289377f7a 127 Angle_Control();
AlexQian 15:934289377f7a 128 Set_Pwm();
AlexQian 15:934289377f7a 129 }
AlexQian 15:934289377f7a 130
AlexQian 3:ee5e434e047e 131
AlexQian 3:ee5e434e047e 132 int main() {
AlexQian 15:934289377f7a 133 Serial_1.baud(115200);
AlexQian 15:934289377f7a 134 jy901_Serial_3.baud(115200);
AlexQian 15:934289377f7a 135 tick969801.attach(&tick969801_handle,0.04);
AlexQian 15:934289377f7a 136 init();
AlexQian 15:934289377f7a 137 while (true)
AlexQian 15:934289377f7a 138 {
AlexQian 15:934289377f7a 139 jy901_Serial_3.receiveData();
AlexQian 15:934289377f7a 140 jy901_Serial_3.getAttitude(Roll,Pitch,Yaw);
AlexQian 15:934289377f7a 141 jy901_Serial_3.getGyo(Gyo_x,Gyo_y,Gyo_z);
AlexQian 15:934289377f7a 142 }
glintligo 1:1e3eb2d1496b 143 }