/** initialize()
Sets up the UARTS, configures pin directions, says hello, then blinks at you.
*/
void initialize(void)
{
//init the UART0 to the Computer
uart_init();
stdout = &uart_output;
stdin = &uart_input;
//Initialize the AX12 UART1
ax12Init(1000000);
//Initialize Pin Directions and States for I/O
DDRB |= (DEBUG_LED_NUM);
DEBUG_LED_OFF();
//We're live. Say Hello
fprintf(stdout,".h\n");
//Blink The Board LED and all Dynamixel LEDs to show We're good to go
for (int i = 0; i<NUM_MOTORS; i++) ax12SetRegister(i, AX_LED, 0);
DEBUG_LED_OFF();
_delay_ms(500);
for (int i = 0; i<NUM_MOTORS; i++) ax12SetRegister(i, AX_LED, 1);
DEBUG_LED_ON();
_delay_ms(500);
for (int i = 0; i<NUM_MOTORS; i++) ax12SetRegister(i, AX_LED, 0);
DEBUG_LED_OFF();
}
// Set the PID gains of an MX servo.
//
// Parameters:
// id: The ID of the servo to setup.
// p_gain: The proportional gain [0; 31,75] (default for dynamixels is 4)
// i_gain: The integral gain [0; 124,02] (default for dynamixels is 0)
// d_gain: The diferential gain [0; 1.016] (default for dynamixels is 0)
void SetPidGains(int id, float p_gain, float i_gain, float d_gain)
{
char p = (char)(p_gain * 8);
ax12SetRegister(id, MX_P_GAIN, p);
char i = (char)(i_gain * 2.048);
ax12SetRegister(id, MX_I_GAIN, i);
char d = (char)(d_gain * 250);
ax12SetRegister(id, MX_D_GAIN, d);
}
//--------------------------------------------------------------------
// Cleanup
//--------------------------------------------------------------------
void ServoDriver::Cleanup(void) {
// Do any cleanup that the driver may need.
printf("ServoDriver::Cleanup\n\r");
#ifdef USB2AX_REG_VOLTAGE
ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, 0); // Turn off the voltage testing...
#endif
// Turn off all of the servo LEDS... Maybe use broadcast?
for (int iServo=0; iServo < NUMSERVOS; iServo++) {
dxl_write_byte((cPinTable[iServo]), AX_LED, 0);
dxl_get_result(); // don't care for now
}
bioloid.end(); // tell the driver to abort
}
//--------------------------------------------------------------------
//Init
//--------------------------------------------------------------------
void ServoDriver::Init(void) {
// First lets get the actual servo positions for all of our servos...
g_fServosFree = true;
bioloid.poseSize(NUMSERVOS); // Method in this version
bioloid.readPose();
ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, cPinTable[FIRSTFEMURPIN]);
g_fAXSpeedControl = false;
#ifdef OPT_GPPLAYER
_fGPEnabled = true; // assume we support it.
#endif
// Currently have Turret pins not necessarily same as numerical order so
// Maybe should do for all pins and then set the positions by index instead
// of having it do a simple search on each pin...
#ifdef cTurretRotPin
bioloid.setId(FIRSTTURRETPIN, cTurretRotPin);
bioloid.setId(FIRSTTURRETPIN+1, cTurretTiltPin);
#endif
}
//--------------------------------------------------------------------
//Init
//--------------------------------------------------------------------
void ServoDriver::Init(void) {
// First lets get the actual servo positions for all of our servos...
g_fServosFree = true;
// We will duplicate and expand on the functionality of the bioloid readPose,
// function. In our loop we will set the IDs to that of our table, so they
// will be in the same order. Plus we will verify we have all of the servos
// If we care configured such that none of the servos in our loop have id #1 and
// we find that servo #1 and only one other is not found, we will assume that
// servo did the renumber to 1 problem and we will renumber it to the missing one.
// But again only if 1 servo is missing.
// Note: We are not saving the read positions, but the make sure servos are on will...
bioloid.poseSize(NUMSERVOS); // Method in this version
uint16_t w;
int count_missing = 0;
int missing_servo = -1;
bool servo_1_in_table = false;
for (int i = 0; i < NUMSERVOS; i++) {
// Set the id
bioloid.setId(i, cPinTable[i]);
if (cPinTable[i] == 1)
servo_1_in_table = true;
// Now try to get it's position
w = ax12GetRegister(cPinTable[i], AX_PRESENT_POSITION_L, 2);
if (w == 0xffff) {
// Try a second time to make sure.
delay(25);
w = ax12GetRegister(cPinTable[i], AX_PRESENT_POSITION_L, 2);
if (w == 0xffff) {
// We have a failure
printf("Servo(%d): %d not found", i, cPinTable[i]);
if (++count_missing == 1)
missing_servo = cPinTable[i];
}
}
delay(25);
}
// Now see if we should try to recover from a potential servo that renumbered itself back to 1.
if (count_missing)
printf("ERROR: Servo driver init: %d servos missing\n", count_missing);
if ((count_missing == 1) && !servo_1_in_table) {
if (dxl_read_word(1, AX_PRESENT_POSITION_L) != 0xffff) {
printf("Servo recovery: Servo 1 found - setting id to %d\n", missing_servo);
dxl_write_byte(1, AX_ID, missing_servo);
}
}
#ifdef USB2AX_REG_VOLTAGE
ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, cPinTable[FIRSTFEMURPIN]);
#endif
g_fAXSpeedControl = false;
#ifdef OPT_GPPLAYER
_fGPEnabled = true; // assume we support it.
#endif
}
//--------------------------------------------------------------------
// Cleanup
//--------------------------------------------------------------------
void ServoDriver::Cleanup(void) {
// Do any cleanup that the driver may need.
printf("ServoDriver::Cleanup\n\r");
ax12SetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, 0); // Turn off the voltage testing...
}
