robot arm demo team / Mbed 2 deprecated RobotArmDemo Featured

demo project

Dependencies:   AX-12A Dynamixel mbed iothub_client EthernetInterface NTPClient ConfigFile SDFileSystem iothub_amqp_transport mbed-rtos proton-c-mbed wolfSSL

main.cpp@2:37021fb3b45b, 2015-12-15 (annotated)

Committer:
jepickett
Date:
Tue Dec 15 04:38:56 2015 +0000
Revision:
2:37021fb3b45b
Parent:
0:547ac906c46b
Child:
4:36a4eceb1b7f
arm from home and back

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jepickett 0:547ac906c46b 1 #include "mbed.h"
jepickett 2:37021fb3b45b 2 #include <DynamixelBus.h>
jepickett 2:37021fb3b45b 3 #include <AX12.h>
jepickett 2:37021fb3b45b 4 #include <Terminal.h>
jepickett 2:37021fb3b45b 5 #include <vector>
jepickett 2:37021fb3b45b 6 using namespace std;
jepickett 0:547ac906c46b 7
jepickett 2:37021fb3b45b 8 DigitalOut myled(LED_GREEN);
jepickett 2:37021fb3b45b 9 Terminal pc(USBTX, USBRX);
jepickett 2:37021fb3b45b 10 DynamixelBus bus( PTC17, PTC16, D7, D6, 1000000 );
jepickett 2:37021fb3b45b 11 vector<AX12> servos;
jepickett 2:37021fb3b45b 12
jepickett 0:547ac906c46b 13
jepickett 0:547ac906c46b 14 int main()
jepickett 0:547ac906c46b 15 {
jepickett 2:37021fb3b45b 16 pc.baud(115200);
jepickett 2:37021fb3b45b 17
jepickett 2:37021fb3b45b 18 pc.cls();
jepickett 2:37021fb3b45b 19 pc.foreground(Yellow);
jepickett 2:37021fb3b45b 20 pc.background(Black);
jepickett 2:37021fb3b45b 21
jepickett 2:37021fb3b45b 22 pc.locate( 0, 0 );
jepickett 2:37021fb3b45b 23 pc.printf("**********************\r\n");
jepickett 2:37021fb3b45b 24 pc.printf("RobotArmDemo start\r\n");
jepickett 2:37021fb3b45b 25 pc.printf("**********************\r\n");
jepickett 2:37021fb3b45b 26
jepickett 2:37021fb3b45b 27 pc.foreground(Teal);
jepickett 2:37021fb3b45b 28 pc.background(Black);
jepickett 2:37021fb3b45b 29
jepickett 2:37021fb3b45b 30 pc.locate( 0 , 5 );
jepickett 2:37021fb3b45b 31 for( int n = 1; n <= 254; n++)
jepickett 2:37021fb3b45b 32 {
jepickett 2:37021fb3b45b 33 if( bus.Ping(n) & statusValid )
jepickett 2:37021fb3b45b 34 {
jepickett 2:37021fb3b45b 35 pc.printf("Found servo ID = %d\r\n", n);
jepickett 2:37021fb3b45b 36 AX12 servo( &bus, n );
jepickett 2:37021fb3b45b 37 servos.push_back( servo );
jepickett 2:37021fb3b45b 38 servo.TorqueEnable(true);
jepickett 2:37021fb3b45b 39 }
jepickett 2:37021fb3b45b 40 }
jepickett 2:37021fb3b45b 41
jepickett 2:37021fb3b45b 42 // time delay is to allow the position encoders to come online after initial power supply event
jepickett 2:37021fb3b45b 43 wait(5);
jepickett 2:37021fb3b45b 44
jepickett 2:37021fb3b45b 45 // get home positions of servos
jepickett 2:37021fb3b45b 46 pc.foreground(Purple);
jepickett 2:37021fb3b45b 47 pc.background(Black);
jepickett 2:37021fb3b45b 48
jepickett 2:37021fb3b45b 49 vector<float> homePositions;
jepickett 2:37021fb3b45b 50 homePositions.clear();
jepickett 2:37021fb3b45b 51 pc.locate( 0 , 12 );
jepickett 2:37021fb3b45b 52 for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++)
jepickett 2:37021fb3b45b 53 {
jepickett 2:37021fb3b45b 54 float homePosition = servos[servoIndex].GetPosition();
jepickett 2:37021fb3b45b 55 pc.printf( "home[%d] = %f\r\n", servoIndex, homePosition);
jepickett 2:37021fb3b45b 56 homePositions.push_back( homePosition );
jepickett 2:37021fb3b45b 57 }
jepickett 2:37021fb3b45b 58
jepickett 2:37021fb3b45b 59 while( true )
jepickett 2:37021fb3b45b 60 {
jepickett 2:37021fb3b45b 61 // determine positions to extend arm into middle range
jepickett 2:37021fb3b45b 62 pc.foreground(Green);
jepickett 2:37021fb3b45b 63 pc.background(Black);
jepickett 2:37021fb3b45b 64 pc.locate( 0 , 18 );
jepickett 2:37021fb3b45b 65
jepickett 2:37021fb3b45b 66 int steps = 100;
jepickett 2:37021fb3b45b 67 vector<float> differentials;
jepickett 2:37021fb3b45b 68 for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++)
jepickett 2:37021fb3b45b 69 {
jepickett 2:37021fb3b45b 70 float difference = 150.0f - homePositions[servoIndex];
jepickett 2:37021fb3b45b 71 pc.printf( "diff[%d] = %f\r\n", servoIndex, difference);
jepickett 2:37021fb3b45b 72 differentials.push_back( difference );
jepickett 2:37021fb3b45b 73 }
jepickett 2:37021fb3b45b 74
jepickett 2:37021fb3b45b 75 // slowly move arm our of home position
jepickett 2:37021fb3b45b 76 for( int step = 0; step < steps; step++)
jepickett 2:37021fb3b45b 77 {
jepickett 2:37021fb3b45b 78 pc.foreground(Red);
jepickett 2:37021fb3b45b 79 pc.background(Black);
jepickett 2:37021fb3b45b 80 pc.locate( 0 , 24 );
jepickett 2:37021fb3b45b 81
jepickett 2:37021fb3b45b 82 for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++ )
jepickett 2:37021fb3b45b 83 {
jepickett 2:37021fb3b45b 84 float goal = homePositions[servoIndex] + (differentials[servoIndex] * ((float)step / (float)steps));
jepickett 2:37021fb3b45b 85 pc.printf( "goal[%d] = %f\r\n", servoIndex, goal);
jepickett 2:37021fb3b45b 86 servos[servoIndex].SetGoal( goal );
jepickett 2:37021fb3b45b 87 }
jepickett 2:37021fb3b45b 88
jepickett 2:37021fb3b45b 89 wait(.05);
jepickett 2:37021fb3b45b 90 }
jepickett 2:37021fb3b45b 91
jepickett 2:37021fb3b45b 92 // pause for 10 seconds
jepickett 2:37021fb3b45b 93 wait(10);
jepickett 2:37021fb3b45b 94
jepickett 2:37021fb3b45b 95
jepickett 2:37021fb3b45b 96 // slowly move all servos back to home position
jepickett 2:37021fb3b45b 97 for( int step = steps; step > 0; step--)
jepickett 2:37021fb3b45b 98 {
jepickett 2:37021fb3b45b 99 pc.foreground(Red);
jepickett 2:37021fb3b45b 100 pc.background(Black);
jepickett 2:37021fb3b45b 101 pc.locate( 0 , 24 );
jepickett 2:37021fb3b45b 102
jepickett 2:37021fb3b45b 103 for( int servoIndex = 0; servoIndex < servos.size(); servoIndex++ )
jepickett 2:37021fb3b45b 104 {
jepickett 2:37021fb3b45b 105 float goal = homePositions[servoIndex] + (differentials[servoIndex] * ((float)step / (float)steps));
jepickett 2:37021fb3b45b 106 pc.printf( "goal[%d] = %f\r\n", servoIndex, goal);
jepickett 2:37021fb3b45b 107 servos[servoIndex].SetGoal( goal );
jepickett 2:37021fb3b45b 108 }
jepickett 2:37021fb3b45b 109
jepickett 2:37021fb3b45b 110 wait(.05);
jepickett 2:37021fb3b45b 111 }
jepickett 2:37021fb3b45b 112
jepickett 2:37021fb3b45b 113 // pause for 10 seconds
jepickett 2:37021fb3b45b 114 wait(10);
jepickett 0:547ac906c46b 115 }
jepickett 0:547ac906c46b 116 }