James Hilder
/
PR_RobotArmSetup
Code to set up an AX-12 or MX-28 (and to scan for unidentified servos) ready for use in the robot arm.
main.cpp@0:03f84c95f73b, 2017-02-16 (annotated)
- Committer:
- jah128
- Date:
- Thu Feb 16 20:45:33 2017 +0000
- Revision:
- 0:03f84c95f73b
- Child:
- 1:21a24da53b7f
Initial commit
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jah128 | 0:03f84c95f73b | 1 | // Dynamixel Servo Setup Code |
jah128 | 0:03f84c95f73b | 2 | // |
jah128 | 0:03f84c95f73b | 3 | // Modify the commented lines in main() to setup required servos |
jah128 | 0:03f84c95f73b | 4 | |
jah128 | 0:03f84c95f73b | 5 | #include "servo.h" |
jah128 | 0:03f84c95f73b | 6 | #include "mbed.h" |
jah128 | 0:03f84c95f73b | 7 | #include "SerialHalfDuplex.h" |
jah128 | 0:03f84c95f73b | 8 | |
jah128 | 0:03f84c95f73b | 9 | #define BASE 10 |
jah128 | 0:03f84c95f73b | 10 | #define SHOULDER 11 |
jah128 | 0:03f84c95f73b | 11 | #define ELBOW 12 |
jah128 | 0:03f84c95f73b | 12 | #define WRIST 13 |
jah128 | 0:03f84c95f73b | 13 | |
jah128 | 0:03f84c95f73b | 14 | Serial pc(USBTX,USBRX); |
jah128 | 0:03f84c95f73b | 15 | Servo servo(p9,p10); |
jah128 | 0:03f84c95f73b | 16 | |
jah128 | 0:03f84c95f73b | 17 | void scan_for_servos() |
jah128 | 0:03f84c95f73b | 18 | { |
jah128 | 0:03f84c95f73b | 19 | pc.printf("Scanning for servos at 57600 baud:\n"); |
jah128 | 0:03f84c95f73b | 20 | servo.SetInitBaud(57600,250); |
jah128 | 0:03f84c95f73b | 21 | for(int i=0;i<254;i++){ |
jah128 | 0:03f84c95f73b | 22 | int model = servo.GetModelNumber(i); |
jah128 | 0:03f84c95f73b | 23 | if(model == AX12_MODEL) pc.printf("AX-12 found at ID %d",i); |
jah128 | 0:03f84c95f73b | 24 | if(model == MX28_MODEL) pc.printf("MX-28 found at ID %d",i); |
jah128 | 0:03f84c95f73b | 25 | } |
jah128 | 0:03f84c95f73b | 26 | wait(0.5); |
jah128 | 0:03f84c95f73b | 27 | pc.printf("\n\nScanning for servos at 1M baud:\n"); |
jah128 | 0:03f84c95f73b | 28 | servo.SetInitBaud(1000000,50); |
jah128 | 0:03f84c95f73b | 29 | for(int i=0;i<254;i++){ |
jah128 | 0:03f84c95f73b | 30 | int model = servo.GetModelNumber(i); |
jah128 | 0:03f84c95f73b | 31 | if(model == AX12_MODEL) pc.printf("AX-12 found at ID %d",i); |
jah128 | 0:03f84c95f73b | 32 | if(model == MX28_MODEL) pc.printf("MX-28 found at ID %d",i); |
jah128 | 0:03f84c95f73b | 33 | } |
jah128 | 0:03f84c95f73b | 34 | while(1); |
jah128 | 0:03f84c95f73b | 35 | } |
jah128 | 0:03f84c95f73b | 36 | |
jah128 | 0:03f84c95f73b | 37 | void initialise_mx28_servo(int current_address, int target_servo) |
jah128 | 0:03f84c95f73b | 38 | { |
jah128 | 0:03f84c95f73b | 39 | pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo); |
jah128 | 0:03f84c95f73b | 40 | |
jah128 | 0:03f84c95f73b | 41 | // MX-28s default to a baud rate of 57600 with delay time of 500us |
jah128 | 0:03f84c95f73b | 42 | servo.SetInitBaud(57600, 250); |
jah128 | 0:03f84c95f73b | 43 | int model = servo.GetModelNumber(current_address); |
jah128 | 0:03f84c95f73b | 44 | if(model != MX28_MODEL) { |
jah128 | 0:03f84c95f73b | 45 | pc.printf("Error: No MX-28 servo with ID %d found...\n",current_address); |
jah128 | 0:03f84c95f73b | 46 | while(1); |
jah128 | 0:03f84c95f73b | 47 | } |
jah128 | 0:03f84c95f73b | 48 | pc.printf("\n\nSetting up MX-28 servo to ID %d\n",target_servo); |
jah128 | 0:03f84c95f73b | 49 | servo.SetID(current_address,target_servo); |
jah128 | 0:03f84c95f73b | 50 | wait(0.25); |
jah128 | 0:03f84c95f73b | 51 | pc.printf("Getting servo data:\n"); |
jah128 | 0:03f84c95f73b | 52 | servo.DebugData(target_servo); |
jah128 | 0:03f84c95f73b | 53 | pc.printf("\n\nDone (check messages above to verify status)\n"); |
jah128 | 0:03f84c95f73b | 54 | while(1); |
jah128 | 0:03f84c95f73b | 55 | } |
jah128 | 0:03f84c95f73b | 56 | |
jah128 | 0:03f84c95f73b | 57 | void initialise_ax12_servo(int current_address, int target_servo) |
jah128 | 0:03f84c95f73b | 58 | { |
jah128 | 0:03f84c95f73b | 59 | pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo); |
jah128 | 0:03f84c95f73b | 60 | |
jah128 | 0:03f84c95f73b | 61 | // AX-12s default to a baud rate of 1M with delay time of 100us |
jah128 | 0:03f84c95f73b | 62 | servo.SetInitBaud(1000000, 50); |
jah128 | 0:03f84c95f73b | 63 | int model = servo.GetModelNumber(current_address); |
jah128 | 0:03f84c95f73b | 64 | if(model != AX12_MODEL) { |
jah128 | 0:03f84c95f73b | 65 | pc.printf("Error: No AX12 servo with ID %d found...\n",current_address); |
jah128 | 0:03f84c95f73b | 66 | while(1); |
jah128 | 0:03f84c95f73b | 67 | } |
jah128 | 0:03f84c95f73b | 68 | pc.printf("\n\nSetting up AX-12 servo to ID %d\n",target_servo); |
jah128 | 0:03f84c95f73b | 69 | servo.SetID(current_address,target_servo); |
jah128 | 0:03f84c95f73b | 70 | wait(0.25); |
jah128 | 0:03f84c95f73b | 71 | pc.printf("Setting return delay time for servo to 500us\n"); |
jah128 | 0:03f84c95f73b | 72 | servo.SetDelayTime(target_servo,250); |
jah128 | 0:03f84c95f73b | 73 | wait(0.25); |
jah128 | 0:03f84c95f73b | 74 | pc.printf("Setting baud rate for servo to 57600\n"); |
jah128 | 0:03f84c95f73b | 75 | servo.SetBaud(target_servo,0x22); |
jah128 | 0:03f84c95f73b | 76 | wait(0.25); |
jah128 | 0:03f84c95f73b | 77 | pc.printf("Resetting connecting baud rate\n"); |
jah128 | 0:03f84c95f73b | 78 | servo.SetInitBaud(57600, 250); |
jah128 | 0:03f84c95f73b | 79 | wait(0.25); |
jah128 | 0:03f84c95f73b | 80 | pc.printf("Getting servo data:\n"); |
jah128 | 0:03f84c95f73b | 81 | servo.DebugData(target_servo); |
jah128 | 0:03f84c95f73b | 82 | pc.printf("\n\nDone (check messages above to verify status)\n"); |
jah128 | 0:03f84c95f73b | 83 | while(1); |
jah128 | 0:03f84c95f73b | 84 | } |
jah128 | 0:03f84c95f73b | 85 | int main() |
jah128 | 0:03f84c95f73b | 86 | { |
jah128 | 0:03f84c95f73b | 87 | pc.baud(115200); |
jah128 | 0:03f84c95f73b | 88 | pc.printf("Dynamixel Servo Setup\n"); |
jah128 | 0:03f84c95f73b | 89 | |
jah128 | 0:03f84c95f73b | 90 | // To scan for servos, uncomment the following line: |
jah128 | 0:03f84c95f73b | 91 | //scan_for_servos(); |
jah128 | 0:03f84c95f73b | 92 | |
jah128 | 0:03f84c95f73b | 93 | // To setup a new MX-28 servo, uncomment the following line and replace first number with current ID and second number with target ID: |
jah128 | 0:03f84c95f73b | 94 | //initialise_mx28_servo(1,BASE); |
jah128 | 0:03f84c95f73b | 95 | |
jah128 | 0:03f84c95f73b | 96 | // To setup a new AX-12 servo, uncomment the following line and replace first number with current ID and second number with target ID: |
jah128 | 0:03f84c95f73b | 97 | //initialise_ax12_servo(1,WRIST); |
jah128 | 0:03f84c95f73b | 98 | |
jah128 | 0:03f84c95f73b | 99 | pc.printf("Edit the comments in main() to check and program servos.\n"); |
jah128 | 0:03f84c95f73b | 100 | } |