![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Very simple program to demonstrate MX-28 servo; turns from -90 to +90 degrees and back.
Fork of PR_RobotArmSetup by
Revision 1:311dbbc3aaa3, committed 2017-02-16
- Comitter:
- jah128
- Date:
- Thu Feb 16 21:21:09 2017 +0000
- Parent:
- 0:03f84c95f73b
- Commit message:
- Initial commit
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 03f84c95f73b -r 311dbbc3aaa3 main.cpp --- a/main.cpp Thu Feb 16 20:45:33 2017 +0000 +++ b/main.cpp Thu Feb 16 21:21:09 2017 +0000 @@ -1,100 +1,33 @@ -// Dynamixel Servo Setup Code -// -// Modify the commented lines in main() to setup required servos +// Dynamixel MX-28 Demo Code #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"); -} + pc.printf("Dynamixel MX-28 Servo Demo\n"); + int model = servo.GetModelNumber(1); + if(model != MX28_MODEL) { + pc.printf("Error: No MX-28 servo with ID 1 found...\n"); + while(1); + } + while(1){ + pc.printf("Set servo position to 1024"); + servo.SetGoal(1,1024,1); + wait(1); + pc.printf("Set servo position to 2048"); + servo.SetGoal(1,2048,1); + wait(1); + pc.printf("Set servo position to 3072"); + servo.SetGoal(1,3072,1); + wait(1); + pc.printf("Set servo position to 2048"); + servo.SetGoal(1,2048,1); + wait(1); + } +} \ No newline at end of file