Diff: main.cpp

Revision:

2:05e91f425d43

Parent:

1:bae7d30014b7

Child:

3:09a27cf1c9fe

--- a/main.cpp	Tue Jul 05 12:11:26 2016 +0000
+++ b/main.cpp	Wed Jul 06 00:46:40 2016 +0000
@@ -14,6 +14,7 @@
 Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600);
 DigitalOut myled(LED1);
 uint8_t pos;
+int echo;
 
 void centrePosition() {
     Base.setGoalPosition(800);
@@ -113,23 +114,40 @@
     setSpeed(200);
     
     while(1) {
-        retractedPosition();
-        wait(2);
-        leftPosition();
+        if (echo == 0) {  
+            Base.setGoalPosition(1300);
+            Elbow.setGoalPosition(365); 
+        }
+         
+        echo = pc.getc();
+        pc.printf("\r\n");
+        pc.putc(echo);
+        if (echo == 49) {    retractedPosition();    }
+        else if (echo == 50) {    centrePosition();  }
+        else if (echo == 51) {    leftPosition();    }
+        else if (echo == 52) {    rightPosition();   }
+        else {  continue;    }
         wait(2);
-        rightPosition();
-        wait(2);
-        centrePosition();
-        wait(2);
+            
+        /*
         retractedPosition();
-        wait(2);
+        wait(3);
+        leftPosition();
+        wait(3);
         rightPosition();
-        wait(2);
+        wait(3);
         centrePosition();
-        wait(2);
+        wait(3);
+        retractedPosition();
+        wait(3);
+        rightPosition();
+        wait(3);
+        centrePosition();
+        wait(3);
         leftPosition();
-        wait(2);
+        wait(3);*/
     }
+    
         
 }
 
@@ -163,26 +181,6 @@
         baud = MX28a.getBaudRate();
         pc.printf("Baud Rate: %d\r\n",baud);
         
-int Xm = 0;
-int Ym = 0;
-int Xe = 0;
-int Ye = 0;
-
-int L = 147; // mm
-int S = 97; // mm
-
-int theta = 370;   // Base angle
-int gamma = 60;   // Elbow angle
-
-
-//As Xe increases, alter Xm and Ym to keep Ye at 0
-while (1) {
-    if (Xm > 50) {
-        Xm += 1;
-        theta = arccos(Xm/L);
-        gamma = arccos(Xm/S);
-    }
-}
         
         */
 /*int rar = 1;