//exec time: about 900us
void analog_update_fsm(u08 cmd, u08 *param)
{
static uint8 count=0;
static uint32 vbatt;
uint8 i;
static u08 initialized=0;
//task_open();
if(!initialized)
{
initialized = 1;
usb_printf("analog_update_fsm()\n");
//for(i=0;i<=7;i++) s.inputs.analog[i] = (analog_read(i))>>2;
s.inputs.vbatt = read_battery_millivolts_svp();
vbatt = read_battery_millivolts_svp();
}
//while(1)
{
count++;
//for(i=0;i<=7;i++) s.inputs.analog[i] = (uint8)((((uint16)(s.inputs.analog[i]))*1 + (uint16)(analog_read(i)>>2) )/2);
for(i=0; i<=15; i++)
{
set_digital_output(ANALOG_MUX_ADDR_0_PIN, i & 0x01);
set_digital_output(ANALOG_MUX_ADDR_1_PIN, i & 0x02);
set_digital_output(ANALOG_MUX_ADDR_2_PIN, i & 0x04);
set_digital_output(ANALOG_MUX_ADDR_3_PIN, i & 0x08);
s.inputs.analog[i] = (uint16)(analog_read(4)>>2);
}
s.line[RIGHT_LINE] = s.inputs.analog[AI_LINE_RIGHT];
s.line[LEFT_LINE] = s.inputs.analog[AI_LINE_LEFT];
//sample the following only once every 5 * 20 == 100ms
if( count >= 5)
{
count = 0;
vbatt = (vbatt*7UL + (uint32)read_battery_millivolts_svp())/8UL;
s.inputs.vbatt = (uint16)vbatt;
}
//task_wait(interval);
}
//task_close();
}
void check_for_qtr_shorts()
{
// Test for shorts
unsigned int sensor_pins[] = {IO_C0,IO_C1,IO_C2,IO_C3,IO_C4};
unsigned char shorts = 1;
clear();
print("qtr");
lcd_goto_xy(0,1);
print("shorts");
while (shorts)
{
unsigned char i;
shorts = 0;
for(i=0;i<5;i++)
{
unsigned char j;
for (j=0;j<5;j++)
{
set_digital_input(sensor_pins[j],PULL_UP_ENABLED);
}
set_digital_output(sensor_pins[i],LOW);
delay_ms(2);
for (j=0;j<5;j++)
{
if (i == j && is_digital_input_high(sensor_pins[j]))
shorts = 1;
if (i != j && !is_digital_input_high(sensor_pins[j]))
shorts = 1;
}
}
}
}
int main()
{
int i, off=0, on=1, lports[]=LIGHT_PORTS, pcnt, w=40, m=5;
float s;
graphics_open(320,240);
for(i=0;i<256;i+=5) {
graphics_fill(i,i,i);
graphics_update();
}
SplashBG(320, 240);
msleep(1000);
graphics_close();
pcnt=NPORTS(lports);
for(i=8;i<16;i++) { // change the digital ports to output
set_digital_output(i,1);
}
srand(time(NULL)); // prime random number generation
while(1) {
table_lights(lports,pcnt,off); // shut off table lights
setup(); // team setup (light calibration w/time out option)
// setup done, including any time outs so proceed with judge's setup
//get_shelf_positions(); // display orange/yellow block set up for judges - 2014
get_bgal_pod_positions(); // display left-right positions for Botgal & pod - 2015
set_a_button_text("QUIT"); set_b_button_text("-"); set_c_button_text("-");
graphics_close(); // close any open graphics
graphics_open(3*w,62);
display_clear();
display_printf(0,0,"************** BOTBALL 2015 **************");
display_printf(0,1," LIGHTS ON IN");
display_printf(0,2,"******************************************");
display_printf(0,9," Press QUIT to stop count down");
if (countdown_sd(5,1,BLUE,YELLOW,m,w)) continue;
s=seconds();
// 2015
table_lights(lports,pcnt,on); // on for 115 seconds
display_printf(0,1," GAME CLOCK - LIGHTS ON TILL 5");
if (countdown(120,11,GREEN,BLUE,m,w)) continue;
// end 2015
/*
// 2014
table_lights(lports,pcnt,on); // on for 15 seconds
display_printf(0,1," GAME CLOCK - LIGHTS ON TILL 105");
if (countdown(120,106,GREEN,BLUE,m,w)) continue;
table_lights(lports,pcnt,off); // of for 100 seconds
display_printf(0,1," GAME CLOCK - LIGHTS OFF TILL 5");
if (countdown(105,11,LTBLUE,RED,m,w)) continue;
// end 2014
*/
if (countdown(10,6,LTORANGE,BLUE,m,w)) continue;
display_printf(0,1," GAME CLOCK - LIGHTS BLINK TILL 0");
i=5+(seconds()-s);
if (blinkdown(5,0,RED,YELLOW,m,w,lports,pcnt)) continue;
display_printf(0,1," GAME CLOCK - %d ",i);
display_printf(0,9," ***** GAME OVER! ***** ");
table_lights(lports,pcnt,off); // shut off table lights
msleep(5000); // pause display
}
return 0;
}
int main()
{
// Make PC1 be an input with its internal pull-up resistor enabled.
// It will read high when nothing is connected to it.
set_digital_input(IO_C1, PULL_UP_ENABLED);
while(1)
{
if(is_digital_input_high(IO_C1)) // Take digital reading of PC1.
{
set_digital_output(IO_D1, HIGH); // PC1 is high, so drive PD1 high.
}
else
{
set_digital_output(IO_D1, LOW); // PC1 is low, so drive PD1 low.
}
}
}
int main()
{
// Make SSbar be an output so it does not interfere with SPI communication.
set_digital_output(IO_B4, LOW);
// Set the mode to SVP_MODE_ANALOG so we can get analog readings on line D/RX.
svp_set_mode(SVP_MODE_ANALOG);
while(1)
{
clear(); // Erase the LCD.
if (usb_configured())
{
// Connected to USB and the computer recognizes the device.
print("USB");
}
else if (usb_power_present())
{
// Connected to USB.
print("usb");
}
if (usb_suspend())
{
// Connected to USB, in the Suspend state.
lcd_goto_xy(4,0);
print("SUS");
}
if (dtr_enabled())
{
// The DTR virtual handshaking line is 1.
// This often means that a terminal program is conencted to the
// Pololu Orangutan SVP USB Communication Port.
lcd_goto_xy(8,0);
print("DTR");
}
if (rts_enabled())
{
// The RTS virtual handshaking line is 1.
lcd_goto_xy(12,0);
print("RTS");
}
// Display an analog reading from channel D, in millivolts.
lcd_goto_xy(0,1);
print("Channel D: ");
print_long(analog_read_millivolts(CHANNEL_D));
// Wait for 100 ms, otherwise the LCD would flicker.
delay_ms(100);
}
}
void freeze_halt()
{
ao();
for(int i = 0; i < 8; ++i) {
set_analog_pullup(i, 1);
}
for(int i = 8; i < 16; ++i) {
set_digital_output(i, 0);
set_digital_pullup(i, 1);
}
create_stop();
}
int main(int argc, char ** argv)
{
// first 5 digital pins will be outputs
int i;
for (i = 0; i < 5; ++i)
{
set_digital_output(i, 1);
}
// next 5 will be inputs
for (i = 5; i < 10; ++i)
{
set_digital_output(i, 0);
}
int on = 0;
while(1)
{
// set pins on/off
on ^= 1;
for (i = 0; i < 5; ++i)
{
set_digital_value(i, on);
}
// wait so we don't display too often
msleep(1000);
for (i = 0; i < 10; ++i)
{
printf("%d ", get_digital_value(i));
}
printf("\n");
}
return 0;
}
/*This function takes in "wheel1", "wheel2", "wheel3", or "wheel4"
* and speed between -100 and 100; -100 full reverse, 100 full forward,
* 0 for stop
* as paramters and set motor speed as output
*/
void mSpeed(char* wheelnum, double speed)
{
if(speed != 0) //wheels in forward or backward
{
speed = (speed * 5.0) + 1500.0; //convert -100 to 100 to apporiate speed
}
else //speed = 0 -> stop
{
speed = 1500.0;
}
set_digital_output(wheelnum, HIGH);
delay_us(speed);
set_digitial_output(wheelnum, LOW);
}
void test_task(u08 cmd, u08 *param)
{
static u16 i;
float time_to_stop=0,distance_to_stop;
static t_scan_result scan_result;
DEFINE_CFG2(s16,accel, 99,1);
DEFINE_CFG2(s16,decel, 99,2);
DEFINE_CFG2(s16,speed, 99,3);
DEFINE_CFG2(s16,distance, 99,4);
DEFINE_CFG2(s16,angle, 99,5);
task_open_1();
//code between _1() and _2() will get executed every time the scheduler resumes this task
if(cmd==0)
{
NOP();
}
else
{
NOP();
return;
}
task_open_2();
//execution below this point will resume wherever it left off when a context switch happens
usb_printf("test_task()\n");
PREPARE_CFG2(accel);
PREPARE_CFG2(decel);
PREPARE_CFG2(speed);
PREPARE_CFG2(distance);
PREPARE_CFG2(angle);
test_task(1,(uint8 *)0x1234);
/*
for(;;)
{
dbg_printf("0123456789\n");
task_wait(100);
}
*/
/*
task_wait(200);
motor_command(2,0,0,0,0);
task_wait(200);
motor_command(7,0,0,100,100);
task_wait(500);
motor_command(6,2,2,0,0);
task_wait(500);
*/
while(1)
{
task_wait(100);
UPDATE_CFG2(accel);
UPDATE_CFG2(decel);
UPDATE_CFG2(speed);
UPDATE_CFG2(distance);
UPDATE_CFG2(angle);
if(s.behavior_state[TEST_LOGIC_FSM]==1)
{
/*
1) gradually ramp up (at specified rate) to target speed
2) when we are <= 30degrees from the target, start ramping down to speed 15
3) when we are <= 10degrees from the target, apply target speed 5 (w/ feed forward) & regulate to maintain 5
3) when we are at the target, hit the brakes - full stop w/out ramping down
*/
/*
odometry_set_checkpoint();
motor_command(6,1,1,(speed),-(speed));
while ( abs(odometry_get_rotation_since_checkpoint()) < 60 ) { task_wait(10); }
motor_command(6,1,1,15,-15);
while ( abs(odometry_get_rotation_since_checkpoint()) < 80 ) { task_wait(10); }
motor_command(7,1,1,5,-5);
while ( abs(odometry_get_rotation_since_checkpoint()) < 90 ) { task_wait(10); }
motor_command(7,1,1,0,0);
*/
/*
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
MOVE(50,100);
TURN_IN_PLACE( 50, 90);
*/
#if 0
//set_digital_output(IO_D1,0);
set_digital_output(IO_D0,0);
task_wait(2000);
//set_digital_output(IO_D1,1);
set_digital_output(IO_D0,1);
#endif
#if 0
//dbg_printf("Starting scan....\n");
TURN_IN_PLACE_AND_SCAN( 40, 220 );
//dbg_printf("....done\n");
scan_result = find_peak_in_scan(scan_data,360,30);
dbg_printf("scan_result: %d,%d,%d,%d,%d,%d\n",
scan_result.flame_center_value, scan_result.rising_edge_position, scan_result.falling_edge_position,
scan_result.center_angle, scan_result.rising_edge_angle, scan_result.falling_edge_angle);
for(i=0;i<360;i++) {dbg_printf("scan_data[%03d]=%03d,%03d\n",i, scan_data[i].angle, scan_data[i].flame);task_wait(10);}
TURN_IN_PLACE( 40, -(220-scan_result.center_angle) );
#endif
TURN_IN_PLACE_AND_SCAN( 40, 90, 1);
scan_result = find_path_in_scan(scan_data, 100, 300, 0, 1);
dbg_printf("scan_result: %d,%d,%d,%d\n", scan_result.opening, scan_result.center_angle, scan_result.rising_edge_angle, scan_result.falling_edge_angle);
//for(i=0;i<100;i++) {dbg_printf("scan_data[%03d]=%03d,%03d\n",i, scan_data[i].angle, scan_data[i].ir_north);task_wait(10);}
TURN_IN_PLACE(40,-90);
/*
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, -90 );
scan_result = find_peak_in_scan(scan_data,360,3);
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, 180 );
scan_result = find_peak_in_scan(scan_data,360,3);
task_wait(2000);
TURN_IN_PLACE_AND_SCAN( 100, -180 );
scan_result = find_peak_in_scan(scan_data,360,3);
*/
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==2)
{
TURN_IN_PLACE(speed,angle);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==3)
{
TURN_IN_PLACE(40, -90);
TURN_IN_PLACE_AND_SCAN(40, 180, 4);
scan_result = find_flame_in_scan(scan_data,360,30);
if(scan_result.flame_center_value > 150) //TODO: make the minimum flame value a parameter
{
static int i, i_min;
static u16 min=999;
static float d;
static u08 stop=0;
TURN_IN_PLACE( 40, -(180-scan_result.center_angle+2) );
/*
min=999;
for(i=scan_result.rising_edge_position-10; i<=scan_result.falling_edge_position+10; i++)
{
dbg_printf("scan_data[%3d]: ir_n=%3d, a=%d, f=%d\n",i,scan_data[i].ir_north, scan_data[i].angle, scan_data[i].flame);
task_wait(50);
if(scan_data[i].ir_north < min) { min=scan_data[i].ir_north; i_min=i; }
}
dbg_printf("distance to candle: %d @i=%d,a=%d\n",min,i_min,scan_data[i_min].angle);
if(min<100) min=100;
d = (float) (((min-100)*25)/10);
MOVE2(50,d,60,60);
*/
stop=0;
move_manneuver2(1,30,9999,(80),(90));
while(move_manneuver2(0,30,9999,(70),(70)))
{
OS_SCHEDULE;
if( (s.ir[IR_N] <= 60) ) stop |= 0x01;
if( (s.ir[IR_NE] <= 60)) stop |= 0x02;
if( (s.ir[IR_NW] <= 60)) stop |= 0x04;
if( (s.inputs.sonar[0] <= 100) ) stop |= 0x08;
if(stop != 0)
{
dbg_printf("too close to object/wall! reason: 0x%02x\n",stop);
HARD_STOP();
break;
}
}
}
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==4)
{
PUMP_ON();
task_wait(1000);
PUMP_OFF();
s.behavior_state[TEST_LOGIC_FSM]=0;
}
if(s.behavior_state[TEST_LOGIC_FSM]==5)
{
dbg_printf("start = %d\n",is_digital_input_high(IO_B3));
task_wait(500);
}
/*
while(s.behavior_state[11]==1)
{
time_to_stop = (float)s.inputs.actual_speed[0] / (float)50;
distance_to_stop = ((float)s.inputs.actual_speed[0] * time_to_stop)/2.0;
distance_to_stop *= 50.0; //adjust for seconds
distance_to_stop *= 0.13466716824940938560464; //adjust for mm
if(s.inputs.x + distance_to_stop < distance)
{
motor_command(6,accel,decel,speed,speed);
}
else
{
motor_command(6,accel,decel,0,0);
s.behavior_state[TEST_LOGIC_FSM]=0;
}
task_wait(20);
}
*/
}
task_close();
}
void sparrow_init() {
//First: Turn off external power blocks
set_digital_output(BOOST_5V_EN_PIN);
write_pin(BOOST_5V_EN_PIN, false);
set_digital_output(PORT_5V_EN_PIN);
write_pin(PORT_5V_EN_PIN, false);
//TODO: Disable 5V USB generation (USB0_PPWR, P2_0)
//TODO: Set fault input interrupt handlers to shutdown peripheral and react accordingly
//E.g. USB_PWR_FAULT_HANDLER (P2_1)
set_digital_output(AUDIO_POWER_EN_PIN);
write_pin(AUDIO_POWER_EN_PIN, false);
set_digital_output(WIFI_POWER_EN_PIN );
write_pin(WIFI_POWER_EN_PIN, false );
//TODO: Why does this pin not work? For now, the input pullup keeps the eth domain powered up permamently.
//Setting OUT and writing this bit caused the device to hang. Might be correct now, check again.
// set_digital_output(ETH_POWER_EN_PIN);
write_pin(ETH_POWER_EN_PIN, false);
//Set all ext devices to !reset
set_digital_output(AUDIO_NRESET);
write_pin(AUDIO_NRESET, false);
//init base clock to xtal, main clock, via pll1
clock_set_xtal_core_freq(MAIN_CLOCK_MHZ/XTAL_MHZ, 1);
//enable derived base clocks for shared use. Dedicated clocks are enabled by the
//respective peripheral interface
clock_set_source(BASE_APB1_CLK, true, CLKSRC_PLL1); //I2C0
clock_set_source(BASE_APB3_CLK, true, CLKSRC_PLL1); //I2C1
//GPIO
set_digital_output(LED1_PIN);
set_digital_output(LED2_PIN);
set_digital_output(LED3_PIN);
set_digital_input(BUTTON_PIN, true, false, false);
write_pin(LED1_PIN, false);
write_pin(LED2_PIN, false);
write_pin(LED3_PIN, false);
//I2C
i2c_configure_pin(I2C1_SDA_PIN_GROUP, I2C1_SDA_PIN_IDX, I2C1_SDA_PIN_MODE);
i2c_configure_pin(I2C1_SCL_PIN_GROUP, I2C1_SCL_PIN_IDX, I2C1_SCL_PIN_MODE);
i2c_init(0, I2C_MODE_FAST);
i2c_init(1, I2C_MODE_FAST);
//TODO: I2C1 might also be used as GPIOs
//SPI
scu_set_pin(SPI0_MISO_GROUP, SPI0_MISO_IDX, SPI0_MISO_MODE, false, false, true, true, true);
scu_set_pin(SPI0_MOSI_GROUP, SPI0_MOSI_IDX, SPI0_MOSI_MODE, false, false, true, false, true);
scu_set_pin(SPI0_SCK_GROUP, SPI0_SCK_IDX, SPI0_SCK_MODE, false, false, true, false, true);
scu_set_pin(SPI0_SSEL_GROUP, SPI0_SSEL_IDX, SPI0_SSEL_MODE, false, false, true, false, true);
scu_set_pin(SPI1_MISO_GROUP, SPI1_MISO_IDX, SPI1_MISO_MODE, false, false, true, true, true);
scu_set_pin(SPI1_MOSI_GROUP, SPI1_MOSI_IDX, SPI1_MOSI_MODE, false, false, true, false, true);
scu_set_pin(SPI1_SCK_GROUP, SPI1_SCK_IDX, SPI1_SCK_MODE, false, false, true, false, true);
scu_set_pin(SPI1_SSEL_GROUP, SPI1_SSEL_IDX, SPI1_SSEL_MODE, false, false, true, false, true);
//I2S
i2s_configure_pin(I2S0_TX_MCLK_GROUP, I2S0_TX_MCLK_IDX, I2S0_TX_MCLK_MODE);
i2s_configure_pin(I2S0_TX_WS_GROUP, I2S0_TX_WS_IDX, I2S0_TX_WS_MODE);
i2s_configure_pin(I2S0_TX_SCK_GROUP, I2S0_TX_SCK_IDX, I2S0_TX_SCK_MODE);
i2s_configure_pin(I2S0_TX_SD_GROUP, I2S0_TX_SD_IDX, I2S0_TX_SD_MODE);
//i2s_configure_pin(I2S0_RX_MCLK_GROUP, I2S0_RX_MCLK_IDX, I2S0_RX_MCLK_MODE);
//i2s_configure_pin(I2S0_RX_WS_GROUP, I2S0_RX_WS_IDX, I2S0_RX_WS_MODE);
//i2s_configure_pin(I2S0_RX_SCK_GROUP, I2S0_RX_SCK_IDX, I2S0_RX_SCK_MODE);
//i2s_configure_pin(I2S0_RX_SD_GROUP, I2S0_RX_SD_IDX, I2S0_RX_SD_MODE);
//SD
scu_set_pin_mode(SD_CD_GROUP, SD_CD_PIN, SD_CD_MODE);
scu_enable_pin_in_buffer(SD_CD_GROUP, SD_CD_PIN, true);
scu_set_pin_mode(SD_WP_GROUP, SD_WP_PIN, SD_WP_MODE);
//TODO: configure WP pin
scu_set_pin_mode(SD_CMD_GROUP, SD_CMD_PIN, SD_CMD_MODE);
scu_set_pin_pullup(SD_CMD_GROUP, SD_CMD_PIN, true);
scu_enable_pin_in_buffer(SD_CMD_GROUP, SD_CMD_PIN, true);
scu_set_pin_mode(SD_D0_GROUP, SD_D0_PIN, SD_D0_MODE);
scu_set_pin_pullup(SD_D0_GROUP, SD_D0_PIN, true);
scu_enable_pin_in_buffer(SD_D0_GROUP, SD_D0_PIN, true);
scu_set_pin_mode(SD_D1_GROUP, SD_D1_PIN, SD_D1_MODE);
scu_set_pin_pullup(SD_D1_GROUP, SD_D1_PIN, true);
scu_enable_pin_in_buffer(SD_D1_GROUP, SD_D1_PIN, true);
scu_set_pin_mode(SD_D2_GROUP, SD_D2_PIN, SD_D2_MODE);
scu_set_pin_pullup(SD_D2_GROUP, SD_D2_PIN, true);
scu_enable_pin_in_buffer(SD_D2_GROUP, SD_D2_PIN, true);
scu_set_pin_mode(SD_D3_GROUP, SD_D3_PIN, SD_D3_MODE);
scu_set_pin_pullup(SD_D3_GROUP, SD_D3_PIN, true);
scu_enable_pin_in_buffer(SD_D3_GROUP, SD_D3_PIN, true);
scu_set_clock_pin_mode(SD_CLK_CLK, SD_CLK_MODE, false, false, true, false, false);
//Ethernet RMII
creg_init();
creg_set_eth_interface(true);
scu_set_clock_pin_mode(ETH_CLK_CLK, ETH_CLK_MODE, false, false, true, true, false);
scu_set_pin(ETH_RXD0_GROUP, ETH_RXD0_PIN, ETH_RXD0_MODE, false, false, true, true, false);
scu_set_pin(ETH_RXD1_GROUP, ETH_RXD1_PIN, ETH_RXD1_MODE, false, false, true, true, false);
scu_set_pin(ETH_TXD0_GROUP, ETH_TXD0_PIN, ETH_TXD0_MODE, false, false, true, false, false);
scu_set_pin(ETH_TXD1_GROUP, ETH_TXD1_PIN, ETH_TXD1_MODE, false, false, true, false, false);
scu_set_pin(ETH_MDC_GROUP, ETH_MDC_PIN, ETH_MDC_MODE, false, false, true, false, false);
scu_set_pin(ETH_TXEN_GROUP, ETH_TXEN_PIN, ETH_TXEN_MODE, false, false, true, false, false);
scu_set_pin(ETH_CRS_DV_GROUP, ETH_CRS_DV_PIN, ETH_CRS_DV_MODE, false, false, true, true, false);
scu_set_pin(ETH_MDIO_GROUP, ETH_MDIO_PIN, ETH_MDIO_MODE, false, false, true, true, false);
set_digital_input(ETH_NINT_PIN, true, false, true);
set_digital_output(ETH_NRST_PIN);
write_pin(ETH_NRST_PIN, false); //put into reset
//CC3000 WIFI
set_digital_output(CC3000_SW_EN_PIN);
write_pin(CC3000_SW_EN_PIN, false);
set_digital_input(CC3000_IRQ_PIN, false, false, true);
}