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.
Diff: main.cpp
- Revision:
- 0:03f84c95f73b
- Child:
- 1:21a24da53b7f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Thu Feb 16 20:45:33 2017 +0000 @@ -0,0 +1,100 @@ +// Dynamixel Servo Setup Code +// +// Modify the commented lines in main() to setup required servos + +#include "servo.h" +#include "mbed.h" +#include "SerialHalfDuplex.h" + +#define BASE 10 +#define SHOULDER 11 +#define ELBOW 12 +#define WRIST 13 + +Serial pc(USBTX,USBRX); +Servo servo(p9,p10); + +void scan_for_servos() +{ + pc.printf("Scanning for servos at 57600 baud:\n"); + servo.SetInitBaud(57600,250); + for(int i=0;i<254;i++){ + int model = servo.GetModelNumber(i); + if(model == AX12_MODEL) pc.printf("AX-12 found at ID %d",i); + if(model == MX28_MODEL) pc.printf("MX-28 found at ID %d",i); + } + wait(0.5); + pc.printf("\n\nScanning for servos at 1M baud:\n"); + servo.SetInitBaud(1000000,50); + for(int i=0;i<254;i++){ + int model = servo.GetModelNumber(i); + if(model == AX12_MODEL) pc.printf("AX-12 found at ID %d",i); + if(model == MX28_MODEL) pc.printf("MX-28 found at ID %d",i); + } + while(1); +} + +void initialise_mx28_servo(int current_address, int target_servo) +{ + pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo); + + // MX-28s default to a baud rate of 57600 with delay time of 500us + servo.SetInitBaud(57600, 250); + int model = servo.GetModelNumber(current_address); + if(model != MX28_MODEL) { + pc.printf("Error: No MX-28 servo with ID %d found...\n",current_address); + while(1); + } + pc.printf("\n\nSetting up MX-28 servo to ID %d\n",target_servo); + servo.SetID(current_address,target_servo); + wait(0.25); + pc.printf("Getting servo data:\n"); + servo.DebugData(target_servo); + pc.printf("\n\nDone (check messages above to verify status)\n"); + while(1); +} + +void initialise_ax12_servo(int current_address, int target_servo) +{ + pc.printf("RUNNING SERVO SETUP ROUTINE FOR SERVO %d\n\n",target_servo); + + // AX-12s default to a baud rate of 1M with delay time of 100us + servo.SetInitBaud(1000000, 50); + int model = servo.GetModelNumber(current_address); + if(model != AX12_MODEL) { + pc.printf("Error: No AX12 servo with ID %d found...\n",current_address); + while(1); + } + pc.printf("\n\nSetting up AX-12 servo to ID %d\n",target_servo); + servo.SetID(current_address,target_servo); + wait(0.25); + pc.printf("Setting return delay time for servo to 500us\n"); + servo.SetDelayTime(target_servo,250); + wait(0.25); + pc.printf("Setting baud rate for servo to 57600\n"); + servo.SetBaud(target_servo,0x22); + wait(0.25); + pc.printf("Resetting connecting baud rate\n"); + servo.SetInitBaud(57600, 250); + wait(0.25); + pc.printf("Getting servo data:\n"); + servo.DebugData(target_servo); + pc.printf("\n\nDone (check messages above to verify status)\n"); + while(1); +} +int main() +{ + pc.baud(115200); + pc.printf("Dynamixel Servo Setup\n"); + + // To scan for servos, uncomment the following line: + //scan_for_servos(); + + // To setup a new MX-28 servo, uncomment the following line and replace first number with current ID and second number with target ID: + //initialise_mx28_servo(1,BASE); + + // To setup a new AX-12 servo, uncomment the following line and replace first number with current ID and second number with target ID: + //initialise_ax12_servo(1,WRIST); + + pc.printf("Edit the comments in main() to check and program servos.\n"); +}