/** 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();
}
/**
USB interrupt handler
@todo Get all 11 bits of frame number instead of just 8
Endpoint interrupts are mapped to the slow interrupt
*/
void USBHwISR(void)
{
U32 dwStatus;
U32 dwIntBit;
U8 bEPStat, bDevStat, bStat;
int i;
U16 wFrame;
// LED9 monitors total time in interrupt routine
DEBUG_LED_ON(9);
// handle device interrupts
dwStatus = USBDevIntSt;
// frame interrupt
if (dwStatus & FRAME) {
// clear int
USBDevIntClr = FRAME;
// call handler
if (_pfnFrameHandler != NULL) {
wFrame = USBHwCmdRead(CMD_DEV_READ_CUR_FRAME_NR);
_pfnFrameHandler(wFrame);
}
}
// device status interrupt
if (dwStatus & DEV_STAT) {
/* Clear DEV_STAT interrupt before reading DEV_STAT register.
This prevents corrupted device status reads, see
LPC2148 User manual revision 2, 25 july 2006.
*/
USBDevIntClr = DEV_STAT;
bDevStat = USBHwCmdRead(CMD_DEV_STATUS);
if (bDevStat & (CON_CH | SUS_CH | RST)) {
// convert device status into something HW independent
bStat = ((bDevStat & CON) ? DEV_STATUS_CONNECT : 0) |
((bDevStat & SUS) ? DEV_STATUS_SUSPEND : 0) |
((bDevStat & RST) ? DEV_STATUS_RESET : 0);
// call handler
if (_pfnDevIntHandler != NULL) {
DEBUG_LED_ON(8);
_pfnDevIntHandler(bStat);
DEBUG_LED_OFF(8);
}
}
}
// endpoint interrupt
if (dwStatus & EP_SLOW) {
// clear EP_SLOW
USBDevIntClr = EP_SLOW;
// check all endpoints
for (i = 0; i < 32; i++) {
dwIntBit = (1 << i);
if (USBEpIntSt & dwIntBit) {
// clear int (and retrieve status)
USBEpIntClr = dwIntBit;
Wait4DevInt(CDFULL);
bEPStat = USBCmdData;
// convert EP pipe stat into something HW independent
bStat = ((bEPStat & EPSTAT_FE) ? EP_STATUS_DATA : 0) |
((bEPStat & EPSTAT_ST) ? EP_STATUS_STALLED : 0) |
((bEPStat & EPSTAT_STP) ? EP_STATUS_SETUP : 0) |
((bEPStat & EPSTAT_EPN) ? EP_STATUS_NACKED : 0) |
((bEPStat & EPSTAT_PO) ? EP_STATUS_ERROR : 0);
// call handler
if (_apfnEPIntHandlers[i / 2] != NULL) {
DEBUG_LED_ON(10);
_apfnEPIntHandlers[i / 2](IDX2EP(i), bStat);
DEBUG_LED_OFF(10);
}
}
}
}
DEBUG_LED_OFF(9);
}
/** 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
}
}
}
