Diff: main.cpp

Revision:

1:bae7d30014b7

Parent:

0:9e2ed7d4d2ea

Child:

2:05e91f425d43

--- a/main.cpp	Mon Jul 04 06:09:33 2016 +0000
+++ b/main.cpp	Tue Jul 05 12:11:26 2016 +0000
@@ -1,30 +1,218 @@
+/*  Test    MX-28 ID: 0x01
+    Pan     RX-28 ID: 0x02 (DMXL04)
+    Tilt    RX-28 ID: 0x03 (DMXL05)
+    Base    MX-28 ID: 0x04
+    Elbow   RX-28 ID: 0x05 (DMXL06)
+    Wrist   RX-28 ID: 0x06 (DMXL08)
+    //DON'T GO TO ZERO ON BASE SERVO
+*/
+
 #include "mbed.h"
 #include "Dynamixel.h"
 Serial pc(USBTX,USBRX);
+Dynamixel Base(PA_9, PA_10, PB_3, 4, 57600);
+Dynamixel Elbow(PA_9, PA_10, PB_3, 5, 57600);
+DigitalOut myled(LED1);
+uint8_t pos;
+
+void centrePosition() {
+    Base.setGoalPosition(800);
+    Elbow.setGoalPosition(50);
+    pos = 2;
+}
+
+void leftPosition() {
+    uint16_t currentPosBase;
+    uint16_t currentPosElbow;
+    
+    if (pos == 1) {
+        centrePosition();
+        currentPosBase = Base.getPresentPosition();
+        currentPosElbow = Elbow.getPresentPosition();
+        while(currentPosBase > 820 && currentPosElbow > 70) {
+            currentPosBase = Base.getPresentPosition();
+            currentPosElbow = Elbow.getPresentPosition();
+        }
+    }
+    Base.setGoalPosition(300);
+    Elbow.setGoalPosition(50);
+    pos = 3;
+}
+
+void rightPosition() {
+    uint16_t currentPosBase;
+    uint16_t currentPosElbow;
+    
+    if (pos == 1) {
+        centrePosition();
+        currentPosBase = Base.getPresentPosition();
+        currentPosElbow = Elbow.getPresentPosition();
+        while(currentPosBase > 820 && currentPosElbow > 70) {
+            currentPosBase = Base.getPresentPosition();
+            currentPosElbow = Elbow.getPresentPosition();
+        }
+    }
+    Base.setGoalPosition(1300);
+    Elbow.setGoalPosition(50);
+    pos = 4;
+}
+
+void retractedPosition() {
+    uint16_t currentPosBase;
+    
+    if (pos == 3) {
+        centrePosition();
+        currentPosBase = Base.getPresentPosition();
+        while(currentPosBase < 780) {
+            currentPosBase = Base.getPresentPosition();
+        }
+    }
+    else if (pos == 4) {
+        centrePosition();
+        currentPosBase = Base.getPresentPosition();
+        while(currentPosBase > 820) {
+            currentPosBase = Base.getPresentPosition();
+        }
+    }
+    Base.setGoalPosition(1300);
+    Elbow.setGoalPosition(365);
+    pos = 1;
+}
+
+void measureTime() {
+    uint16_t currentPosBase;
+    Timer t;
+    uint32_t time;
+    
+    t.start();
+    currentPosBase = Base.getPresentPosition();
+    while(currentPosBase < 1280) {
+        currentPosBase = Base.getPresentPosition();
+    }
+    time = t.read_ms();
+    pc.printf("Time: %d\r\n",time);
+    t.stop();
+    t.reset();
+}
+
+void setSpeed(uint16_t speed) {
+    Base.setMovingSpeed(1024+speed);
+    Elbow.setMovingSpeed(1024+2.2*speed);
+}
+
+void setLimits() {
+    Base.setCWAngleLimit(300);
+    Elbow.setCWAngleLimit(50);
+    Base.setCCWAngleLimit(1300);
+    Elbow.setCCWAngleLimit(365);
+}
  
 int main()
 {
-    uint8_t delayTest;
-    uint16_t modelNumber;
-    uint16_t id;
-    Dynamixel DX116(PA_9, PA_10, PB_3, 1, 57600);
     pc.baud(9600);
+    setSpeed(200);
     
-    DX116.toggleLED(1);
-    wait(1);
-    DX116.toggleLED(0);
-    wait(1);
-    DX116.move(0);
-    wait(1);
-    DX116.move(4095);
-    wait(1);
-    delayTest = DX116.getReturnDelayTime();
-    pc.printf("Delay Time: 0x%04x\r\n",delayTest);
-    wait(1);
-    modelNumber = DX116.getModelNumber();
-    pc.printf("Model Number: 0x%04x\r\n",modelNumber);
-    wait(1);
-    id = DX116.getId();
-    pc.printf("ID Number: 0x%04x\r\n",id);
+    while(1) {
+        retractedPosition();
+        wait(2);
+        leftPosition();
+        wait(2);
+        rightPosition();
+        wait(2);
+        centrePosition();
+        wait(2);
+        retractedPosition();
+        wait(2);
+        rightPosition();
+        wait(2);
+        centrePosition();
+        wait(2);
+        leftPosition();
+        wait(2);
+    }
+        
+}
+
+/*//MX28a.reset();
+        MX28a.setId(6);
+        //wait(1);
+        MX28a.setEnableLED(1);
+        //RX28a.setEnableLED(0);
+        wait(1);
+        MX28a.setEnableLED(0);
+        //RX28a.setEnableLED(1);
+        wait(1);
+        MX28a.setGoalPosition(0);
+        RX28a.setGoalPosition(1023);
+        wait(1);
+        MX28a.setGoalPosition(4095);
+        RX28a.setGoalPosition(0);
+        wait(1);
+        delayTest = MX28a.getReturnDelayTime();
+        pc.printf("Delay Time: 0x%04x\r\n",delayTest);
+        wait(1);
+        modelNumber = MX28a.getModelNumber();
+        pc.printf("Model Number: 0x%04x\r\n",modelNumber);
+        wait(1);
+        id = MX28a.getId();
+        pc.printf("ID Number: 0x%04x\r\n",id);
+        if (id == 0x01) {
+            myled = !myled;
+        }
+        wait(1);
+        baud = MX28a.getBaudRate();
+        pc.printf("Baud Rate: %d\r\n",baud);
         
-}
\ No newline at end of file
+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;
+int goalPos = 60;
+
+int Xm = 0;
+int Ym = 0;
+int Xe = 0;
+int Ye = 0;
+
+int L = 147; // mm
+int S = 97; // mm
+int thet = 0;
+int gamm = 0;*/
+
+/*currentPosBase = Base.getPresentPosition();
+        pc.printf("Base Current Position: %d\r\n",currentPosBase);
+        currentPosElbow = Elbow.getPresentPosition();
+        pc.printf("Elbow Current Position: %d\r\n",currentPosElbow);
+        //wait(3);
+        if (currentPosElbow == 360 && rar == 1) {
+            rar = -1;
+        }
+        else if (currentPosElbow == 60 && rar == -1) {
+            rar = 1;
+        }
+        Elbow.setGoalPosition(goalPos);
+        
+        goalPos += rar;*/
+        //error = Base.setGoalPosition(0);
+        //Elbow.setGoalPosition(0);
+        //pc.printf("Error: 0x%04x\r\n",error);
+        //wait(1);
\ No newline at end of file