Revision 15:934289377f7a, committed 2019-12-14

Comitter:

AlexQian

Date:

Sat Dec 14 05:45:16 2019 +0000

Parent:

14:5bc7349d0e3b

Commit message:

Balance_Car;

Changed in this revision

--- a/main.cpp	Fri Dec 13 01:32:26 2019 +0000
+++ b/main.cpp	Sat Dec 14 05:45:16 2019 +0000
@@ -4,18 +4,140 @@
 #include <string>
 typedef bool boolean;
 typedef std::string String;
-#include "sensors.h"
+#include "nRF24L01P.h"
 #include "JY901.h"
+#include "QEI.h"
+#include "converters.h"
+float Ps,Ds,Is;
+float Pa,Da,Ia;
+float Error_S,Error_SI,Error_SD,Error_SLast;
+float Error_A,Error_AI,Error_AD,Error_ALast;
+float Target_A,Target_S;
+float Speed_Out,Angle_Out;
+float Roll,Yaw,Pitch;
+float Speed_L,Speed_R;
+float Gyo_x,Gyo_y,Gyo_z;
+float Left_Speed,Right_Speed;
+
+Serial Serial_1(PA_2,PA_3);
+#define NRF24_TRANSFER_SIZE 32
+nRF24L01P nrf24_PB_15(PB_15,PB_14,PB_13,PA_8,PB_12,PA_11);
+JY901 jy901_Serial_3(PB_10,PB_11);
+PwmOut myServoPA_7(PA_7);
+PwmOut myServoPA_6(PA_6);
+PwmOut myServoPB_1(PB_1);
+PwmOut myServoPB_0(PB_0);
+QEI qei_PA_0(PA_0,PA_1,NC,13,QEI::X4_ENCODING);
+QEI qei_PC_14(PC_14,PC_15,NC,13,QEI::X4_ENCODING);
+Ticker tick969801;
+
+void init() {
+    Pa=6;Ia=0;Da=1;
+    Ps=0.1;Is=0;Ds=0;
+    Target_A=0;
+    Target_S=0;
+    Error_SLast=Error_ALast=0;
+    qei_PA_0.reset();
+    qei_PC_14.reset();
+}
 
-char buf[10] ;
-int len;
-int val;
+void Set_Pwm() {
+    Left_Speed=Speed_Out+Angle_Out;
+    Right_Speed=Speed_Out+Angle_Out;
+    if (Left_Speed >=0) 
+    {
+        myServoPA_7.period_ms(20);
+        myServoPA_7.pulsewidth_us(0);
+        myServoPA_6.period_ms(20);
+        myServoPA_6.pulsewidth_us((Left_Speed * (200000 / 1000)));
+    } 
+    else 
+    {
+        myServoPA_7.period_ms(20);
+        myServoPA_7.pulsewidth_us(((-Left_Speed) * (200000 / 1000)));
+        myServoPA_6.period_ms(20);
+        myServoPA_6.pulsewidth_us(0);
+    }
+    if (Right_Speed >=0) 
+    {
+        myServoPB_1.period_ms(20);
+        myServoPB_1.pulsewidth_us(0);
+        myServoPB_0.period_ms(20);
+        myServoPB_0.pulsewidth_us((Right_Speed * (200000 / 1000)));
+    } 
+    else {
+        myServoPB_1.period_ms(20);
+        myServoPB_1.pulsewidth_us(((-Right_Speed) * (200000 / 1000)));
+        myServoPB_0.period_ms(20);
+        myServoPB_0.pulsewidth_us(0);
+    }
+}
+void Speed_Control()
+{
+    Target_S = 0;
+    Error_S=Target_S-(-Speed_L+Speed_R)/2;
+    Error_SD=Error_S-Error_SLast;
+    Error_SLast=Error_S;
+    Error_SI+=Error_S;
+    if(Error_SI>500)
+        Error_SI=500;
+    else if(Error_SI<-500)
+        Error_SI=-500;
+    Target_A=Ps * Error_S + Is * Error_SI + Ds * Error_SD;
+    if(Target_A>10) Target_A=10;
+    else if(Target_A<-10) Target_A=-10;   
+}
+void Angle_Control() {
+    Error_A = Target_A - Roll;
+    Error_AD = Error_A - Error_ALast;
+    Error_ALast = Error_A;
+    Error_AI += Error_A;
+    if (Error_AI > 500) 
+    {
+        Error_AI = 500;
+    } 
+    else if (Error_AI < -500) 
+    {
+        Error_AI = -500;
+    }
+    Angle_Out = Pa * Error_A + (Ia * Error_AI - Da * Gyo_x);
+    if(Angle_Out>1000)
+        Angle_Out=1000;
+    else if(Angle_Out<-1000)
+        Angle_Out=-1000;
+}
+void read_speed()
+{
+    Speed_R = qei_PA_0.getPulses();
+    Speed_L = qei_PC_14.getPulses();
+    qei_PA_0.reset();
+    qei_PC_14.reset();
+}
 
-JY901 jy901_Serial_1(PB_6,PA_10);
+void tick969801_handle() {
+    read_speed();
+//    Serial_1.printf("%.2f %.2f\n",_p(Speed_L),_p(Speed_R));
+    if(Roll>180)
+    Roll-=360;
+    if(Gyo_x>2000)
+    Gyo_x-=4000;
+    Roll-=3;
+//    Serial_1.printf("%.2f %.2f\n",_p(Roll),_p(Gyo_x));
+    Speed_Control();
+    Angle_Control();
+    Set_Pwm();
+}
+
 
 int main() {
-
-
-jy901_Serial_1.baud(9600);
-
+    Serial_1.baud(115200);
+    jy901_Serial_3.baud(115200);
+    tick969801.attach(&tick969801_handle,0.04);
+    init();
+    while (true) 
+    {
+        jy901_Serial_3.receiveData();
+        jy901_Serial_3.getAttitude(Roll,Pitch,Yaw);
+        jy901_Serial_3.getGyo(Gyo_x,Gyo_y,Gyo_z);
+    }
 }
\ No newline at end of file