Diff: main.cpp

Revision:

19:aa7e88a631ad

Parent:

18:60251896da8d

Child:

20:3424ef411538

--- a/main.cpp	Mon Nov 02 14:54:54 2015 +0000
+++ b/main.cpp	Mon Nov 02 15:44:43 2015 +0000
@@ -15,7 +15,6 @@
 AnalogIn    EMG_bicepsleft(A1);
 AnalogIn    EMG_legright(A2);
 AnalogIn    EMG_legleft(A3);
-Ticker      controller;
 Ticker      ticker_regelaar;
 Ticker      EMG_Filter;
 Ticker      Scope;
@@ -35,13 +34,12 @@
 }
 
 //define states
-enum state {HOME, MOVE_MOTORS, BACKTOHOMEPOSITION, STOP};
-uint8_t state = HOME;
+enum state {MOVE_MOTORS, BACKTOHOMEPOSITION, STOP};
+uint8_t state = MOVE_MOTORS;
 
-// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering (PI-controller)
-const double g = 360; // Aantal graden 1 rotatie
-const double c = 4200; // Aantal counts 1 rotatie
-const double q = g/c;
+// Berekening van de output shaft resolution. Het aantal counts per 1 graden verandering, 4200 counts per rotatie
+const double q1 = 360/4200; // graden per counts motor 1
+const double q2 = 2.166*360/4200; // graden per counts motor 2 (gear ratio 2.166)
 
 // Filter1 = High pass filter tot 20 Hz
 double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;
@@ -165,19 +163,13 @@
 
 const int pressed = 0;
 
-// constantes voor berekening homepositie
-double P1 = q*motor1.getPosition();
-double P2 = 2.166*q*motor2.getPosition();
-// Safety stop. Motoren mogen niet verder dan 90 graden bewegen.
-volatile bool safety_stop;
-
 void move_motor1()
 {
-    if (button_1 == pressed || (final_filter1 > 0.05 && final_filter2 < 0.02 && final_filter3 < 0.04 && final_filter4 < 0.04)) {
+    if (button_2 == pressed || (final_filter1 > 0.05 && final_filter2 < 0.02 && final_filter3 < 0.04 && final_filter4 < 0.04)) {
         pc.printf("motor1 cw \n\r");
         motor1_direction = 1; //clockwise
         motor1_speed = 0.1;
-    } else if (safety_stop == true && (final_filter2 > 0.05 && final_filter1 < 0.02 && final_filter3 < 0.04 && final_filter4 < 0.04)) {
+    } else if (final_filter2 > 0.05 && final_filter1 < 0.02 && final_filter3 < 0.04 && final_filter4 < 0.04) {
         pc.printf("motor1 ccw \n\r");
         motor1_direction = 0; //counterclockwise
         motor1_speed = 0.1  ;
@@ -189,31 +181,14 @@
 
 void move_motor2()
 {
-    if (safety_stop == true && (final_filter3 > 0.04 && final_filter1 < 0.02 && final_filter2 < 0.02 && final_filter4 < 0.02)) {
+    if (final_filter3 > 0.04 && final_filter1 < 0.02 && final_filter2 < 0.02 && final_filter4 < 0.02) {
         pc.printf("motor2 cw \n\r");
-        motor2_direction = 1; //clockwise
-        motor2_speed = 0.4;
-    } else if (button_2 == pressed || (final_filter4 > 0.03 && final_filter1 < 0.02 && final_filter2 < 0.02 && final_filter3 < 0.02)) {
+        motor2_direction = 0; //clockwise (M2 tegengestelde richting v. m1 0 = cw, 1 = ccw)
+        motor2_speed = 0.4; // snelheid op 1 zetten voor gebruik, lager voor testen
+    } else if (button_1 == pressed || (final_filter4 > 0.03 && final_filter1 < 0.02 && final_filter2 < 0.02 && final_filter3 < 0.02)) {
         pc.printf("motor2 ccw \n\r");
-        motor2_direction = 0; //counterclockwise
-        motor2_speed = 0.08;
-        }
-         else if (P2 > 500) {
-        safety_stop = false;
-        pc.printf("Stop! /n/r");
-        motor2_direction = 1;
-        motor2_speed = 0.1;
-        wait(0.2);
-        safety_stop = true;
-        pc.printf("En door /n/r");
-    } else if (P2 < -500) {
-        safety_stop = false;
-        pc.printf("Stop! /n/r");
-        motor2_direction = 0;
-        motor2_speed = 0.1;
-        wait(0.2);
-        safety_stop = true;
-        pc.printf("En gaan /n/r");
+        motor2_direction = 1; //counterclockwise
+        motor2_speed = 0.1; // snelheid op 1 zetten voor gebruik, lager voor testen   
     } else {
         pc.printf("Not moving2 \n\r");
         motor2_speed = 0;
@@ -223,14 +198,14 @@
 
 void movetohome1()
 {
-   double Q1 = motor1.getPosition();
+   double P1 = motor1.getPosition();
 
-    if ((Q1 > -13 && Q1 < 13)) {
+    if ((P1 > -13 && P1 < 13)) {
         motor1_speed = 0;
-    } else if (Q1 > 12) {
+    } else if (P1 > 12) {
         motor1_direction = 1;
         motor1_speed = 0.1;
-    } else if (Q1 < -12) {
+    } else if (P1 < -12) {
         motor1_direction = 0;
         motor1_speed = 0.1;
     } 
@@ -238,14 +213,14 @@
 
 void movetohome2()
 {
-  double Q2 = motor2.getPosition();
-
-    if (Q2 > -26 && Q2 < 26){
+   double P2 = motor2.getPosition();
+   
+    if (P2 > -13 && P2 < 13){
         motor2_speed = 0;
-    }else if (Q2 > 25) {
+    } else if (P2 > 12) {
         motor2_direction = 0;
         motor2_speed = 0.1;
-    } else if (Q2 < -12) {
+    } else if (P2 < -12) {
         motor2_direction = 1;
         motor2_speed = 0.1;
     }
@@ -253,33 +228,27 @@
 
 void HIDScope ()
 {
-    scope.set (0, final_filter1);
-    scope.set (1, final_filter2);
-    scope.set (2, final_filter3);
-    scope.set (3, final_filter4);
-    scope.set (4, P1);
-    scope.set (5, P2);
+    scope.set (0, q1*motor1.getPosition());
+    scope.set (1, q2*motor2.getPosition());
+    scope.set (2, final_filter1);
+    scope.set (3, final_filter2);
+    scope.set (4, final_filter3);
+    scope.set (5, final_filter4);
     scope.send ();
 }
 
 int main()
 {
-    safety_stop = true;
     while (true) {
         pc.baud(9600);                          //pc baud rate van de computer
-        EMG_Filter.attach_us(Filters, 5e3);     //filters uitvoeren
-        Scope.attach_us(HIDScope, 5e3);         //EMG en encoder signaal naar de hidscope sturen
+        EMG_Filter.attach_us(Filters, 5e4);     //filters uitvoeren
+        Scope.attach_us(HIDScope, 5e4);         //EMG en encoder signaal naar de hidscope sturen
 
         switch (state) {                            //zorgt voor het in goede volgorde uitvoeren van de cases
 
-            case HOME: {    //positie op 0 zetten voor arm 1
-                pc.printf("home\n\r");
-                state = MOVE_MOTORS;
-                wait(2);
-                break;
-            }
+        
 
-            case MOVE_MOTORS: {          //motor kan cw en ccw bewegen a.d.h.v. buttons
+            case MOVE_MOTORS: {          //Bewegen motor met EMG, knoppen indrukken om terug te gaan
                 pc.printf("move_motor\n\r");
                 while (state == MOVE_MOTORS) {
                     move_motor1();
@@ -306,13 +275,11 @@
                     while(regelaar_ticker_flag != true);
                     regelaar_ticker_flag = false;
 
-                    pc.printf("motor1 pos %d, motor2 pos %d", P1, P2);
+                    pc.printf("motor1 pos %d counts \n\r motor2 pos %d counts \n\r", motor1.getPosition(), motor2.getPosition());
 
-                    if (P1 < 2 && P1 > -2 && P2 < 2 && P2 > -2) {
-                        pc.printf("motor1 pos %d \n\r", P1);
-                        pc.printf("motor2 pos %d \n\r", P2);
+                    if (motor1_speed == 0 && motor2_speed == 0) {
+                        pc.printf("Laatste positie m1 %d graden, m2 %d graden\n\r", q1*motor1.getPosition(),q2*motor2.getPosition());
                         state = STOP;
-                        pc.printf("Laatste positie m1 %d deg, m2 %d deg\n\r", P1, P2);
                         break;
                     }
                 }