/**
* The robot following/avoid a light and will be stop at a distance predefinited
* @global _mode_light_ it must be set
*/
void go_light_stop(int distance)
{
int goForward; /* 1=> forward, 0 => STOP, -1 => backward */
int margin; /* The margin used to stop the robot in a aera is function by the stop distance */
int avCells; /* The average value between photo cells */
_go_light_stop_running_ = 1;
distance = cmToPhotons(distance+1); /* Photos cells and protection added */
margin = cmToPhotons(_motor_initial_speed_/50);
goForward = 1;
while(_go_light_stop_running_ && (goForward != 0))
{
int diff; /* Diffference between photos cells */
diff = light_diff();
diff *= _light_environnement_; /* Go more efficiently to the light specifing the environnement */
/* Invert the mode */
if(_mode_light_ == GO_DARK)
{
diff= -diff;
}
/*display_info();*/ /* Display the difference and values of photo cells */
avCells = (analog(photo_left) + analog(photo_right))/2; /* Refresh the average of cells */
goForward = (avCells > (distance+margin)) - (avCells < (distance-margin));
printf("%d - %d - %d\n", avCells, distance, margin);
driveb(goForward*_motor_initial_speed_, diff);
} /* The robot is on the right position */
stop();
_go_light_stop_running_ = -1;
}
/**
* The robot following/avoid a light infinitely
* @global _mode_light_ it must be set
*/
void go_light()
{
_go_light_running_ = 1;
while(_go_light_running_)
{
int diff;
diff = light_diff();
diff *= _light_environnement_; /* Go more efficiently to the light specifing the environnement */
/* Invert the mode */
if(_mode_light_ == GO_DARK)
{
diff= -diff;
}
print_diff(diff);
driveb(_motor_initial_speed_, diff);
}
stop();
_go_light_running_ = -1;
}
/**
* Move forward, and when a wall is detected, follow it
* Last update: -
* @version -
*/
void main()
{
int ir_current; /* Store, temporarily the ir values */
int ir_previous = 0;
int light;
/* Initialize all components needed to run the robot */
init_motors();
_motor_initial_speed_ = 100;
start_process(running_forever());
while(1)
{
int enc_dif = _right_enc_counts_-_left_enc_counts_;
/*
* Depending on the ir_detect() value),
* make it rotate, and move forward again
*/
ir_current = ir_detect();
/* Something is detected */
if(ir_current != 0)
{
if(ir_current == OBSTACLE_FRONT)
{
printf("FRONT\n");
/* Stop the robot */
stop_process(_running_process_running_);
wait_process(_running_process_running_);
/* Check the light difference */
light = light_diff();
rotate(C_MOTOR, ((light < 0) - (light >= 0))*90);
/* The robot can go foward after the dodge */
start_process(running_forever());
ir_previous = 0;
}
else
{
printf("move out %d\n", 10 * ((ir_previous == OBSTACLE_LEFT) - (ir_previous == OBSTACLE_RIGHT)));
/* Stop the robot */
stop_process(_running_process_running_);
wait_process(_running_process_running_);
rotate(C_MOTOR, 10 * ((ir_current == OBSTACLE_LEFT) - (ir_current == OBSTACLE_RIGHT)));
/* The robot can go foward after the dodge */
start_process(running_forever());
ir_previous = ir_current;
}
}
else if(ir_previous)
{
printf("move to %d\n", -10 * ((ir_previous == OBSTACLE_LEFT) - (ir_previous == OBSTACLE_RIGHT)));
/* Stop the robot */
stop_process(_running_process_running_);
wait_process(_running_process_running_);
/* Check the light difference */
rotate(C_MOTOR, -10 * ((ir_previous == OBSTACLE_LEFT) - (ir_previous == OBSTACLE_RIGHT)));
/* The robot can go foward after the dodge */
start_process(running_forever());
}
sleep(0.1);
}
}
