Diff: main.cpp

Revision:

12:9a2d3d544426

Parent:

11:325a545a757e

Child:

13:2d2763be031c

--- a/main.cpp	Fri Oct 26 08:34:50 2018 +0000
+++ b/main.cpp	Tue Oct 30 09:22:44 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,7 +42,7 @@
 volatile char c;
 
 //values of PID controller
-const float Kp = 2;
+const float Kp = 1;
 const float Ki = 0.2;
 const float Kd = 0;
 float Output1 = 0 ;      //Starting value
@@ -65,28 +66,27 @@
 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){
+    if (emg0Bool || emg1Bool){  
+        //correction from motor 1 to keep x position the same
+        
+        
         //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
+        motor2_angle = asin( y_position / length )* C1;     //saw tooth motor angle in rad
+        //correction on x- axis
         old_x_position = x_position;
-        x_position = old_x_position + cos( motor2_angle ) - cos( old_motor2_angle );
+        x_position = old_x_position + (cos(motor2_angle/ C1)-cos(old_motor2_angle/ C1)); // old x + correction
         old_motor1_angle = motor1_angle;
-        motor1_angle = old_motor1_angle + ( x_position - old_x_position) / C1;     //rotational-gear motor angle in rad
+        motor1_angle = old_motor1_angle + ( x_position - length ) / C2;
+        
     }
     
     //reset the booleans, only for demo purposes
@@ -104,17 +104,15 @@
     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);
+        x_position = old_x_position + (0.01f * direction);
         old_motor1_angle = motor1_angle;
         motor1_angle = old_motor1_angle + ( x_position - length ) / C2; // sawtooth-gear motor angle in rad
         
@@ -123,48 +121,47 @@
     }
 }
 
-void PIDController1() {   
-    P1 = motor1.getPosition() / 8400 * 2*PI;    //actual motor angle in rad  
+volatile float Plek1;
+void PIDController1() { 
+    Plek1 = motor1.getPulses();  
+    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);
 }
-
+volatile float Plek2;
 void PIDController2() {   
-    P2 = motor2.getPosition() / 8400 * 2*PI; // actual motor angle in rad
-    f1 = f2;
+    Plek2 = motor2.getPulses();
+    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 +173,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 +194,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();
@@ -242,13 +234,28 @@
                 }
             }
         }
-                       
+        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\n", motor2_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(0.25f); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
+        wait(Timestep); //can also be done with ticker, to be sure that it happens exactly every 0.5 seconds
         pwmpin1 = 0;
         pwmpin2 = 0;
     }