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;
}
