Diff: main.cpp

Revision:

2:37021fb3b45b

Parent:

0:547ac906c46b

Child:

4:36a4eceb1b7f

--- a/main.cpp	Tue Dec 15 01:22:45 2015 +0000
+++ b/main.cpp	Tue Dec 15 04:38:56 2015 +0000
@@ -1,13 +1,116 @@
 #include "mbed.h"
+#include <DynamixelBus.h>
+#include <AX12.h>
+#include <Terminal.h>
+#include <vector>
+using namespace std;
 
-DigitalOut gpo(D0);
-DigitalOut led(LED_RED);
+DigitalOut myled(LED_GREEN);
+Terminal pc(USBTX, USBRX);
+DynamixelBus bus( PTC17, PTC16, D7, D6, 1000000 );
+vector<AX12> servos;
+
 
 int main()
 {
-    while (true) {
-        gpo = !gpo; // toggle pin
-        led = !led; // toggle led
-        wait(0.2f);
+    pc.baud(115200); 
+
+    pc.cls();
+    pc.foreground(Yellow);
+    pc.background(Black);
+
+    pc.locate( 0, 0 );
+    pc.printf("**********************\r\n");
+    pc.printf("RobotArmDemo start\r\n");
+    pc.printf("**********************\r\n");
+
+    pc.foreground(Teal);
+    pc.background(Black);
+
+    pc.locate( 0 , 5 );
+    for( int n = 1; n <= 254; n++)
+    {
+        if( bus.Ping(n) & statusValid )
+        {
+            pc.printf("Found servo ID = %d\r\n", n);
+            AX12 servo( &bus, n );
+            servos.push_back( servo );
+            servo.TorqueEnable(true);
+        } 
+    }
+
+    // time delay is to allow the position encoders to come online after initial power supply event
+    wait(5);
+
+    // get home positions of servos
+    pc.foreground(Purple);
+    pc.background(Black);
+
+    vector<float> homePositions;
+    homePositions.clear();
+    pc.locate( 0 , 12 );
+    for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++)
+    {
+        float homePosition = servos[servoIndex].GetPosition();
+        pc.printf( "home[%d] = %f\r\n", servoIndex, homePosition); 
+        homePositions.push_back( homePosition );
+    }
+    
+    while( true )
+    {
+        // determine positions to extend arm into middle range
+        pc.foreground(Green);
+        pc.background(Black);
+        pc.locate( 0 , 18 );
+
+        int steps = 100;
+        vector<float> differentials;
+        for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++)
+        {
+            float difference = 150.0f - homePositions[servoIndex];
+            pc.printf( "diff[%d] = %f\r\n", servoIndex, difference); 
+            differentials.push_back( difference );
+        }
+    
+        // slowly move arm our of home position
+        for( int step = 0; step < steps; step++)
+        {
+            pc.foreground(Red);
+            pc.background(Black);
+            pc.locate( 0 , 24 );
+            
+            for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++ )
+            {
+                float goal = homePositions[servoIndex] + (differentials[servoIndex] * ((float)step / (float)steps));
+                pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); 
+                servos[servoIndex].SetGoal( goal );
+            }        
+    
+            wait(.05);
+        }
+        
+        // pause for 10 seconds
+        wait(10);
+        
+        
+        // slowly move all servos back to home position
+        for( int step = steps; step > 0; step--)
+        {
+            pc.foreground(Red);
+            pc.background(Black);
+            pc.locate( 0 , 24 );
+
+            for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++ )
+            {
+                float goal = homePositions[servoIndex] + (differentials[servoIndex] * ((float)step / (float)steps));
+                pc.printf( "goal[%d] = %f\r\n", servoIndex, goal); 
+                servos[servoIndex].SetGoal( goal );
+            }        
+    
+            wait(.05);
+        }
+        
+        // pause for 10 seconds
+        wait(10);
     }
 }
\ No newline at end of file