int main(void)
{
//lprintf("%d", 4);
init_ADC();
init_timer3();
init_timer();
init_USART(calcUBRR(BAUD));
char * str = "Degrees\tIR Distance (cm)\tSonar Distance (cm)\n\n\r";
move_servo(0);
char degrees = 0;
while(1)
{
//send_pulse();
Transmit_String(str);
sprintf(str, "%d\t%d\t%d\n\r", degrees, ir_distance(), time2dist(ping_read()));
move_servo(degrees);
degrees += 2;
if(degrees > 180)
break;
}
}
/**
* Runs the main part of the program. Note it is an infinite loop.
* @author Group B1
* @date 12/4/2012
*/
void run_robot(oi_t *oi)
{
int x = STARTING_X_POS;
int y = STARTING_Y_POS;
int o = STARTING_ORENTATION;
int msg = 0;
while(1){
char c = read_one_char();
oi_update(oi);
printf("11 %d\n", oi->virtual_wall);
lprintf("\n Group B1\n\n\n\n");
lprintf("%d %d %d %d\n%d", oi->cliff_left_signal , oi->cliff_frontleft_signal , oi->cliff_frontright_signal , oi->cliff_right_signal, oi->virtual_wall );
if( oi->cliff_left_signal > 1000 && oi->cliff_frontleft_signal > 1000 && oi->cliff_frontright_signal > 1000 && oi->cliff_right_signal > 1000 ){
printf("10\n");
}
if(c == FORWARD)
{
if(move_forward(oi, DEFAULT_DISTANCE) >= DEFAULT_DISTANCE){
if(o == NORTH){
y = y + 1 * STEP;
}
else if(o == EAST){
x = x + 1 * STEP;
}
else if(o == SOUTH){
y = y - 1 * STEP;
}
else if(o == WEST){
x = x - 1 * STEP;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
}
else if(c == RIGHT_TURN)
{
turn_clockwise();
if(o == NORTH){
o = WEST;
}
else if(o == EAST){
o = NORTH;
}
else if(o == SOUTH){
o = EAST;
}
else if(o == WEST){
o = SOUTH;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == LEFT_TURN)
{
turn_counter_clockwise();
if(o == NORTH){
o = EAST;
}
else if(o == EAST){
o = SOUTH;
}
else if(o == SOUTH){
o = WEST;
}
else if(o == WEST){
o = NORTH;
}
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == RESET)
{
move_servo(0);
x = STARTING_X_POS;
y = STARTING_Y_POS;
o = STARTING_ORENTATION;
msg = 0;
printf("2 %d %d %d %d\n", x,y,o,msg);
}
else if(c == MUSIC)
{
load_song();
oi_play_song(JAWS);
}
else if(c == SCAN)
{
oi_update(oi);
printf("1 %d %d %d %d %d ", oi->virtual_wall ,x,y,o,msg);
for(int i = 0 ; i <= 1800 ;i = i + 1)
{
move_servo(i * 1);
int x = ir_distance();
printf("%d ", x);
}
printf("\n");
move_servo(0);
}
}
}
/**
* @brief Interpret IR data into more user friendly variables.
*
* @param wm Pointer to a wiimote_t structure.
*/
static void interpret_ir_data(struct wiimote_t* wm) {
struct ir_dot_t* dot = wm->ir.dot;
int i;
float roll = 0.0f;
int last_num_dots = wm->ir.num_dots;
if (WIIMOTE_IS_SET(wm, WIIMOTE_STATE_ACC))
roll = wm->orient.roll;
/* count visible dots */
wm->ir.num_dots = 0;
for (i = 0; i < 4; ++i) {
if (dot[i].visible)
wm->ir.num_dots++;
}
switch (wm->ir.num_dots) {
case 0:
{
wm->ir.state = 0;
/* reset the dot ordering */
for (i = 0; i < 4; ++i)
dot[i].order = 0;
wm->ir.x = 0;
wm->ir.y = 0;
wm->ir.z = 0.0f;
return;
}
case 1:
{
fix_rotated_ir_dots(wm->ir.dot, roll);
if (wm->ir.state < 2) {
/*
* Only 1 known dot, so use just that.
*/
for (i = 0; i < 4; ++i) {
if (dot[i].visible) {
wm->ir.x = dot[i].x;
wm->ir.y = dot[i].y;
wm->ir.ax = wm->ir.x;
wm->ir.ay = wm->ir.y;
/* can't calculate yaw because we don't have the distance */
//wm->orient.yaw = calc_yaw(&wm->ir);
ir_convert_to_vres(&wm->ir.x, &wm->ir.y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
break;
}
}
} else {
/*
* Only see 1 dot but know theres 2.
* Try to estimate where the other one
* should be and use that.
*/
for (i = 0; i < 4; ++i) {
if (dot[i].visible) {
int ox = 0;
int x, y;
if (dot[i].order == 1)
/* visible is the left dot - estimate where the right is */
ox = dot[i].x + wm->ir.distance;
else if (dot[i].order == 2)
/* visible is the right dot - estimate where the left is */
ox = dot[i].x - wm->ir.distance;
x = ((signed int)dot[i].x + ox) / 2;
y = dot[i].y;
wm->ir.ax = x;
wm->ir.ay = y;
wm->orient.yaw = calc_yaw(&wm->ir);
if (ir_correct_for_bounds(&x, &y, wm->ir.aspect, wm->ir.offset[0], wm->ir.offset[1])) {
ir_convert_to_vres(&x, &y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
wm->ir.x = x;
wm->ir.y = y;
}
break;
}
}
}
break;
}
case 2:
case 3:
case 4:
{
/*
* Two (or more) dots known and seen.
* Average them together to estimate the true location.
*/
int x, y;
wm->ir.state = 2;
fix_rotated_ir_dots(wm->ir.dot, roll);
/* if there is at least 1 new dot, reorder them all */
if (wm->ir.num_dots > last_num_dots) {
reorder_ir_dots(dot);
wm->ir.x = 0;
wm->ir.y = 0;
}
wm->ir.distance = ir_distance(dot);
wm->ir.z = 1023 - wm->ir.distance;
get_ir_dot_avg(wm->ir.dot, &x, &y);
wm->ir.ax = x;
wm->ir.ay = y;
wm->orient.yaw = calc_yaw(&wm->ir);
if (ir_correct_for_bounds(&x, &y, wm->ir.aspect, wm->ir.offset[0], wm->ir.offset[1])) {
ir_convert_to_vres(&x, &y, wm->ir.aspect, wm->ir.vres[0], wm->ir.vres[1]);
wm->ir.x = x;
wm->ir.y = y;
}
break;
}
default:
{
break;
}
}
#ifdef WITH_WIIUSE_DEBUG
{
int ir_level;
WIIUSE_GET_IR_SENSITIVITY(wm, &ir_level);
WIIUSE_DEBUG("IR sensitivity: %i", ir_level);
WIIUSE_DEBUG("IR visible dots: %i", wm->ir.num_dots);
for (i = 0; i < 4; ++i)
if (dot[i].visible)
WIIUSE_DEBUG("IR[%i][order %i] (%.3i, %.3i) -> (%.3i, %.3i)", i, dot[i].order, dot[i].rx, dot[i].ry, dot[i].x, dot[i].y);
WIIUSE_DEBUG("IR[absolute]: (%i, %i)", wm->ir.x, wm->ir.y);
}
#endif
}
