Diff: main.cpp

Revision:

3:09a27cf1c9fe

Parent:

2:05e91f425d43

Child:

4:0c1555b01361

--- a/main.cpp	Wed Jul 06 00:46:40 2016 +0000
+++ b/main.cpp	Wed Jul 06 02:19:02 2016 +0000
@@ -10,16 +10,21 @@
 #include "mbed.h"
 #include "Dynamixel.h"
 Serial pc(USBTX,USBRX);
-Dynamixel Base(PA_9, PA_10, PB_3, 4, 57600);
+Dynamixel Base(PA_9, PA_10, PB_3, 1, 57600);
 Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600);
+Dynamixel Pan(PA_9, PA_10, PB_3, 4, 57600);
+Dynamixel Tilt(PA_9, PA_10, PB_3, 3, 57600);
 DigitalOut myled(LED1);
 uint8_t pos;
+uint16_t tiltInc = 15;
+uint16_t panInc = 60;
 int echo;
 
 void centrePosition() {
     Base.setGoalPosition(800);
     Elbow.setGoalPosition(50);
     pos = 2;
+    wait(2);
 }
 
 void leftPosition() {
@@ -38,6 +43,7 @@
     Base.setGoalPosition(300);
     Elbow.setGoalPosition(50);
     pos = 3;
+    wait(2);
 }
 
 void rightPosition() {
@@ -56,6 +62,7 @@
     Base.setGoalPosition(1300);
     Elbow.setGoalPosition(50);
     pos = 4;
+    wait(2);
 }
 
 void retractedPosition() {
@@ -78,6 +85,7 @@
     Base.setGoalPosition(1300);
     Elbow.setGoalPosition(365);
     pos = 1;
+    wait(2);
 }
 
 void measureTime() {
@@ -106,28 +114,88 @@
     Elbow.setCWAngleLimit(50);
     Base.setCCWAngleLimit(1300);
     Elbow.setCCWAngleLimit(365);
+    Tilt.setCWAngleLimit(210);
+    Tilt.setCCWAngleLimit(1000);
+}
+
+void tiltIncrementUp() {
+    uint16_t currentPosTilt;
+    
+    currentPosTilt = Tilt.getPresentPosition();
+    Tilt.setGoalPosition(currentPosTilt+tiltInc);
+    wait_ms(500);
+}
+
+void tiltIncrementDown() {
+    uint16_t currentPosTilt;
+    
+    currentPosTilt = Tilt.getPresentPosition();
+    Tilt.setGoalPosition(currentPosTilt-tiltInc);
+    wait_ms(500);
+}
+
+void panIncrementLeft() {
+    uint16_t currentPosPan;
+    
+    currentPosPan = Pan.getPresentPosition();
+    Pan.setMovingSpeed(1124);
+    Pan.setGoalPosition(4095);
+}
+
+void panIncrementRight() {
+    uint16_t currentPosPan;
+    
+    currentPosPan = Pan.getPresentPosition();
+    Pan.setMovingSpeed(1124);
+    Pan.setGoalPosition(0);
 }
  
 int main()
 {
     pc.baud(9600);
-    setSpeed(200);
+    //setSpeed(200);
+    uint16_t currentPosTilt;
+    uint16_t currentPosPan;
+    Pan.setCWAngleLimit(0);
+    Pan.setCCWAngleLimit(4095);
+    Pan.setGoalPosition(500);
+    Tilt.setGoalPosition(700);
     
     while(1) {
-        if (echo == 0) {  
-            Base.setGoalPosition(1300);
-            Elbow.setGoalPosition(365); 
-        }
+        //if (echo == 0) {  
+            //Base.setGoalPosition(1300);
+            //Elbow.setGoalPosition(365);
+            
+        //}
          
         echo = pc.getc();
-        pc.printf("\r\n");
+        pc.printf("\r\n %d \r\n", echo);
         pc.putc(echo);
-        if (echo == 49) {    retractedPosition();    }
+        /*if (echo == 49) {    retractedPosition();    }
         else if (echo == 50) {    centrePosition();  }
         else if (echo == 51) {    leftPosition();    }
-        else if (echo == 52) {    rightPosition();   }
-        else {  continue;    }
-        wait(2);
+        else if (echo == 52) {    rightPosition();   }*/
+        if (echo == 119) {   tiltIncrementUp();   }
+        else if (echo == 115) {   tiltIncrementDown();   }
+        else if (echo == 97) {    panIncrementLeft();   }
+        else if (echo == 100) {   panIncrementRight();   }
+        else {  
+            Tilt.setMovingSpeed(1024);
+            Pan.setMovingSpeed(1024);
+            currentPosPan = Pan.getPresentPosition();
+            Pan.setGoalPosition(currentPosPan);
+        }
+        echo = 0;
+        echo = pc.getc();
+        pc.printf("\r\n %d \r\n", echo);
+        pc.putc(echo);
+        
+    
+        currentPosTilt = Tilt.getPresentPosition();
+        //pc.printf("Tilt Position: %d\r\n",currentPosTilt);
+        currentPosPan = Pan.getPresentPosition();
+        //pc.printf("Pan Position: %d\r\n",currentPosPan);
+        
             
         /*
         retractedPosition();