James Hilder / Mbed 2 deprecated 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.

Dependencies:   mbed

main.cpp@1:21a24da53b7f, 2018-02-23 (annotated)

Committer:
jah128
Date:
Fri Feb 23 13:25:08 2018 +0000
Revision:
1:21a24da53b7f
Parent:
0:03f84c95f73b 
uopdated;

Who changed what in which revision?

UserRevisionLine numberNew 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 1:21a24da53b7f 17 void scan_for_servos(int baud_rate, int delay)
jah128 0:03f84c95f73b 18 {
jah128 1:21a24da53b7f 19 pc.printf("Scanning for servos at %d baud:\n",baud_rate);
jah128 1:21a24da53b7f 20 servo.SetInitBaud(baud_rate, delay);
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 1:21a24da53b7f 26 wait(1);
jah128 0:03f84c95f73b 27 }
jah128 0:03f84c95f73b 28
jah128 1:21a24da53b7f 29 void initialise_mx28_servo(int current_address, int target_servo, int init_baud, int init_delay)
jah128 0:03f84c95f73b 30 {
jah128 0:03f84c95f73b 31 pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo);
jah128 0:03f84c95f73b 32
jah128 0:03f84c95f73b 33 // MX-28s default to a baud rate of 57600 with delay time of 500us
jah128 1:21a24da53b7f 34 servo.SetInitBaud(init_baud,init_delay);
jah128 1:21a24da53b7f 35
jah128 0:03f84c95f73b 36 int model = servo.GetModelNumber(current_address);
jah128 0:03f84c95f73b 37 if(model != MX28_MODEL) {
jah128 0:03f84c95f73b 38 pc.printf("Error: No MX-28 servo with ID %d found...\n",current_address);
jah128 0:03f84c95f73b 39 while(1);
jah128 0:03f84c95f73b 40 }
jah128 0:03f84c95f73b 41 pc.printf("\n\nSetting up MX-28 servo to ID %d\n",target_servo);
jah128 0:03f84c95f73b 42 servo.SetID(current_address,target_servo);
jah128 0:03f84c95f73b 43 wait(0.25);
jah128 1:21a24da53b7f 44 if(init_baud != 57600){
jah128 1:21a24da53b7f 45 pc.printf("Setting return delay time for servo to 500us\n");
jah128 1:21a24da53b7f 46 servo.SetDelayTime(target_servo,250);
jah128 1:21a24da53b7f 47 wait(0.25);
jah128 1:21a24da53b7f 48 pc.printf("Setting baud rate for servo to 57600\n");
jah128 1:21a24da53b7f 49 servo.SetBaud(target_servo,0x22);
jah128 1:21a24da53b7f 50 wait(0.25);
jah128 1:21a24da53b7f 51 pc.printf("Resetting connecting baud rate\n");
jah128 1:21a24da53b7f 52 servo.SetInitBaud(57600, 250);
jah128 1:21a24da53b7f 53 }
jah128 0:03f84c95f73b 54 pc.printf("Getting servo data:\n");
jah128 0:03f84c95f73b 55 servo.DebugData(target_servo);
jah128 0:03f84c95f73b 56 pc.printf("\n\nDone (check messages above to verify status)\n");
jah128 0:03f84c95f73b 57 while(1);
jah128 0:03f84c95f73b 58 }
jah128 0:03f84c95f73b 59
jah128 1:21a24da53b7f 60 void initialise_ax12_servo(int current_address, int target_servo, int init_baud, int init_delay)
jah128 0:03f84c95f73b 61 {
jah128 0:03f84c95f73b 62 pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo);
jah128 0:03f84c95f73b 63
jah128 0:03f84c95f73b 64 // AX-12s default to a baud rate of 1M with delay time of 100us
jah128 1:21a24da53b7f 65 servo.SetInitBaud(init_baud,init_delay);
jah128 0:03f84c95f73b 66 int model = servo.GetModelNumber(current_address);
jah128 0:03f84c95f73b 67 if(model != AX12_MODEL) {
jah128 0:03f84c95f73b 68 pc.printf("Error: No AX12 servo with ID %d found...\n",current_address);
jah128 0:03f84c95f73b 69 while(1);
jah128 0:03f84c95f73b 70 }
jah128 0:03f84c95f73b 71 pc.printf("\n\nSetting up AX-12 servo to ID %d\n",target_servo);
jah128 0:03f84c95f73b 72 servo.SetID(current_address,target_servo);
jah128 0:03f84c95f73b 73 wait(0.25);
jah128 0:03f84c95f73b 74 pc.printf("Setting return delay time for servo to 500us\n");
jah128 0:03f84c95f73b 75 servo.SetDelayTime(target_servo,250);
jah128 0:03f84c95f73b 76 wait(0.25);
jah128 0:03f84c95f73b 77 pc.printf("Setting baud rate for servo to 57600\n");
jah128 0:03f84c95f73b 78 servo.SetBaud(target_servo,0x22);
jah128 0:03f84c95f73b 79 wait(0.25);
jah128 0:03f84c95f73b 80 pc.printf("Resetting connecting baud rate\n");
jah128 0:03f84c95f73b 81 servo.SetInitBaud(57600, 250);
jah128 0:03f84c95f73b 82 wait(0.25);
jah128 0:03f84c95f73b 83 pc.printf("Getting servo data:\n");
jah128 0:03f84c95f73b 84 servo.DebugData(target_servo);
jah128 0:03f84c95f73b 85 pc.printf("\n\nDone (check messages above to verify status)\n");
jah128 0:03f84c95f73b 86 while(1);
jah128 0:03f84c95f73b 87 }
jah128 0:03f84c95f73b 88 int main()
jah128 0:03f84c95f73b 89 {
jah128 0:03f84c95f73b 90 pc.baud(115200);
jah128 0:03f84c95f73b 91 pc.printf("Dynamixel Servo Setup\n");
jah128 0:03f84c95f73b 92
jah128 0:03f84c95f73b 93 // To scan for servos, uncomment the following line:
jah128 1:21a24da53b7f 94 //scan_for_servos(57600,250);
jah128 1:21a24da53b7f 95 //scan_for_servos(1000000,50);
jah128 0:03f84c95f73b 96 // 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 1:21a24da53b7f 97
jah128 1:21a24da53b7f 98 //This will initialise an mx-28 currently set to id 8 and set to baud 1mb, to id 10, 57600 baud
jah128 1:21a24da53b7f 99 initialise_mx28_servo(8,10,1000000,50);
jah128 0:03f84c95f73b 100
jah128 0:03f84c95f73b 101 // 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 1:21a24da53b7f 102 //initialise_ax12_servo(1,WRIST,1000000,50);
jah128 1:21a24da53b7f 103 pc.printf("Scan Complete...");
jah128 1:21a24da53b7f 104 //pc.printf("Edit the comments in main() to check and program servos.\n");
jah128 0:03f84c95f73b 105 }