Revision 18:60251896da8d, committed 2015-11-02

Comitter:

arunr

Date:

Mon Nov 02 14:54:54 2015 +0000

Parent:

17:416876824d8c

Child:

19:aa7e88a631ad

Commit message:

homepos werkt niet meer;

Changed in this revision

--- a/main.cpp	Mon Nov 02 14:24:46 2015 +0000
+++ b/main.cpp	Mon Nov 02 14:54:54 2015 +0000
@@ -11,8 +11,6 @@
 PwmOut      motor2_speed(D6);
 DigitalIn   button_1(PTC6); //counterclockwise
 DigitalIn   button_2(PTA4); //clockwise
-AnalogIn    PotMeter1(A4);
-AnalogIn    PotMeter2(A5);
 AnalogIn    EMG_bicepsright(A0);
 AnalogIn    EMG_bicepsleft(A1);
 AnalogIn    EMG_legright(A2);
@@ -45,8 +43,6 @@
 const double c = 4200; // Aantal counts 1 rotatie
 const double q = g/c;
 
-
-
 // Filter1 = High pass filter tot 20 Hz
 double ah1_v1=0, ah1_v2=0, ah2_v1=0, ah2_v2=0;
 double bh1_v1=0, bh1_v2=0, bh2_v1=0, bh2_v2=0;
@@ -170,10 +166,8 @@
 const int pressed = 0;
 
 // constantes voor berekening homepositie
-double H1;
-double H2;
-double P1;
-double P2;
+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;
 
@@ -202,7 +196,7 @@
     } else if (button_2 == 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.4;
+        motor2_speed = 0.08;
         }
          else if (P2 > 500) {
         safety_stop = false;
@@ -229,14 +223,14 @@
 
 void movetohome1()
 {
-    P1 = motor1.getPosition();
+   double Q1 = motor1.getPosition();
 
-    if ((P1 > -25 && P1 < 25)) {
+    if ((Q1 > -13 && Q1 < 13)) {
         motor1_speed = 0;
-    } else if (P1 > 24) {
+    } else if (Q1 > 12) {
         motor1_direction = 1;
         motor1_speed = 0.1;
-    } else if (P1 < -24) {
+    } else if (Q1 < -12) {
         motor1_direction = 0;
         motor1_speed = 0.1;
     } 
@@ -244,13 +238,14 @@
 
 void movetohome2()
 {
-    P2 = motor2.getPosition();
-    if (P2 > -25 && P2 < 25){
+  double Q2 = motor2.getPosition();
+
+    if (Q2 > -26 && Q2 < 26){
         motor2_speed = 0;
-    }else if (P2 > 24) {
+    }else if (Q2 > 25) {
         motor2_direction = 0;
         motor2_speed = 0.1;
-    } else if (P2 < -24) {
+    } else if (Q2 < -12) {
         motor2_direction = 1;
         motor2_speed = 0.1;
     }
@@ -262,8 +257,8 @@
     scope.set (1, final_filter2);
     scope.set (2, final_filter3);
     scope.set (3, final_filter4);
-    scope.set (4, motor1.getPosition());
-    scope.set (5, motor2.getPosition());
+    scope.set (4, P1);
+    scope.set (5, P2);
     scope.send ();
 }
 
@@ -279,10 +274,6 @@
 
             case HOME: {    //positie op 0 zetten voor arm 1
                 pc.printf("home\n\r");
-                H1 = motor1.getPosition();
-                H2 = motor2.getPosition();
-                pc.printf("Home-position is %f\n\r", H1);
-                pc.printf("Home-pso is %f\n\r", H2);
                 state = MOVE_MOTORS;
                 wait(2);
                 break;
@@ -315,15 +306,13 @@
                     while(regelaar_ticker_flag != true);
                     regelaar_ticker_flag = false;
 
-                    pc.printf("motor1 pos %d, motor2 pos %d", motor1.getPosition(), motor2.getPosition());
-                    pc.printf("referentie %f, %f \n\r", H1, H2);
+                    pc.printf("motor1 pos %d, motor2 pos %d", P1, P2);
 
-                    if (P1 <=24 && P1 >= -24 && P2 <=24 && P2 >= -24) {
-                        pc.printf("motor1 pos %d", motor1.getPosition());
-                        pc.printf("motor2 pos %d", motor2.getPosition());
-                        pc.printf("referentie %f %f\n\r", H1, H2);
+                    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);
                         state = STOP;
-                        pc.printf("Laatste positie %d %d\n\r", motor1.getPosition(),motor2.getPosition());
+                        pc.printf("Laatste positie m1 %d deg, m2 %d deg\n\r", P1, P2);
                         break;
                     }
                 }