Diff: main.cpp

Revision:

8:fd00916552e0

Parent:

7:9e0ded88fe60

Child:

9:e144fd53f606

--- a/main.cpp	Mon Oct 22 10:00:09 2018 +0000
+++ b/main.cpp	Tue Oct 23 13:15:22 2018 +0000
@@ -4,13 +4,18 @@
 #include "HIDScope.h"
 #include "encoder.h"
 
-DigitalIn button(SW2);
-DigitalOut directionpin1(D7);
-DigitalOut directionpin2(D8);
+PwmOut pwmpin1(D6);
+PwmOut pwmpin2(D5);
+DigitalOut directionpin1(D4);
+DigitalOut directionpin2(D7);
+Encoder motor1(D13,D12);
+Encoder motor2(D11,D10);
+DigitalOut ledred(LED_RED);
+
+DigitalIn KillSwitch(SW2);
+DigitalIn button(SW3);
 MODSERIAL pc(USBTX, USBRX);
 HIDScope scope(2);
-Encoder motor1(D13,D12);
-Encoder motor2(D13,D12); //has to be changed!!!
 
 //values of inverse kinematics
 volatile bool emg0Bool = false;
@@ -38,11 +43,10 @@
 const float Kp = 2;
 const float Ki = 0.2;
 const float Kd = 0;
-const float Timestep = 0.001;
-float G ;               //desired motor angle
 float Output1 = 0 ;      //Starting value
 float Output2 = 0 ;      //Starting value
-float P = 0;           //encoder value
+float P1 = 0;           //encoder value
+float P2 = 0;
 float e1 = 0 ;          //Starting value 
 float e2 = 0 ;          //Starting value
 float e3;  
@@ -51,20 +55,22 @@
 float f3;     
 float Output_Last1;      // Remember previous position
 float Output_Last2;      // Remember previous position
-float Y;                // Value that is outputted to motor control
+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
-const float Max_Speed = 400;      //Max speed of the motor
       
 void xDirection() {
     //direction of the motion
     if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left
-        directionpin1 = true;
-        directionpin2 = true;
+//        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;
+//        directionpin1 = false;
+//        directionpin2 = false;
         direction = 1;
     }
 
@@ -79,7 +85,7 @@
         motor2_angle = ( y_position / C2) + acos( x_position / length );     //sawtooth-gear motor angle in rad
     }
     
-    //reset the booleans
+    //reset the booleans, only for demo purposes
     emg0Bool = false;
     emg1Bool = false;
 }
@@ -88,11 +94,11 @@
     if (emg2Bool) { //if w is pressed, move up/down
         //direction of the motion
         if (y_direction) { 
-            directionpin2 = true;
+//            directionpin2 = true;
             direction = 1;
         }
         else if (!y_direction) {
-            directionpin2 = false;
+//            directionpin2 = false;
             direction = -1;
         }
         
@@ -101,64 +107,92 @@
         y_position = old_y + (0.1f * direction);
         motor2_angle = y_position / C2; // sawtooth-gear motor angle in rad
         
-        //reset the boolean
+        //reset the boolean, only for demo purposes
         emg2Bool = false; 
     }
 }
 
 void PIDController1() {   
-    P = motor1.getPosition() / 8400 * 360;        
+    P1 = motor1.getPosition() / 8400 * 360;        
     e1 = e2;
     e2 = e3;
-    e3 = G - P;
+    e3 = motor1_angle - P1;
     Output_Last1 = Output1;
     Output1 = Kp * (e3 - e2) + Output_Last1 +Ki * e3 + Kd * (e3 - 2*e2 + e1);
-    Y = Output1;
+    Y1 = Output1;
      
     if (Output1 >= 1){
-        Y = 1;
+        Y1 = 1;
     }
     else if (Output1 <= -1){
-        Y = -1;
+        Y1 = -1;
     }    
-    P = P_Last + Y * Timestep * Max_Speed;
 
     scope.set(0,Output1);
-    scope.set(1,P);
+    scope.set(1,P1);
     scope.send();
+    pc.printf("motor1 encoder: %f\r\n", P1);
 }
 
 void PIDController2() {   
-    P = motor2.getPosition() / 8400 * 360;        
+    P2 = motor2.getPosition() / 8400 * 360;        
     f1 = f2;
     f2 = f3;
-    f3 = G - P;
+    f3 = motor2_angle - P2;
     Output_Last2 = Output2;
     Output2 = Kp * (f3 - f2) + Output_Last2 +Ki * f3 + Kd * (f3 - 2*f2 + f1);
-    Y = Output2;
+    Y2 = Output2;
      
     if (Output2 >= 1){
-        Y = 1;
+        Y2 = 1;
     }
     else if (Output2 <= -1){
-        Y = -1;
+        Y2 = -1;
     }    
-    P = P_Last + Y * Timestep * Max_Speed;
 
     scope.set(0,Output2);
-    scope.set(1,P);
+    scope.set(1,P2);
     scope.send();
+    pc.printf("motor2 encoder: %f\r\n", P2);
+}
+
+void ControlMotor1() {
+    if (Y1 >= 0) {
+        Y1 = 0.6f * Y1 + 0.4f;
+        directionpin1 = true;
+    } 
+    else if(Y1 < 0){
+        Y1 = 0.6f - 0.4f * Y1;
+        directionpin1 = false;
+    }
+    
+    pwm1 = abs(Y1);
+    pwmpin1 = pwm1;
+}
+
+void ControlMotor2() {
+    if (Y1 >= 0) {
+        Y1 = 0.5f * Y1 + 0.5f;
+        directionpin2 = true;
+    } 
+    else if(Y1 < 0){
+        Y1 = 0.5f - 0.5f * Y1;
+        directionpin2 = false;
+    }
+    pwm2 = abs(Y2);
+    pwmpin2 = pwm2;
 }
 
 int main() {
     pc.baud(115200);
     pc.printf(" ** program reset **\n\r");
+    ledred = true;
     
     while (true) {
         //if the button is pressed, reverse the y direction
         if (!button) {
             y_direction = !y_direction;
-            wait(0.5);
+            wait(0.5f); //wait for person to release the button
         }
         
         //testing the code from keyboard imputs: a-d: left to right, w: forward/backwards
@@ -181,7 +215,26 @@
         xDirection(); //call the function to move in the x direction
         PIDController1();
         PIDController2();
+        ControlMotor1();
+        ControlMotor2();
         
+        if (!KillSwitch) {
+            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);
+                }
+            }
+        }
+                       
         // 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);