Diff: main.cpp

Revision:

12:bf27dea46565

Parent:

11:325a545a757e

--- a/main.cpp	Fri Oct 26 08:34:50 2018 +0000
+++ b/main.cpp	Mon Oct 29 16:00:27 2018 +0000
@@ -2,32 +2,33 @@
 #include "math.h"
 #include "MODSERIAL.h"
 #include "HIDScope.h"
-#include "encoder.h"
+#include "QEI.h"
 #define PI 3.141592f //65358979323846  // pi 
 
 PwmOut pwmpin1(D6);
 PwmOut pwmpin2(D5);
-DigitalOut directionpin1(D4);
-DigitalOut directionpin2(D7);
-Encoder motor1(D13,D12);
-Encoder motor2(D11,D10);
+DigitalOut directionpin2(D4);
+DigitalOut directionpin1(D7);
+QEI motor2(D13,D12,NC, 32);
+QEI motor1(D11,D10,NC, 32);
 DigitalOut ledred(LED_RED);
 
 DigitalIn KillSwitch(SW2);
 DigitalIn button(SW3);
 MODSERIAL pc(USBTX, USBRX);
-//HIDScope scope(2);
+HIDScope scope(6);
 
 //values of inverse kinematics
 volatile bool emg0Bool = false;
 volatile bool emg1Bool = false;
 volatile bool emg2Bool = false;
-volatile bool y_direction = true;
+volatile bool x_direction = true;
 volatile bool a;
 
 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;
 
 volatile float x_position = length;
 volatile float y_position = 0.0;
@@ -41,8 +42,8 @@
 volatile char c;
 
 //values of PID controller
-const float Kp = 2;
-const float Ki = 0.2;
+const float Kp = 1;
+const float Ki = 0.0001;
 const float Kd = 0;
 float Output1 = 0 ;      //Starting value
 float Output2 = 0 ;      //Starting value
@@ -50,121 +51,57 @@
 float P2 = 0;
 float e1 = 0 ;          //Starting value 
 float e2 = 0 ;          //Starting value
-float e3;  
+float e3 = 0;  
 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 f3 = 0;     
+float Output_Last1 = 0;      // Remember previous position
+float Output_Last2 = 0;      // 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
-      
-void yDirection() {
-    //direction of the motion
-    if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
-//        directionpin1 = true;
-//        directionpin2 = true;
-        direction = -1;
-    }
-    else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right
-//        directionpin1 = false;
-//        directionpin2 = false;
-        direction = 1;
-    }
 
-    if (emg0Bool || emg1Bool){
-        //calculating the motion
-        old_y_position = y_position;
-        y_position = old_y_position + (0.1f * direction);
-        old_motor2_angle = motor2_angle;
-        motor2_angle = asin( y_position / length );     //saw tooth motor angle in rad
-        
-        //correction from motor 1 to keep x position the same
-        old_x_position = x_position;
-        x_position = old_x_position + cos( motor2_angle ) - cos( old_motor2_angle );
-        old_motor1_angle = motor1_angle;
-        motor1_angle = old_motor1_angle + ( x_position - old_x_position) / C1;     //rotational-gear motor angle in rad
-    }
-    
-    //reset the booleans, only for demo purposes
-    emg0Bool = false;
-    emg1Bool = false;
-}
-
-void xDirection () {      
-    //if the button is pressed, reverse the y direction
-    if (!button) {
-        x_direction = !x_direction;
-        wait(0.5f);
-        }     
-        
-    if (emg2Bool) { //if w is pressed, move up/down
-        //direction of the motion
-        if (x_direction) { 
-//            directionpin2 = true;
-            direction = 1.0;
-        }
-        else if (!x_direction) {
-//            directionpin2 = false;
-            direction = -1.0;
-        }
-        
-        //calculating the motion
-        old_x_position = x_position;
-        x_position = old_x_position + (0.1f * direction);
-        old_motor1_angle = motor1_angle;
-        motor1_angle = old_motor1_angle + ( x_position - length ) / C2; // sawtooth-gear motor angle in rad
-        
-        //reset the boolean, for demo purposes
-        emg2Bool = false; 
-    }
-}
-
-void PIDController1() {   
-    P1 = motor1.getPosition() / 8400 * 2*PI;    //actual motor angle in rad  
+void PIDController1() { 
+  
+    P1 = motor1.getPulses() / 8400 * 2*PI;    //actual motor angle in rad  
     e1 = e2;
     e2 = e3;
     e3 = motor1_angle - P1;
-    Output_Last1 = Output1;
-    Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
-    Y1 = Output1;
-     
-    if (Output1 >= 1){
+    float de3 = (e3-e2)/Timestep;
+    float ie3 = ie3 + e3*Timestep;
+    Output2 = Kp * e3 + Ki * ie3 + Kd * de3;
+    
+//    Output_Last1 = Output1;
+//    Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
+    Y1 = 0.5f * Output1;
+    
+    if (Y1 >= 1){
         Y1 = 1;
     }
-    else if (Output1 <= -1){
+    else if (Y1 <= -1){
         Y1 = -1;
     }    
-
-/*    scope.set(0,Output1);
-    scope.set(1,P1);
-    scope.send();
-*/    pc.printf("motor1 encoder: %f\r\n", P1);
 }
 
 void PIDController2() {   
-    P2 = motor2.getPosition() / 8400 * 2*PI; // actual motor angle in rad
-    f1 = f2;
+    P2 = motor2.getPulses() / 8400.0f * 2.0f*PI; // actual motor angle in rad
     f2 = f3;
     f3 = motor2_angle - P2;
-    Output_Last2 = Output2;
-    Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
-    Y2 = Output2;
+    float df3 = (f3-f2)/Timestep;
+    float 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);
+    Y2 = 0.5f * Output2;
      
-    if (Output2 >= 1){
+    if (Y2 >= 1){
         Y2 = 1;
     }
-    else if (Output2 <= -1){
+    else if (Y2 <= -1){
         Y2 = -1;
-    }    
-
-/*    scope.set(0,Output2);
-    scope.set(1,P2);
-    scope.send();
-*/    pc.printf("motor2 encoder: %f\r\n", P2);
+    }   
 }
 
 void ControlMotor1() {
@@ -176,19 +113,19 @@
         Y1 = 0.6f - 0.4f * Y1;
         directionpin1 = false;
     }    
-    pwmpin1 = abs(Y1);
+    pwmpin2 = abs(Y1);
 }
 
 void ControlMotor2() {
-    if (Y1 > 0) {
-        Y1 = 0.5f * Y1 + 0.5f;
+    if (Y2 > 0) {
+        Y2 = 0.5f * Y2 + 0.5f;
         directionpin2 = true;
     } 
-    else if(Y1 < 0){
-        Y1 = 0.5f - 0.5f * Y1;
+    else if(Y2 < 0){
+        Y2 = 0.5f - 0.5f * Y2;
         directionpin2 = false;
     }
-    pwmpin2 = abs(Y2);
+    pwmpin1 = abs(Y2);
 }
 
 int main() {
@@ -197,11 +134,6 @@
     ledred = true;
     
     while (true) {
-        //if the button is pressed, reverse the y direction
-        if (!button) {
-            y_direction = !y_direction;
-            wait(0.5f); //wait for person to release the button
-        }
         
         //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
         a = pc.readable();
@@ -211,20 +143,15 @@
                 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 (emg0Bool == true){
+            motor1_angle = 1/6*3.14; //45 graden
+            PIDController1();
+            PIDController2();
+            ControlMotor1();
+            ControlMotor2();
+            }
         
         if (!KillSwitch) { //even in mekaar gebeund voor het hebben van een stop knop
             ledred = false;
@@ -242,14 +169,6 @@
                 }
             }
         }
-                       
-        // 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\n", motor2_angle);
-
-        wait(0.25f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
-        pwmpin1 = 0;
-        pwmpin2 = 0;
+        wait(Timestep); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
     }
 }
\ No newline at end of file