Revision 1:21a24da53b7f, committed 2018-02-23

Comitter:

jah128

Date:

Fri Feb 23 13:25:08 2018 +0000

Parent:

0:03f84c95f73b

Commit message:

uopdated;

Changed in this revision

diff -r 03f84c95f73b -r 21a24da53b7f main.cpp
--- a/main.cpp	Thu Feb 16 20:45:33 2017 +0000
+++ b/main.cpp	Fri Feb 23 13:25:08 2018 +0000
@@ -14,32 +14,25 @@
 Serial pc(USBTX,USBRX);
 Servo servo(p9,p10);
 
-void scan_for_servos()
+void scan_for_servos(int baud_rate, int delay)
 {
-    pc.printf("Scanning for servos at 57600 baud:\n");
-    servo.SetInitBaud(57600,250);
+    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(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);
+    wait(1);
 }
 
-void initialise_mx28_servo(int current_address, int target_servo)
+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(57600, 250);
+    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);
@@ -48,18 +41,28 @@
     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)
+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(1000000, 50);
+    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);
@@ -88,13 +91,15 @@
     pc.printf("Dynamixel Servo Setup\n");
     
     // To scan for servos, uncomment the following line:
-    //scan_for_servos();
-   
+    //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:
-    //initialise_mx28_servo(1,BASE);
+    
+    //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); 
-    
-    pc.printf("Edit the comments in main() to check and program servos.\n");
+    //initialise_ax12_servo(1,WRIST,1000000,50); 
+    pc.printf("Scan Complete...");
+    //pc.printf("Edit the comments in main() to check and program servos.\n");
 }