/** 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 } } }