word ServoDriver::GetBatteryVoltage(void) {
// The USB2AX is caching informatation for us, so simply ask it for the data. If this is still
// too much overhead, then we may want to only do this on timer...
// Lets cycle through the Tibia servos asking for voltages as they may be the ones doing the most work...
unsigned long uldt = millis() - g_ulTimeLastBatteryVoltage;
if ((uldt > VOLTAGE_MAX_TIME_BETWEEN_CALLS) || ((uldt > VOLTAGE_MIN_TIME_BETWEEN_CALLS && !bioloid.interpolating()))) {
word wVoltage = (word)ax12GetRegister(cPinTable[FIRSTFEMURPIN /*+ g_bLegVoltage++*/], AX_PRESENT_VOLTAGE, 1);
if (g_bLegVoltage == CNT_LEGS)
g_bLegVoltage = 0;
if (wVoltage && (wVoltage != 0xffff)) {
g_ulTimeLastBatteryVoltage = millis();
if (wVoltage != g_wLastVoltage) {
printf("Voltage: %d\n\r", wVoltage);
g_wLastVoltage = wVoltage;
}
} else if ((uldt > VOLTAGE_TIME_TO_ERROR) && (g_wLastVoltage != 0xffff)) {
printf("Voltage: error timeout");
g_wLastVoltage = 0xffff;
}
}
return ((g_wLastVoltage != (word)-1)? g_wLastVoltage*10 : (word)-1);
}
//==============================================================================
// ProcessTerminalCommand: The terminal monitor will call this to see if the
// command the user entered was one added by the servo driver.
//==============================================================================
boolean ServoDriver::ProcessTerminalCommand(byte *psz, byte bLen)
{
if ((bLen == 1) && ((*psz == 'm') || (*psz == 'M'))) {
g_fEnableServos = !g_fEnableServos;
if (g_fEnableServos)
DBGSerial.println(F("Motors are on"));
else
DBGSerial.println(F("Motors are off"));
return true;
}
if ((bLen == 1) && ((*psz == 't') || (*psz == 'T'))) {
// Test to see if all servos are responding...
for(int i=1;i<=NUMSERVOS;i++){
word w;
w = ax12GetRegister(i,AX_PRESENT_POSITION_L, 2);
DBGSerial.print(i,DEC);
DBGSerial.print(F("="));
DBGSerial.println(w, DEC);
delay(25);
}
}
if ((bLen == 1) && ((*psz == 'a') || (*psz == 'A'))) {
g_fAXSpeedControl = !g_fAXSpeedControl;
if (g_fAXSpeedControl)
DBGSerial.println(F("AX12 Speed Control"));
else
DBGSerial.println(F("Bioloid Speed"));
}
if ((bLen >= 1) && ((*psz == 'f') || (*psz == 'F'))) {
psz++; // need to get beyond the first character
while (*psz == ' ')
psz++; // ignore leading blanks...
byte bFrame = 0;
while ((*psz >= '0') && (*psz <= '9')) { // Get the frame count...
bFrame = bFrame*10 + *psz++ - '0';
}
if (bFrame != 0) {
DBGSerial.print(F("New Servo Cycles per second: "));
DBGSerial.println(1000/bFrame, DEC);
extern BioloidControllerEx bioloid;
bioloid.frameLength = bFrame;
}
}
#ifdef OPT_FIND_SERVO_OFFSETS
else if ((bLen == 1) && ((*psz == 'o') || (*psz == 'O'))) {
FindServoOffsets();
}
#endif
#ifdef OPT_PYPOSE
else if ((*psz == 'p') || (*psz == 'P')) {
DoPyPose(++psz);
}
#endif
return false;
}
// Get the present speed of an MX servo.
//
// Parameters:
// id: ID of the servo to query.
//
// Returns the speed in rpm. Negative is clockwise.
float GetSpeed(int id)
{
int v = ax12GetRegister(id, MX_PRESENT_SPEED_L, 2);
float velocity = 1;
if (v > 1023) { // clockwise
velocity = -1;
v -= 1024;
}
velocity *= v * 0.11443;
return velocity;
}
word ServoDriver::GetBatteryVoltage(void) {
// The USB2AX is caching informatation for us, so simply ask it for the data. If this is still
// too much overhead, then we may want to only do this on timer...
// Lets cycle through the Tibia servos asking for voltages as they may be the ones doing the most work...
word wVoltage = (word)ax12GetRegister(AX_ID_DEVICE, USB2AX_REG_VOLTAGE, 1);
if (wVoltage != g_ulTimeLastBatteryVoltage) {
printf("Voltage: %d\n\r", wVoltage);
g_ulTimeLastBatteryVoltage = wVoltage;
}
return ((wVoltage != (word)-1)? wVoltage*10 : (word)-1);
}
//--------------------------------------------------------------------
//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
}
// Get the current consumption of an MX servo.
//
// Parameters:
// id: ID of the servo to query.
//
// Returns the current in mA. The reading is precise to
// one half digit.
float GetCurrent(int id)
{
int i = ax12GetRegister(id, MX_CURRENT_L, 2);
return 4.5 * (i - 2048);
}
// Get the voltage supplied to an MX servo.
//
// Parameters:
// id: ID of the servo to query.
//
// Returns the voltage in V. The reading is precise to
// one decimal.
float GetVoltage(int id)
{
int v = ax12GetRegister(id, MX_PRESENT_VOLTAGE, 1);
return v / 10.0;
}
/** Main program entry point. This routine contains the overall program flow
*/
int main(void)
{
initialize(); //Configures Ports, sets default values, etc.
//Loop Forever
for (;;)
{
//we add data starting with an '.' to the buffer until we get the newline character.
fgets(input, sizeof(input), stdin); //Get the actual input (reads up to and including newline character)
cmd = input[1]; //command char is always the first char of the input data after the '.'
//Command = 'q' - Query for the current status
if (cmd == 'q')
{
memset(input, 0, sizeof(input)); //Clear previous input
fprintf(stdout, ".q%d,%d\n", FIRMWARE_VERSION, NUM_MOTORS); //ACK w/ Firmware Version, # of Motors
}
//Command = 'l' - Command to Control Debug LED
else if (cmd == 'l')
{
if (input[2] == '1') DEBUG_LED_ON(); //Turn LED On
else if (input[2] == '0') DEBUG_LED_OFF(); //Turn LED Off
memset(input, 0, sizeof(input)); //Clear previous input
fprintf(stdout, ".l%d\n", DEBUG_LED_STATE()); //ACK
}
//Command = 'v' - Sets all motor speeds
//Comma separated entries telling all motors to set certain speeds
//Assumes motor IDs are 0 <-> NUM_MOTORS-1
else if (cmd == 'v')
{
parse_serial(); //Read the input string to an array of values
memset(input, 0, sizeof(input)); //Clear previous input
//send those speed commands
for (int i = 0; i<NUM_MOTORS; i++)
{
ax12SetRegister2(i, AX_GOAL_SPEED_L, vals[i]);
}
_delay_ms(25);
//Only after we have commanded all the speeds, can we check the status
fprintf(stdout, ".v"); //ACK Character
//Send ACK Info
for (int i = 0; i<NUM_MOTORS; i++)
{
fprintf(stdout, "%d", ax12GetRegister(i, AX_GOAL_SPEED_L, 2)); //Return velocity setting
if (i<NUM_MOTORS-1) fprintf(stdout, ","); //Print delimiter
}
fprintf(stdout, "\n"); //ACK Newline
}
//Command = 'c' - Command all the motors to a new position
//Comma separated entries telling all motors to move to positions from 0-1023
//Assumes motor IDs are 0 <-> NUM_MOTORS-1
else if (cmd == 'c')
{
parse_serial(); //Read the input string to an array of positions
memset(input, 0, sizeof(input)); //Clear previous input
//send those position commands
for (int i = 0; i<NUM_MOTORS; i++)
{
ax12SetRegister2(i, AX_GOAL_POSITION_L, vals[i]);
}
//Only after we have commanded all the positions, can we check the status
fprintf(stdout, ".c"); //ACK Character
//Send ACK Info
for (int i = 0; i<NUM_MOTORS; i++)
{
while(ax12GetRegister(i,AX_MOVING,1)); //Wait for this motor to finish moving
fprintf(stdout, "%d", ax12GetRegister(i, AX_PRESENT_POSITION_L, 2)); //Return the present position
if (i<NUM_MOTORS-1) fprintf(stdout, ","); //Print delimiter
}
fprintf(stdout, "\n"); //ACK Newline
}
}
}
