Howdy,
I'm new to this community and I have a question for you. Sorry if this is in the wrong section also.
My goal is to control 6 servos connected to the same serial port at the same time with 6 potentiometers. I have made a program that simply controls motion of an AX-12A servo through serial port 3 of my mega with a potentiometer. I have tested the program with up to 3 servos connected in a chain to serial 3 and 3 separate potentiometers and it works fine. When I go passed 3 the program starts to fail. I am using the Tx,Rx trickery method to communicate with the servos and i'm using the RegWrite command in addition to the Action command to make the servos move. I may have configured my code wrong or I'm misunderstanding how to control the motors.
A couple side notes:
I am still using the 1000000 baud rate because I changed one of the servo's rates to a lower rate and changed the rate in my code and I could no longer communicate with that servo until i changed the rate back.
My code is not pretty:
Code:// instruction table for Dynamixel AX-12A //--------------------------------------- #define ping_ins 1 // checked #define read_ins 2 // checked #define write_ins 3 // Checked #define reg_write_ins 4 // Checked #define action_ins 5 // Checked #define reset_ins 6 // Checked #define sync_write_ins 131 //--------------------------------------- //registers table for Dynamixel AX-12A //--------------------------------------- #define servo_id 3 #define baud_rate 4 #define max_torque 14 #define alarm_led 17 #define alarm_shutdown 18 #define torque_enable 24 #define goal_position 30 #define moving_speed 32 //--------------------------------------- // Code Begins... int length,location,temp,checksum,N,L; // used for status packet & return packet int old_val = 0; int old_val2 = 0; int old_val3 = 0; int old_val4 = 0; int old_val5 = 0; int old_val6 = 0; int present_val = 0; int present_val2 = 0; int present_val3 = 0; int present_val4 = 0; int present_val5 = 0; int present_val6 = 0; int id; int analogPin = A0; // 5 Potenimeters. each 1 controls 1 motor int analogPin2 = A1; int analogPin3 = A2; int analogPin4 = A3; int analogPin5 = A4; int analogPin6 = A5; int value; void setup() { Serial3.begin(1000000); // Servo Communication Speed Serial.begin(9600); // Communication speed Arduino/PC } //------------------------------------ void loop() { location = goal_position; present_val = analogRead(analogPin); // read value of A0 present_val2 = analogRead(analogPin2); present_val3 = analogRead(analogPin3); present_val4 = analogRead(analogPin4); present_val5 = analogRead(analogPin5); present_val6 = analogRead(analogPin6); if ((present_val != old_val)||(present_val2 != old_val2)|| (present_val3 != old_val3)||(present_val4 != old_val4)|| (present_val5 != old_val5)||(present_val6 != old_val6)) // { old_val = present_val; // old_val to present_val old_val2 = present_val2; old_val3 = present_val3; old_val4 = present_val4; old_val5 = present_val5; old_val6 = present_val6; transmit(); //enable trasmission // Reg_Write Instructions //------------------------------------ reg_write_2_byte(1,location,old_val);// put location value into buffer reg_write_2_byte(2,location,old_val2); reg_write_2_byte(3,location,old_val3); reg_write_2_byte(4,location,old_val4); reg_write_2_byte(5,location,old_val5); reg_write_2_byte(6,location,old_val6); Action(0xFE); //execute buffer recieve(); //enable recieving } } void transmit() { bitSet(UCSR3B,3); // Sets Tx pin bitClear(UCSR3B,4); // Clear Rx pin bitClear(UCSR3B,7); // Disable Rx Interrupt } void recieve() { bitClear(UCSR3B,3); // Clear Tx pin bitSet(UCSR3B,4); // Set Rx Pin bitSet(UCSR3B,7); // Allows Rx Interrupt } //Recieve Interrupt Subroutine void serialEvent3() { temp =Serial3.read(); Serial.print(temp,HEX); // prints incoming return packet bit } /*--------------------------------------- Function to reg_write to a 2 byte register location = what register to write to val = what value to write to register ---------------------------------------*/ void reg_write_2_byte(int id, int location,int val) { length = 5; // length of 2-byte instruction is 5 checksum = ~((id + length + reg_write_ins + location + (val&0xFF) + ((val&0xFF00) >> 8))%6); //Checksum Value Serial3.write(0xFF); // Starts instruction packet Serial3.write(0xFF); Serial3.write(id); Serial3.write(length); Serial3.write(reg_write_ins); Serial3.write(location); Serial3.write(val&0xFF); // Lower Byte Serial3.write((val&0xFF00) >> 8); // Upper Byte Serial3.write(checksum); } /*--------------------------------------- Action Function to perform Reg_Write Functions ---------------------------------------*/ void Action(int id) { length = 2; checksum = ~((id + length + action_ins)%6); Serial3.write(0xFF); Serial3.write(0xFF); Serial3.write(id); // Broadcast(0xFE) is used when sending action to two Dynamixels Serial3.write(length); Serial3.write(action_ins); Serial3.write(checksum); }
Reply With Quote
Bookmarks