Revision 13:2d2763be031c, committed 2018-10-31

Comitter:

CasperK

Date:

Wed Oct 31 16:15:56 2018 +0000

Parent:

12:9a2d3d544426

Commit message:

P controller now works

Changed in this revision

diff -r 9a2d3d544426 -r 2d2763be031c main.cpp
--- a/main.cpp	Tue Oct 30 09:22:44 2018 +0000
+++ b/main.cpp	Wed Oct 31 16:15:56 2018 +0000
@@ -9,14 +9,16 @@
 PwmOut pwmpin2(D5);
 DigitalOut directionpin2(D4);
 DigitalOut directionpin1(D7);
-QEI motor2(D13,D12,NC, 32);
-QEI motor1(D11,D10,NC, 32);
+QEI motor1(D13,D12,NC, 32);
+QEI motor2(D11,D10,NC, 32);
 DigitalOut ledred(LED_RED);
 
 DigitalIn KillSwitch(SW2);
 DigitalIn button(SW3);
 MODSERIAL pc(USBTX, USBRX);
 HIDScope scope(6);
+DigitalIn buttonleft(D2);
+DigitalIn buttonright(D3);
 
 //values of inverse kinematics
 volatile bool emg0Bool = false;
@@ -28,7 +30,7 @@
 const float C1 = 3.0; //motor 1 gear ratio
 const float C2 = 0.013; //motor 2 gear ratio/radius of the circular gear in m
 const float length = 0.300; //length in m (placeholder)
-const float Timestep = 0.1;
+const float Timestep = 0.01;
 
 volatile float x_position = length;
 volatile float y_position = 0.0;
@@ -42,27 +44,35 @@
 volatile char c;
 
 //values of PID controller
-const float Kp = 1;
-const float Ki = 0.2;
+const float Kp = 0.05;
+const float Ki = 0.01;
 const float Kd = 0;
-float Output1 = 0 ;      //Starting value
-float Output2 = 0 ;      //Starting value
-float P1 = 0;           //encoder value
-float P2 = 0;
-float e1 = 0 ;          //Starting value 
-float e2 = 0 ;          //Starting value
-float e3;  
-float f1 = 0 ;          //Starting value 
-float f2 = 0 ;          //Starting value
-float f3;     
-float Output_Last1;      // Remember previous position
-float Output_Last2;      // Remember previous position
-float Y1;               // Value that is outputted to motor control
-float Y2;               // Value that is outputted to motor control
-float pwm1;
-float pwm2;
-float P_Last = 0;       // Starting position
-      
+volatile float Output1 = 0 ;      //Starting value
+volatile float Output2 = 0 ;      //Starting value
+volatile float P1 = 0;           //encoder value
+volatile float P2 = 0;
+volatile float e1 = 0 ;          //Starting value 
+volatile float e2 = 0 ;          //Starting value
+volatile float e3 = 0;  
+volatile float f1 = 0 ;          //Starting value 
+volatile float f2 = 0 ;          //Starting value
+volatile float f3 = 0; 
+volatile float df3 = 0;
+volatile float if3 = 0;
+volatile float de3 = 0;
+volatile float ie3 = 0;
+    
+volatile float Output_Last1;      // Remember previous position
+volatile float Output_Last2;      // Remember previous position
+volatile float Y1 = 0;               // Value that is outputted to motor control
+volatile float Y2 = 0;               // Value that is outputted to motor control
+volatile float pwm_value1 = 0;
+volatile float pwm_value2 = 0;
+volatile float P_Last = 0;       // Starting position
+
+Ticker pwm_ticker; 
+Ticker mainticker;; 
+    
 void yDirection() {
     //direction of the motion
     if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
@@ -98,21 +108,21 @@
     //if the button is pressed, reverse the y direction
     if (!button) {
         x_direction = !x_direction;
-        wait(0.5f);
+        //wait(0.5f);
         }     
         
     if (emg2Bool) { //if w is pressed, move up/down
         //direction of the motion
         if (x_direction) { 
-            direction = 1.0;
+            direction = 1.0f;
         }
         else if (!x_direction) {
-            direction = -1.0;
+            direction = -1.0f;
         }
         
         //calculating the motion
         old_x_position = x_position;
-        x_position = old_x_position + (0.01f * direction);
+        x_position = old_x_position + (0.0001f * direction);
         old_motor1_angle = motor1_angle;
         motor1_angle = old_motor1_angle + ( x_position - length ) / C2; // sawtooth-gear motor angle in rad
         
@@ -123,14 +133,14 @@
 
 volatile float Plek1;
 void PIDController1() { 
-    Plek1 = motor1.getPulses();  
-    P1 = motor1.getPulses() / 8400 * 2*PI;    //actual motor angle in rad  
+    //Plek1 = motor1.getPulses();  
+    P1 = motor1.getPulses() / 8400.0f * 2.0f*PI;    //actual motor angle in rad  
     e1 = e2;
     e2 = e3;
     e3 = motor1_angle - P1;
-    float de3 = (e3-e2)/Timestep;
-    float ie3 = ie3 + e3*Timestep;
-    Output2 = Kp * e3 + Ki * ie3 + Kd * de3;
+    de3 = (e3-e2)/Timestep;
+    ie3 = ie3 + e3*Timestep;
+    Output1 = Kp * e3 + Ki * ie3 + Kd * de3;
     
 //    Output_Last1 = Output1;
 //    Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
@@ -142,15 +152,16 @@
     else if (Y1 <= -1){
         Y1 = -1;
     }    
+    
 }
 volatile float Plek2;
 void PIDController2() {   
-    Plek2 = motor2.getPulses();
+    //Plek2 = motor2.getPulses();
     P2 = motor2.getPulses() / 8400.0f * 2.0f*PI; // actual motor angle in rad
     f2 = f3;
     f3 = motor2_angle - P2;
-    float df3 = (f3-f2)/Timestep;
-    float if3 = if3 + f3*Timestep;
+    df3 = (f3-f2)/Timestep;
+    if3 = if3 + f3*Timestep;
     Output2 = Kp * f3 + Ki * if3 + Kd * df3;
   //  Output_Last2 = Output2;
   //  Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
@@ -162,101 +173,97 @@
     else if (Y2 <= -1){
         Y2 = -1;
     }   
+    
 }
 
 void ControlMotor1() {
-    if (Y1 > 0) {
-        Y1 = 0.6f * Y1 + 0.4f;
-        directionpin1 = true;
+    if (Y1 > 0.0f) {
+        Y1 = 0.4f * Y1 + 0.2f;
+        directionpin1 = false;
     } 
-    else if(Y1 < 0){
-        Y1 = 0.6f - 0.4f * Y1;
-        directionpin1 = false;
+    else if(Y1 < -0.0f){
+        Y1 = -0.2f + 0.4f * Y1;
+        directionpin1 = true;
     }    
-    pwmpin2 = abs(Y1);
+
+    pwm_value1 = fabs(Y1);
 }
 
 void ControlMotor2() {
-    if (Y2 > 0) {
-        Y2 = 0.5f * Y2 + 0.5f;
+    if (Y2 > 0.1f) {
+        Y2 = 0.3f * Y2 + 0.2f;
         directionpin2 = true;
     } 
-    else if(Y2 < 0){
-        Y2 = 0.5f - 0.5f * Y2;
+    else if(Y2 < -0.1f){
+        Y2 = -0.2f + 0.3f * Y2;
         directionpin2 = false;
     }
-    pwmpin1 = abs(Y2);
+    else {
+        Y2 = 0;
+    }
+    pwm_value2 = fabs(Y2);
+}
+
+void motorPWM() {
+    pwmpin1 = pwm_value1;
+    pwmpin2 = pwm_value2;
+} 
+
+void tickermain() {
+    if(!buttonleft){
+        emg0Bool = true;
+    }
+    else if(!buttonright) {
+        emg2Bool = true;
+    }
+    xDirection(); //call the function to move in the y direction
+    yDirection(); //call the function to move in the x direction
+    PIDController1();
+    PIDController2();
+    ControlMotor1();
+    ControlMotor2();
+    
+    if (!KillSwitch) { //even in mekaar gebeund voor het hebben van een stop knop
+        ledred = false;
+        pwm_value1 = 0;
+        pwm_value2 = 0;
+        //for fun blink sos maybe???
+        wait(2.0f);
+        bool u = true;
+
+        while(u) {
+            if (!KillSwitch) {
+                u = false;
+                ledred = true;
+                wait(1.0f);
+            }
+        }
+    }
+    scope.set(0, pwm_value1);
+    scope.set(1, P1);
+    scope.set(2, Y2);
+    scope.set(3, P2);
+    scope.set(4, motor1_angle);
+    scope.set(5, motor2_angle);
+    scope.send(); 
+                  
+    //pwm_value1 = 0;
+    //pwm_value2 = 0;
+    //Y1 = 0;
+    //Y2 = 0;
+    
+    emg0Bool = false;
+    emg1Bool = false;
+    emg2Bool = false;
 }
 
 int main() {
-    pc.baud(115200);
-    pc.printf(" ** program reset **\n\r");
+    //pc.baud(115200);
+    //pc.printf(" ** program reset **\n\r");
+    mainticker.attach(&tickermain, Timestep);
+    pwm_ticker.attach(&motorPWM, Timestep);
     ledred = true;
     
     while (true) {
-        
-        //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
-        a = pc.readable();
-        if (a) {
-            c = pc.getc();
-            switch (c){
-                case 'a': //move to the left
-                    emg0Bool = true; 
-                    break;           
-                case 'd': //move to the right
-                    emg1Bool = true;
-                    break;            
-                case 'w': //move up/down
-                    emg2Bool = true;
-                    break;
-            }
-        }
-        xDirection(); //call the function to move in the y direction
-        yDirection(); //call the function to move in the x direction
-        PIDController1();
-        PIDController2();
-        ControlMotor1();
-        ControlMotor2();
-        
-        if (!KillSwitch) { //even in mekaar gebeund voor het hebben van een stop knop
-            ledred = false;
-            pwmpin1 = 0;
-            pwmpin2 = 0;
-            //for fun blink sos maybe???
-            wait(2.0f);
-            bool u = true;
-
-            while(u) {
-                if (!KillSwitch) {
-                    u = false;
-                    ledred = true;
-                    wait(1.0f);
-                }
-            }
-        }
-        scope.set(0, Y1);
-        scope.set(1, Y2);
-        scope.set(2, motor1_angle);
-        scope.set(3, motor2_angle);
-        scope.set(4, x_position);
-        scope.set(5, y_position);
-        scope.send(); 
-                      
-        // print the motor angles and coordinates
-        pc.printf("position: (%f, %f)\n\r", x_position, y_position);
-        pc.printf("motor1 angle: %f\n\r", motor1_angle);
-        pc.printf("motor2 angle: %f\n\r", motor2_angle);
-        pc.printf("output1: %f\r\n", Output1);
-        pc.printf("output2: %f\r\n", Output2);
-        pc.printf("P1: %f\r\n", P1);
-        pc.printf("P2: %f\r\n", P2);
-    
-        pc.printf("dirctionpin: %f\r\n\n", (float)directionpin1);
-        pc.printf("plek1: %f\r\n", Plek1);
-        pc.printf("plek2: %f\r\n", Plek2);
-
-        wait(Timestep); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
-        pwmpin1 = 0;
-        pwmpin2 = 0;
     }
 }
\ No newline at end of file