Diff: main.cpp

Revision:

14:a98bc99ea004

Parent:

13:2d2763be031c

Child:

15:6f5b97d006c2

--- a/main.cpp	Wed Oct 31 16:15:56 2018 +0000
+++ b/main.cpp	Thu Nov 01 15:51:40 2018 +0000
@@ -13,7 +13,7 @@
 QEI motor2(D11,D10,NC, 32);
 DigitalOut ledred(LED_RED);
 
-DigitalIn KillSwitch(SW2);
+InterruptIn KillSwitch(SW2);
 DigitalIn button(SW3);
 MODSERIAL pc(USBTX, USBRX);
 HIDScope scope(6);
@@ -34,8 +34,10 @@
 
 volatile float x_position = length;
 volatile float y_position = 0.0;
+volatile float old_x_position;
 volatile float old_y_position;
-volatile float old_x_position;
+volatile float x_correction;
+volatile float old_x_correction;
 volatile float old_motor1_angle;
 volatile float old_motor2_angle;
 volatile float motor1_angle = 0.0; //sawtooth gear motor
@@ -44,9 +46,9 @@
 volatile char c;
 
 //values of PID controller
-const float Kp = 0.05;
-const float Ki = 0.01;
-const float Kd = 0;
+const float Kp = 0.5;
+const float Ki = 0.001;
+const float Kd = 0.02;
 volatile float Output1 = 0 ;      //Starting value
 volatile float Output2 = 0 ;      //Starting value
 volatile float P1 = 0;           //encoder value
@@ -88,14 +90,21 @@
         
         //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 )* C1;     //saw tooth motor angle in rad
+        y_position = old_y_position + (0.0003f * direction);
+        if (y_position > 0.29f){
+            y_position = 0.29f;
+        }
+        else if (y_position < -0.29f){
+            y_position = -0.29f;
+        }
+        
+            
+        old_motor1_angle = motor1_angle;
+        motor1_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/ C1)-cos(old_motor2_angle/ C1)); // old x + correction
-        old_motor1_angle = motor1_angle;
-        motor1_angle = old_motor1_angle + ( x_position - length ) / C2;
+        old_x_correction = x_correction;
+        x_correction = old_x_correction + (length * cos(old_motor1_angle / C1)- length * cos(motor1_angle/ C1)); // old x + correction
+        motor2_angle = ( x_position + x_correction - (length * cos(motor1_angle /C1 ))) / C2;
         
     }
     
@@ -122,9 +131,14 @@
         
         //calculating the motion
         old_x_position = x_position;
-        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
+        x_position = old_x_position + (0.0003f * direction);
+        if (x_position + x_correction > 0.35f ){
+            x_position = 0.35f - x_correction;
+        }
+        else if (x_position < 0.0f){
+            x_position = 0.0f;
+        }
+        motor2_angle =( x_position + x_correction - (length * cos( motor1_angle / C1 ))) /C2 ; // sawtooth-gear motor angle in rad
         
         //reset the boolean, for demo purposes
         emg2Bool = false; 
@@ -134,7 +148,7 @@
 volatile float Plek1;
 void PIDController1() { 
     //Plek1 = motor1.getPulses();  
-    P1 = motor1.getPulses() / 8400.0f * 2.0f*PI;    //actual motor angle in rad  
+    P1 = motor1.getPulses() / 8400.0f * 2.0f * PI;    //actual motor angle in rad  
     e1 = e2;
     e2 = e3;
     e3 = motor1_angle - P1;
@@ -190,17 +204,15 @@
 }
 
 void ControlMotor2() {
-    if (Y2 > 0.1f) {
-        Y2 = 0.3f * Y2 + 0.2f;
+    if (Y2 > 0.0f) {
+        Y2 = 0.8f * Y2 + 0.2f;
         directionpin2 = true;
     } 
-    else if(Y2 < -0.1f){
+    else if(Y2 < -0.0f){
         Y2 = -0.2f + 0.3f * Y2;
         directionpin2 = false;
     }
-    else {
-        Y2 = 0;
-    }
+
     pwm_value2 = fabs(Y2);
 }
 
@@ -225,23 +237,23 @@
     
     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) {
+            pwm_value1 = 0;
+            pwm_value2 = 0;
             if (!KillSwitch) {
                 u = false;
                 ledred = true;
-                wait(1.0f);
+                wait(2.0f);
             }
         }
     }
-    scope.set(0, pwm_value1);
-    scope.set(1, P1);
-    scope.set(2, Y2);
+    scope.set(0, motor2_angle);
+    scope.set(1, P2);
+    scope.set(2, y_position);
     scope.set(3, P2);
     scope.set(4, motor1_angle);
     scope.set(5, motor2_angle);
@@ -266,4 +278,4 @@
     
     while (true) {
     }
-}
\ No newline at end of file
+}