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?
User | Revision | Line number | New 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 | } |