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
- Committer:
- jah128
- Date:
- 2018-02-23
- Revision:
- 1:21a24da53b7f
- Parent:
- 0:03f84c95f73b
File content as of revision 1:21a24da53b7f:
// 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(int baud_rate, int delay) { pc.printf("Scanning for servos at %d baud:\n",baud_rate); servo.SetInitBaud(baud_rate, delay); 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(1); } void initialise_mx28_servo(int current_address, int target_servo, int init_baud, int init_delay) { 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(init_baud,init_delay); 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); if(init_baud != 57600){ 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); } 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, int init_baud, int init_delay) { 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(init_baud,init_delay); 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(57600,250); //scan_for_servos(1000000,50); // To setup a new MX-28 servo, uncomment the following line and replace first number with current ID and second number with target ID: //This will initialise an mx-28 currently set to id 8 and set to baud 1mb, to id 10, 57600 baud initialise_mx28_servo(8,10,1000000,50); // 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,1000000,50); pc.printf("Scan Complete..."); //pc.printf("Edit the comments in main() to check and program servos.\n"); }