Diff: main.cpp

Revision:

0:77ad62c61c78

Child:

1:13d8940f0fd4

diff -r 000000000000 -r 77ad62c61c78 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 23 14:26:28 2017 +0000
@@ -0,0 +1,152 @@
+#include "mbed.h"
+#include "QEI.h"
+#include "MODSERIAL.h"
+#include "math.h"
+
+
+
+DigitalOut gpo(D0);
+DigitalOut ledb(LED_BLUE);
+DigitalOut ledr(LED_RED);
+DigitalOut ledg(LED_GREEN);
+DigitalOut motor1DC(D7);
+PwmOut motor1PWM(D6);
+DigitalOut motor2DC(D4);
+PwmOut motor2PWM(D5);
+
+AnalogIn   potMeter1(A0);
+AnalogIn   potMeter2(A1);
+DigitalIn  button1(D11);
+DigitalIn  button2(D12);
+QEI Encoder1(D12,D13,NC,4200);
+QEI Encoder2(D9,D8,NC,4200);
+
+MODSERIAL pc(USBTX,USBRX);
+
+Ticker controller;
+
+int potmultiplier = 8000; // Multiplier for the pot meter reference which is normally between 0 and 1
+
+//Start constants for the PID ------------------------------- 
+const double pi = 3.1415926535897;
+const double M1_TS = 0.01; // (was 0.0001) 0.001 and 0.01 work without biquad filter. // // Sample time (motor - timestep)
+
+//verplaatst
+const float RAD_PER_PULSE = (2*pi)/4200;
+
+const float M1_KP = 10, M1_KI = 0.5, M1_KD = 0.5; //was KP=10 KI=0.5 KD=0.5
+double m1_err_int = 0, m1_prev_err = 0 ;
+const double M1_F_A1 = 1.0 , M1_F_A2  = 2.0 , M1_F_B0  = 1.0 , M1_F_B1  = 3.0 , M1_F_B2  = 4.0 ;
+double m1_f_v1  = 0  , m1_f_v2  = 0 ;
+//End of constant for the PID
+
+/*Get reference position in different way-----------------START
+float Get_X_Position(){
+    double x = potMeter1 * potmultiplier;
+    return x;
+}
+
+float Get_Y_Position(){
+    double y = potMeter2 * potmultiplier;
+    return y;
+}
+-----------------------------------------------------------END*/
+
+//Start PID part ------------------------------------------START
+double PID(double e, const double Kp, const double Ki, const double Kd, double Ts, double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1, const double f_a2, const double f_b0, const double f_b1, const double f_b2){
+
+// Derivative
+double e_der = (e - e_prev)/Ts; // Ts = motor1-timestep
+
+// biquad part, see slide
+//e_der = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2);
+
+e_prev = e;
+    
+// Integral
+e_int += Ts*e;
+    
+    
+//PID
+return Kp*e + Ki*e_int + Kd * e_der;
+
+}
+
+
+
+
+
+
+void Controller(){
+    double x = potMeter1 * potmultiplier;
+    double y = potMeter2 * potmultiplier;
+    
+    double reference_motor1 = atan(y/x); // reference for Theta
+    double reference_motor2 = sqrt((x*x+y*y));  // reference for radius
+    
+    double position_motor1 = RAD_PER_PULSE*Encoder1.getPulses(); // current position for theta
+    double position_motor2 = RAD_PER_PULSE*Encoder2.getPulses(); // current position for the radius
+    
+    double motor1 = PID(reference_motor1 - position_motor1, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+    double motor2 = PID(reference_motor2 - position_motor2, M1_KP, M1_KI, M1_KD, M1_TS, m1_err_int, m1_prev_err, m1_f_v1, m1_f_v2, M1_F_A1, M1_F_A2, M1_F_B0, M1_F_B1, M1_F_B2);
+
+    pc.baud(115200);
+    pc.printf("\r Position(X,Y)=(%f,%f), Ref(Theta,R): (%f,%f), Pos(Theta,R):(%f,%f), Motor Value(M1,M2):(%f,%f).\n",x, y, reference_motor1, reference_motor2, position_motor1, position_motor2, motor1, motor2);
+    
+    motor1PWM = motor1;
+    motor2PWM = motor2;
+
+    if(motor1 > 0.5){
+        motor1DC = 1;
+        
+        ledr = 1;
+        ledg = 1;       //Blau
+        ledb = 0;
+    }
+    else if (motor1<-0.5) {
+        motor1DC = 0; 
+        
+        ledb = 1;
+        ledr = 1;
+        ledg = 0;       //Groen
+        
+    }
+    else{ 
+    motor1PWM = 0;
+        
+        ledb = 1;       //Rood
+        ledr = 0;
+        ledg = 1;
+    }
+
+if(motor2 > 0.3){
+        motor1DC = 1;
+        
+        ledr = 1;
+        ledg = 1;       //Blau
+        ledb = 0;
+    }
+    else if (motor2<-0.3) {
+        motor1DC = 0; 
+        
+        ledb = 1;
+        ledr = 1;
+        ledg = 0;       //Groen
+        
+    }
+    else{ 
+    motor2PWM = 0;
+        
+        ledb = 1;       //Rood
+        ledr = 0;
+        ledg = 1;
+    }
+}
+
+int main()
+{
+    controller.attach(&Controller, M1_TS);
+    
+    while(1){}    
+    
+}
\ No newline at end of file