int robot_monitor (void) {
// start the calibration period...
printf("Running usetup()...\n");
usetup();
printf("Finished usetup().\n");
printf("Waiting for RF start, or press Go to start now.\n");
while (!rf_start && !go_press())
yield();
printf("Running umain()...\n");
return umain();
}
int umain (void) {
int32_t long_x;
int32_t long_y;
//motor_set_vel(LEFT_MOTOR,left_mot_vel);
//motor_set_vel(RIGHT_MOTOR,right_mot_vel);
//This loop prints out its position, which is set by RF
//When go is pressed, a position is sent out, and other happyboards will update their position accordingly
//Thus, this demo needs two happyboards to witness in action
while(1)
{
//printf("\n(%d,%d) angle: %d",objects[0].x,objects[0].y,objects[0].theta);
long_x = (int32_t)objects[0].x;
long_y = (int32_t)objects[0].y;
if ((long_x*long_x+long_y*long_y) < 750L*750L){
//dec_left();
printf("inside.\n");
}
else{
//inc_left();
printf("outside.\n");
}
if (go_press()) {
//transmit_position_packet(1,get_time()/1000.0,3.14159); //1 is the index of the receiving robot. For now, this is 1 for all robots
}
delay_busy_ms(100);
}
// Will never return, but the compiler complains without a return
// statement.
return 0;
}
int umain() {
uint16_t i,n = 36;
uint16_t port=23;
uint16_t km,kc;
uint16_t xd[36];
uint16_t yd[36];
//happylib_init();
// start
printf("\nIRDistCal Press Go");
go_click();
// get number of samples to read
while (!go_press()) {
printf("\nUse frob to # ofsamples: %2d",n);
switch (frob_read_range(0,2)) {
case 0: n= 9; break;
case 1: n=18; break;
case 2: n=36; break;
}
pause (40);
}
// wait for go release
while (go_press());
// fill distance array
for (i=0;i<n;i++) {
xd[i] = 10 + 2*i*(36/n);
}
// get port number
while (!go_press()) {
port = frob_read_range(8,23);
printf("\nUse frob to select port: %2d",port);
pause (40);
}
// wait for go release
while (go_press());
// read samples
for (i=0;i<n;i++) {
while (!go_press()) {
yd[i] = analog_read(port);
printf("\nSample @ %2dcm =%4d",xd[i],yd[i]);
}
// wait for go release
while (go_press());
}
// calculate & print
irdist_fit(xd,yd,n,&km,&kc);
printf("\nOK: M: %5d C: %d, press Go",km,kc);
go_click();
// save
/*
Disabled until confdb is working
printf("\nGo to Save calibStop to quit");
while (1) {
if (go_press()) {
go_click();
confdb_save_integer(CONF_HLIB_IRDIST_M,km);
confdb_save_integer(CONF_HLIB_IRDIST_C,kc);
break;
}
if (stop_press()) {
stop_press();
break;
}
}
*/
printf("\ncalibration done");
// do nothing forever
while (1);
return 0;
}
// Initialise board
void board_init (void) {
#ifndef SIMULATE
io_init(); // Init GPIOs
uart_init(BAUD_RATE);
stderr = &uartio;
printf(str_boot_uart,BAUD_RATE);
printf(str_boot_start);
#else
printf("Skipping UART initialization...\n");
#endif
#ifndef SIMULATE
digital_init();
#endif
encoder_init();
#ifndef SIMULATE
spi_init();
motor_init();
servo_init();
#ifdef LCD_DEBUG
lcd_init(); //consider wrapping this in an #ifdef LCD_DEBUG tag?
stdout = &lcdout;
#else
stdout = &uartio;
stdin = &uartio;
#endif
adc_init();
isr_init();
memory_init();
#endif
// load config, or fail if invalid
if (!board_load_config())
board_fail("Bad Config");
printf(str_boot_conf);
printf(str_boot_board,
board_config.version>>8,
board_config.version&0xFF);
printf(str_boot_id, board_config.id);
// print boot text to screen
printf(str_boot_message, board_config.version>>8, board_config.version&0xFF);
// check battery, fail if <7.5V
printf(str_boot_batt,read_battery());
#ifdef CHECK_BATTERY
if (!(read_battery()>=7200)) {
// NOTE: in the current 2-battery version of the HappyBoard, the
// battery voltage is the motor battery (P+). Holding GO overrides
// the check so you can run the HappyBoard without a motor battery.
if (go_press())
printf("WARNING: LOW BATTERY\n");
else
board_fail("Low battery");
} else {
printf("Battery OK\n");
}
#endif
#ifndef SIMULATE
// initialise FPGA
if (!fpga_init(FPGA_CONFIG_ADDRESS, board_config.fpga_len))
board_fail("FPGA failure");
printf(str_boot_fpga, fpga_get_version_major(), fpga_get_version_minor());
#else
printf("Skipping FPGA initialization...\n");
#endif
// all ok
#ifndef SIMULATE
#ifdef LCD_DEBUG
lcd_set_pos(31);
lcd_print_char('\1', NULL);
#else
printf("Board init complete.\n");
#endif
#else
printf("Board init complete.\n");
#endif
#ifndef SIMULATE
LED_COMM(0);
#endif
}
void stop_filter() {
if (go_press()) {
state = MOVING;
}
}
