void gain_menu(){ uart0_printf("-----Gain Setting Menu-----\r\n"); uart0_printf("a: display all gain\r\n"); uart0_printf("p: set p_gain\r\n"); uart0_printf("i: set i_gain\r\n"); uart0_printf("d: set d_gain\r\n"); uart0_printf("q: set valid OF quality threshold\r\n"); uart0_printf("s: save all gain setting\r\n"); uart0_printf("r: restore parameters\r\n"); uart0_printf("e: return top menu\r\n"); uart0_printf("---------------------------\r\n"); }
/* ---- Main function ---- */ int main(void) { int temp; init_current_time(); // Intro uart0_printf("\r\n\r\nSTORM SoC Basic Configuration\r\n"); uart0_printf("Demo program + Timer\r\n\r\n"); uart1_print(); while (1) { //TIMER0_LOAD = 1562; check_timer (50) ; } }
void top_menu(){ uart0_printf("---------------------------\r\n"); uart0_printf("-- Test Console ver1.4 ---\r\n"); uart0_printf("---------------------------\r\n"); uart0_printf("r: read radio\r\n"); uart0_printf("f: read flow\r\n"); uart0_printf("w: read radio and flow\r\n"); uart0_printf("g: set gain\r\n"); uart0_printf("---------------------------\r\n"); }
void gain_menu_branch(uint8_t com_type){ switch(com_type){ case DISPLAY: uart0_printf("P: %f, I: %f, D: %f\r\n", c_gain->p_gain, c_gain->i_gain, c_gain->d_gain); break; case SET_P: uart0_printf("P Gain is %f, input new value.\r\n", c_gain->p_gain); c_gain->p_gain = uart0_get_float_input(); break; case SET_I: uart0_printf("I Gain is %f, input new value.\r\n", c_gain->i_gain); c_gain->i_gain = uart0_get_float_input(); break; case SET_D: uart0_printf("D Gain is %f, input new value.\r\n", c_gain->d_gain); c_gain->d_gain = uart0_get_float_input(); break; case SET_Q_TH: uart0_printf("OF quality threshold is %d, input new value. (range: 0~255)\r\n", c_flow->qual_th); c_flow->qual_th = (uint16_t)uart0_get_float_input(); if(c_flow->qual_th > 255) c_flow->qual_th = 255; break; case SAVE: write_float(P_ADD, c_gain->p_gain); write_float(I_ADD, c_gain->i_gain); write_float(D_ADD, c_gain->d_gain); write_uint16(Q_TH_ADD, c_flow->qual_th); uart0_printf("Saved!\r\n"); break; case RESTORE: c_gain->p_gain = read_float(P_ADD); c_gain->i_gain = read_float(I_ADD); c_gain->d_gain = read_float(D_ADD); uart0_printf("Restore Parameters.\r\n"); break; case EXIT: menu_flg = 0; input_detect = 0; top_menu(); break; default: uart0_printf("invalid command\r\n"); input_detect = 0; break; } }
void check_timer (int icount) { if ( TIMER0_VALUE < icount) { uart0_printf("timer under count\n\r"); init_current_time(); } }
void Init_i2c(){ int32_t ret = SUCCESS; init_i2c_port(); i2c = &I2cDev[3]; i2c->Cfg.BaudRate = 400000; i2c->Cfg.Mode = I2cDev_MODE_MASTER; ret = i2c->Init(); if(ret == SUCCESS) uart0_printf("I2C Initialized\r\n"); }
/* ---- Main function ---- */ int main(void) { //int temp; unsigned int temp; unsigned char temp_string[9]="\0"; io_disable_xint(); // timer init //not usingSTME0_CNT = 0; //not using STME0_VAL = 50000000; // threshold value for 1s ticks // not using STME0_CONF = (1<<2) | (1<<1) | (1<<0); // interrupt en, auto reset, timer enable // not using timer VICVectAddr0 = (unsigned long)timer0_isr; //VICVectCntl0 = (1<<5) | 0; // enable and channel select = 0 (timer0) //VICIntEnable = (1<<0); // enable channel 0 (timer0) // ic_init (); //uart0_printf("ic_init complete\n\r"); //io_enable_xint(); // enable IRQ ----still works //uart0_printf("io_enable_xint complete\n\r"); // Intro uart0_printf("\r\n\r\nSTORM SoC Basic Configuration\r\n"); uart0_printf("Interupt Demo program+IRQ test\r\n\r\n"); print_irq() ; uart0_printf("Press any key!\r\n"); uart0_printf("s = print IRQ status\r\n"); uart0_printf("i = Software Int\r\n"); uart0_printf("e = Enable all Ints\r\n"); uart0_printf("d = Disable all Ints\r\n"); // echo received char while(1) { temp = io_uart0_read_byte(); if (temp != -1) io_uart0_send_byte(temp); if (temp == 's') { print_irq(); } if (temp == 'i') { sw_int (); } if (temp == 'e') { enable_all_int (); } if (temp == 'd') { disable_all_int (); } } }
void top_menu_branch(uint8_t com_type){ if(p_flg == 1){ switch(com_type){ case RADIO: uart0_printf("IN: %d, %d, %d OUT: %d, %d\r\n", c_radio->input[0], c_radio->input[1], c_radio->input[2], c_radio->output[0], c_radio->output[1]); break; case FLOW: uart0_printf("%5d, %5d, %5d, %3d, ", c_flow->frame_count, c_flow->flow_comp_m_x, c_flow->flow_comp_m_y, c_flow->qual); //uart0_printf("%5d\r\n", c_flow->ground_distance); uart0_printf("%5d, %5d, %5d, %5d\r\n", c_flow->gyro_x_rate, c_flow->gyro_y_rate, c_flow->gyro_z_rate, c_flow->ground_distance); break; case RADIO_FLOW: uart0_printf("IN: %d, %d OUT: %d, %d ", c_radio->input[0], c_radio->input[1], c_radio->output[0], c_radio->output[1]); uart0_printf("Flow: %+.4f, %+.4f, %3d, %4d\r\n", c_flow->x, c_flow->y, c_flow->qual, c_flow->ground_distance); break; case SET_GAIN: input_detect = 0; menu_flg = 1; gain_menu(); break; default: uart0_printf("invalid command\r\n"); input_detect = 0; break; } p_flg = 0; } }
///////////////////////////////////////////////////////////// // steps to process interrupt // 1) Clear interrupt in Peripheral IC_INT_SOFTCLEAR_0 // 2) clear interrupt in int controller IC_IRQ0_ENABLECLR // 3) Do something // 4) Re-enable interrupt IC_IRQ0_ENABLEST ///////////////////////////////////////////////////////////// void __attribute__ ((interrupt("IRQ"))) sw_int_proc(void) { io_disable_xint(); IC_INT_SOFTCLEAR_0 = 0x1; IC_IRQ0_ENABLECLR = 0x1; uart0_printf("Software Interrupt!\r\n"); IC_IRQ0_ENABLESET = 0x1; io_enable_xint(); // enable IRQ ----still works // toggle status led // set_syscpreg((get_syscpreg(SYS_IO) ^ 0x01), SYS_IO); // // acknowledge interrupt // //VICVectAddr = 0; }
/*------------------------------------------------------------/ 函数名称: RTC_display 功能描述: RTC时间设置函数 返 回 值: 无 -------------------------------------------------------------*/ void RTC_display(void) { unsigned short year ; unsigned char month, day ; //week unsigned char hour, minute, second ; rRTCCON = 1 ; //RTC read and write enable year = 0x2000+rBCDYEAR ; //年 month = rBCDMON ; //月 day = rBCDDATE ; //日 // week = BCDDAY ; //星期 hour = rBCDHOUR ; //小时 minute = rBCDMIN ; //分 second = rBCDSEC ; //秒 rRTCCON &= ~1 ; //RTC read and write disable uart0_printf( "RTC time : %04x-%02x-%02x %02x:%02x:%02x\n", year, month, day, hour, minute, second ); }
/*! Device wrapper for console */ static int uart0_send ( void *data, size_t size, uint flags, device_t *dev ) { int retval; console_cmd_t *cmd; if ( dev->flags & DEV_TYPE_CONSOLE ) { cmd = data; switch ( cmd->cmd ) { case CONSOLE_PRINT: retval = uart0_printf ( cmd->cd.print.attr, &cmd->cd.print.text[0]); break; case CONSOLE_CLEAR: retval = uart0_clear (); break; case CONSOLE_GOTOXY: retval = uart0_gotoxy ( cmd->cd.gotoxy.x, cmd->cd.gotoxy.y ); break; default: retval = EXIT_FAILURE; break; } } else { retval = EXIT_FAILURE; } return retval; }
int main(void) { int i; int led = 0; unsigned long count = 0; // unsigned long baud = 57600; unsigned long baud = 1000000; int ret; volatile float imuAngle[3]; // Variables to keep track for reading servos int nRead=0, idRead=0; uint8_t *ctableByte; int serialWait = 0; // Dynamixel packets and counters for input/output DynamixelPacket *pktReadData, *pktStatus; DynamixelPacket pktSerialInput, pktRs485Input; init(); uart0_printf("\r\nStarting %s\r\n", CONTROLLER_NAME); uart0_printf("Switching serial to %lu baud...\r\n", baud); _delay_ms(100); uart0_setbaud(baud); uart0_printf("\r\nRunning serial at %lu baud\r\n", baud); _delay_ms(100); // Set Dynamixel return delay rs485_putstrn("\xFF\xFF\xFE\x04\x03\x05\x00\xF5", 8); while (1) { count++; if (count % 1000 == 0) { // Toggle MON and Dynamixel Leds if (led) { led = 0; sbi(PORTC, PORTC4); rs485_putstrn("\xFF\xFF\xFE\x04\x03\x19\x00\xE1", 8); } else { led = 1; cbi(PORTC, PORTC4); rs485_putstrn("\xFF\xFF\xFE\x04\x03\x19\x01\xE0", 8); } _delay_us(100); } if (uart0_recv(&pktSerialInput)) { // RxD LED on cbi(PORTC, PORTC1); if (pktSerialInput.id == controlTable.id) { switch (pktSerialInput.instruction) { case INST_WRITE: for (i = 1; i < pktSerialInput.length-2; i++) { controlTablePtr[pktSerialInput.parameter[0]+i-1] = pktSerialInput.parameter[i]; } // Status packet pktStatus = dynamixel_status(controlTable.id, 0, NULL, 0); break; case INST_READ: pktStatus = dynamixel_status(controlTable.id, 0, (uchar *)controlTablePtr+pktSerialInput.parameter[0], pktSerialInput.parameter[1]); break; case INST_PING: pktStatus = dynamixel_status(controlTable.id, 0, NULL, 0); break; default: // Unknown command pktStatus = dynamixel_status(controlTable.id, 1, NULL, 0); break; } uart0_send(pktStatus); } else { // Forward packet to RS485 rs485_send(&pktSerialInput); if (pktSerialInput.id != DYNAMIXEL_BROADCAST_ID) { serialWait = 1; } } // RxD LED off sbi(PORTC, PORTC1); } // if (uart0_recv()) if (!serialWait) { // TxD LED on cbi(PORTC, PORTC2); // Query servo for data in round robin fashion: if (++nRead >= controlTable.nServo) nRead = 0; idRead = controlTable.idMap[nRead]; pktReadData = dynamixel_instruction_read_data(idRead, controlTable.addrRead, controlTable.lenRead); rs485_send(pktReadData); // TxD LED off sbi(PORTC, PORTC2); } // if (!serialWait) while (rs485_recv_timeout(&pktRs485Input, 300)) { // Check if status packet contains requested read data if (serialWait) { // Forward packet to uart0 uart0_send(&pktRs485Input); } else if ((pktRs485Input.id == idRead) && (pktRs485Input.instruction == 0) && (pktRs485Input.length == controlTable.lenRead+2)) { // Packet is correct return, flash EDIT Led cbi(PORTC, PORTC3); ctableByte = controlTable.dataRead + nRead*(controlTable.lenRead+1); *ctableByte++ = idRead; for (i=0; i<controlTable.lenRead; i++) { *ctableByte++ = pktRs485Input.parameter[i]; } sbi(PORTC, PORTC3); } } // while (rs485_recv()) // Timeout waiting for serial response if (serialWait) { if (++serialWait > 3) serialWait = 0; } //check to see if we got full set of adc values //if the data is ready, it will be copied to the provided pointer cli(); //disable interrupts to prevent race conditions while copying, //since the interrupt-based ADC cycle will write asynchronously ret = adc_get_data(controlTable.imuAcc); sei(); //re-enable interrupts /* if (ret > 0) ProcessImuReadings(controlTable.imuAcc,controlTable.imuAngle); */ /* if (ret > 0) { ProcessImuReadings(controlTable.imuAcc,controlTable.imuAngle); for (i = 0; i<3;i++) controlTable.imuAngle2[i]= 32768 + 1024* controlTable.imuAngle[i] ; }*/ if (ret > 0) { ProcessImuReadings(controlTable.imuAcc,imuAngle); for (i = 0; i<3;i++) controlTable.imuAngle[i]= 32768 + (uint16_t) 1024* imuAngle[i] ; } if (PINB & _BV(PB4)) { //if pin high, the button is not pressed controlTable.button = 0; LED_ESTOP_PORT &= ~(_BV(LED_ESTOP_PIN)); } else { //if pin is low, the button is pressed controlTable.button = 1; LED_ESTOP_PORT |= _BV(LED_ESTOP_PIN); } } // while (1) return 0; }
void sw_int () { IC_INT_SOFTSET_0 = 0x1; uart0_printf("SW INTERUPT!\n\r"); print_irq(); }
void print_irq() { //unsigned long reg0; unsigned int temp; char temp_string[10]="\0"; temp = IC_IRQ0_STATUS; long_to_hex_string(temp, temp_string, 8); uart0_printf("IRQ Status: "); uart0_printf(temp_string); uart0_printf(" \r\n"); temp = IC_IRQ0_RAWSTAT; long_to_hex_string(temp, temp_string, 8); uart0_printf("IRQ RAW Status: "); uart0_printf(temp_string); uart0_printf(" \r\n"); temp = IC_IRQ0_ENABLESET; long_to_hex_string(temp, temp_string, 8); uart0_printf("IRQ EN SET Status: "); uart0_printf(temp_string); uart0_printf(" \r\n"); temp = IC_IRQ0_ENABLECLR; long_to_hex_string(temp, temp_string, 8); uart0_printf("IRQ EN CLR Status: "); uart0_printf(temp_string); uart0_printf(" \r\n"); temp = IC_INT_SOFTSET_0; long_to_hex_string(temp, temp_string, 8); uart0_printf("SW INT SET Status: "); uart0_printf(temp_string); uart0_printf(" \r\n"); temp = IC_INT_SOFTCLEAR_0; long_to_hex_string(temp, temp_string, 8); uart0_printf("SW INT CLR Status: "); uart0_printf(temp_string); uart0_printf(" \r\n"); temp = IC_FIRQ0_STATUS; long_to_hex_string(temp, temp_string, 8); uart0_printf("FIRQ Status: "); uart0_printf(temp_string); uart0_printf(" \r\n"); }
void enable_all_int() { IC_IRQ0_ENABLESET = 0xFFFFFFFF; uart0_printf("Enabling all Interrupts\n\r"); print_irq(); }
void disable_all_int() { IC_IRQ0_ENABLECLR = 0xFFFFFFFF; uart0_printf("Disabling all Interrupts\n\r"); print_irq(); }
int main(void) { int c; int ret; uint8_t id; uint8_t type; uint8_t * data; int16_t cntr = 0; DDRA |= _BV(PA6) | _BV(PA7); DDRC = 0xFF; //PORTC= 0X00; int ii=0; uint8_t val = 1; init(); uart0_printf("estop initialized\r\n"); DynamixelPacket xbeePacket; DynamixelPacketInit(&xbeePacket); uint8_t robotId; uint8_t robotState; const int bufSize = 256; uint8_t buf[bufSize]; uint8_t * pbuf; uint8_t estopPacketLen; const int nRobots = 9; uint8_t mode[nRobots+1] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; estopPacketLen = DynamixelPacketWrapData(MMC_ESTOP_DEVICE_ID, MMC_ESTOP_STATE,mode,nRobots+1,buf,bufSize); uint8_t modVal; while(1) { cntr++; if (cntr == 1000) cntr = 0; modVal = cntr % 200; if (modVal == 0) { //generate new packet based on the latest desired estop state estopPacketLen = DynamixelPacketWrapData(MMC_ESTOP_DEVICE_ID, MMC_ESTOP_STATE,commandedState,nRobots+1,buf,bufSize); pbuf = buf; for (ii=0; ii<estopPacketLen; ii++) uart1_putchar(*pbuf++); //uart0_printf("sent estop packet\r\n"); } //show the actual status for brief moment if (modVal < 50) { ShowActualStateAll(); } else { ShowCommandedStateAll(); } //read from xbee while( (c = uart1_getchar()) != EOF ) { //uart0_printf("%X ",c);; ret = DynamixelPacketProcessChar(c,&xbeePacket); if (ret > 0) { id = DynamixelPacketGetId(&xbeePacket); type = DynamixelPacketGetType(&xbeePacket); data = DynamixelPacketGetData(&xbeePacket); if (id == MMC_ESTOP_DEVICE_ID && type == MMC_ESTOP_STATE) { robotId = data[0]; robotState = data[1]; if ( (robotId > 0) && (robotId < 11) && (robotState >= 0) && (robotState < 3) ) { actualState[robotId] = robotState; uart0_printf("Robot%d: estop = %d\r\n",robotId,robotState); } else uart0_printf("xbee received bad data : Robot%d: estop = %d\r\n",robotId,robotState); } } } READ_ESTOP_STATE(1); READ_ESTOP_STATE(2); READ_ESTOP_STATE(3); READ_ESTOP_STATE(4); READ_ESTOP_STATE(5); READ_ESTOP_STATE(6); READ_ESTOP_STATE(7); READ_ESTOP_STATE(8); READ_ESTOP_STATE(9); READ_ESTOP_STATE(10); //check the master pause button if ( (READ_VAL(RP_IN0_PORT,RP_IN0_PIN)) && debounceCntr[0] == DEBOUNCE_DELAY ) { if ( commandedState[0] == 1 ) { for (ii=0; ii<11; ii++) commandedState[ii] = 0; } else if (commandedState[0]==0) { for (ii=0; ii<11; ii++) commandedState[ii] = 1; } debounceCntr[0] = 0; } if (debounceCntr[0] < DEBOUNCE_DELAY) debounceCntr[0]++; SHOW_COMMANDED_STATE(0); /* if ( READ_VAL(RP_IN1_PORT,RP_IN1_PIN) ) SET_PIN(LED_OUT1_PORT,LED_OUT1_PIN); else CLEAR_PIN(LED_OUT1_PORT,LED_OUT1_PIN); */ /* if (val == 0) DDRC=0x00; //val = 1; else val = val << 1; PORTC = val; */ //PORTC = PINL; _delay_ms(5); } return 0; }