Revision 24:830e4dfa112a, committed 2019-10-31

Comitter:

sanou8

Date:

Thu Oct 31 21:15:56 2019 +0000

Parent:

23:b8fa74336d2a

Commit message:

emg mode met werkende x en y aansturing

Changed in this revision

diff -r b8fa74336d2a -r 830e4dfa112a main.cpp
--- a/main.cpp	Thu Oct 31 20:38:05 2019 +0000
+++ b/main.cpp	Thu Oct 31 21:15:56 2019 +0000
@@ -12,6 +12,7 @@
 Serial pc(USBTX,USBRX);
 DigitalIn button(SW3) ;
 InterruptIn sw2(SW2);
+InterruptIn sw3(SW3);
 
 //Define objects
 AnalogIn    emg0( A0 );
@@ -33,16 +34,16 @@
 
 volatile double emg1_filtered;      //measured value of the first emg
 volatile double emg2_filtered;      //measured value of the second emg
-volatile double emg1_cal = 0.8;
+volatile double emg1_cal = 0.8;     // calibrated value of first emg
+volatile double emg2_cal = 0.8 ;    // calibrated value of second emg
 double Pi = 3.14159265359;
 double gearRatio = 40/9;
 
 double initRot1 = 0;
 double initRot2 = -gearRatio*(180 - 48.5)/360;
-volatile double y_new = 20.0 ;
-volatile double y_prev = 20.0 ;
-double displacement = 1 ;
+
 double speedy = 3 ;
+double speedx = 3 ;
 
 
 
@@ -60,10 +61,12 @@
 void findDesiredLocation(double xStart, double yStart, double xEnd, double yEnd, double speed, Timer *t, vector<double> *desiredLocation);
 void EMG_move();
 
-void changespeed(){
+void changespeedy(){
     speedy = speedy*-1;
     }
-
+void changespeedx(){
+    speedx = speedx*-1;
+    }
 
 int main()
 {
@@ -89,15 +92,19 @@
 
     moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
     moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
-    sw2.rise(changespeed);
+    sw2.rise(changespeedy);
+    sw3.rise(changespeedx);
     while(true) {
         
 
-        while(emg1_filtered >= 0.4*emg1_cal) {
+        while(emg1_filtered >= 0.5*emg1_cal) {
             //controlLED = !controlLED ;
             moveWithSpeed(&xStart, &yStart, 0, speedy);
             pc.printf("ystart: %g \n\r",yStart);
         }
+        while(emg2_filtered >= 0.5*emg2_cal) {
+            moveWithSpeed(&xStart, &yStart, speedx, 0);
+            }
 
         rot1 = calcRot1(xStart, yStart);
         rot2 = calcRot2(xStart, yStart);
@@ -107,32 +114,6 @@
 }
 
 
-void EMG_move(){
-    
-    double xStart = 0;
-    double yStart = 20;
-
-    double rot1 = calcRot1(xStart, yStart);
-    double rot2 = calcRot2(xStart, yStart);
-
-    moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
-    moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
-    sw2.rise(changespeed);
-    while(true) {
-        
-
-        while(emg1_filtered >= 0.4*emg1_cal) {
-            //controlLED = !controlLED ;
-            moveWithSpeed(&xStart, &yStart, 0, speedy);
-            pc.printf("ystart: %g \n\r",yStart);
-        }
-
-        rot1 = calcRot1(xStart, yStart);
-        rot2 = calcRot2(xStart, yStart);
-        moveMotorToPoint(&M1, &E1, &Enc1, initRot2, 1, -rot2);
-        moveMotorToPoint(&M2, &E2, &Enc2, initRot1, -1, rot1);
-    }
-}
 
 
 
@@ -163,6 +144,9 @@
             emg1_cal = emg1_filtered ;
             pc.printf("EMG_cal : %g \n\r",emg1_cal);
         }
+        if(emg2_cal < emg2_filtered) {
+            emg2_cal = emg2_filtered ;
+            }
     } while(tijd<10);
 }