void _start() {
while(1) {
switch(mode) {
case(FIND_MODE):
/* Anda em linha reta até encontrar parede */
set_motors_speed(10, 10);
if(find_wall() == 1) {
/* Encontrou uma parede, muda o modo
e pára o robo*/
mode = SET_POSITION_MODE;
set_motors_speed(0, 0);
}
break;
case(SET_POSITION_MODE):
/* Encontrou uma parede, posicione o lado esquerdo ao lado dela */
/* e mude o modo */
if(set_position_to_left() == 1) {
mode = FOLLOW_MODE;
}
break;
case(FOLLOW_MODE):
/* Siga a parede que foi encontrada */
follow_wall();
break;
}
}
}
/* O robo anda em linha reta ate encontrar uma parede e
entao passa a seguir a parede estando com ela sempre
em seu lado direito */
int main() {
unsigned short *dist[16];
/* inicia no modo busca-parede */
mode = 0;
/* cria proximity call back para quando estiver proximo da parede */
register_proximity_callback(3, MIM_DIST, found);
register_proximity_callback(4, MIM_DIST, found);
/* comeca andando em linha reta */
set_motors_speed(SPEED, SPEED);
/* enquanto estiver no modo busca-parede */
while(mode == 0);
/* passa a andar para esquerda */
set_motors_speed(HSPEED, 0);
/* enquanto nao estiver de lado para parede */
do {
read_sonar(3, dist[3]);
read_sonar(4, dist[4]);
} while(*(dist[3]) <= MIM_DIST && *(dist[4]) <= MIM_DIST);
/* volta a andar para frente */
set_motors_speed(SPEED, SPEED);
dir = 0;
/* fica seguindo a parede */
while(mode) {
read_sonar(7, dist[7]);
read_sonar(8, dist[8]);
if ( *(dist[7]) <= MIM_DIST || *(dist[8]) <= MIM_DIST ) {
if(dir != 2) {
set_motor_speed(1, HSPEED);
}
} else if ( *(dist[7]) >= MAX_DIST || *(dist[8]) >= MAX_DIST ) {
if(dir != 1) {
set_motor_speed(0, HSPEED);
}
} else {
if(dir != 0) {
set_motors_speed(SPEED, SPEED);
}
}
}
return 0;
}
void follower(){
while(act <= 0);
pos_t leader_position;
leader_position.x = robot.x + 40*sin(robot.t*M_PI/180.0) + act_val.dist*sin(act_val.angle*M_PI/180.0);
leader_position.y = robot.y + 40*cos(robot.t*M_PI/180.0) + act_val.dist*cos(act_val.angle*M_PI/180.0);
if(robot_rank >0){
printf("[STATUS] Action received, starting to follow\n");
if(act_val.dist < 40 || (act_val.angle > robot.t + 90 && act_val.angle < robot.t + 270 )){
go_to_position_interruptible(leader_position, 40, 2*act_val.speed, 0) ; //now it doers not avoid obstacles
}else{
//must be done with go_to_postion_interruptible too...
leader_position.x = robot.x + 40*sin(robot.t*M_PI/180.0);
leader_position.y = robot.y + 40*cos(robot.t*M_PI/180.0);
//printf("***************************** %d %d\n",robot.x,robot.y);
go_to_position_interruptible(leader_position,0,2*act_val.speed,0);
//printf("****************************** %d %d\n",robot.x,robot.y);
if(!cancel){
leader_position.x = robot.x + (act_val.dist-40)*sin(act_val.angle*M_PI/180.0);
leader_position.y = robot.y + (act_val.dist-40)*cos(act_val.angle*M_PI/180.0);
go_to_position_interruptible(leader_position, 0, 2*act_val.speed, 0);
set_motors_speed(NORMAL_SPEED);
turn_absolute_fb(act_val.angle, get_cal_location(robot.x,robot.y));
robot.t = act_val.angle;
}
}
}
cancel = 0;
act--;
}
void receiveROSData(struct ROSDataDef *msg)
{
Receive_length = 0;
if ( msg->ID == 'm' )
{
set_motors_speed(msg->data1, msg->data2);
}
}
int get_ball(){
int tries = 2; //number of tries when scanning to find the ball
int found = 0;
int angle;
int dist;
while(tries > 0){
found = scan_for_ball();
if(found == 1){
// Min point found
if(min.d <120){
// Grab the ball
set_motors_speed(NORMAL_SPEED);
angle = min.d * 2.1 - 67;
run_relative(angle);
while(!command_finish());
dist = (angle*18.0/360);
robot.x += dist * sin(robot.t);
robot.y += dist * cos(robot.t);
//add check that the ball is really there
tries = (grab_and_check())? 0: 2;
}else{
// Avvicinati
set_motors_speed(NORMAL_SPEED);
angle = (int)((min.d * 2.1 -67)*3/4);
run_relative(angle);
dist = (angle*18.0/360);
robot.x += dist * sin(robot.t);
robot.y += dist * cos(robot.t);
while(!command_finish());
}
}else{
// Minimum not found
//set_motors_speed(NORMAL_SPEED);
//run_relative(0);
//while(!command_finish());
tries--;
}
}
set_light(LIT_LEFT, LIT_OFF);
set_light(LIT_RIGHT, LIT_OFF);
//update robot angle
robot.t = get_gyro_value();
return found;
}
/* Segue lateralmente a parede */
void follow_wall() {
short dist1, dist14, dist3, dist4;
/* Se o sonar 1 estiver lendo uma distância muito alta vira à esquerda */
read_sonar(1, &dist1);
if(dist1 > THRESHOLD ) {
set_motors_speed(10, 2);
busy_wait(300000);
set_motors_speed(0, 0);
}
/* Se o sonar 1 estiver lendo uma distância muito baixa vira à direita */
else if(dist1 < THRESHOLD - ACCEPTABLE_DIFFERENCE) {
set_motors_speed(2, 10);
busy_wait(300000);
set_motors_speed(0, 0);
}
/* Está a uma boa distância da parede */
else {
set_motors_speed(10, 10);
busy_wait(300000);
}
/* Se houver uma parede à frente rodar até evitar a parede */
read_sonar(3, &dist3);
while(dist3 < THRESHOLD) {
set_motors_speed(0, 10);
busy_wait(1000);
set_motors_speed(0, 0);
read_sonar(3, &dist3);
}
}
/* In order to avoid a wall */
void turn_right() {
set_motors_speed(0, DEFAULT_SPEED/2);
/* Keeps turning until 90 degrees was made
* according to DEFAULT_INT */
delay(DEFAULT_INT);
forward();
}
/* Posiciona a lateral esquerda do robô paralelamente à parede */
int set_position_to_left() {
short dist0, dist14;
/* Roda o robo até encontrar uma parede à esquerda */
set_motors_speed(0, 10);
/* Se a distância do sonar da esquerda foi menor que o limiar + erro */
/* é porque encontrou uma parede. */
read_sonar(0, &dist0);
read_sonar(14, &dist14);
if(dist0 < THRESHOLD + ACCEPTABLE_ERROR && dist14 < THRESHOLD + ACCEPTABLE_ERROR) {
/* O robo está lado a lado com a parede.
/* Pare-o */
set_motors_speed(0, 0);
return 1;
}
/* Não conseguiu encontrar parede */
return 0;
}
/* main function */
int main() {
int i;
/* register_proximity_callback(3, LIMIAR, right); */
/* register_proximity_callback(4, LIMIAR, left); */
set_motors_speed(SPEED,SPEED);
/* add_alarm(stop, 10); */
while(1) {
for(i = 0; i < 1000000; i++)
turn90();
}
return 0;
}
void move_simple(float distance, int motor_speed){
float distance_remaining = distance;
bool move_is_done = false;
set_motors_speed(motor_speed);
while(!move_is_done){
THREAD_MSleep(__PID_TIME_INTERVAL__);
int encoder_read_right = ENCODER_Read(ENCODER_RIGHT);
distance_remaining = distance_remaining - (encoder_read_right * __DISTANCE_PER_ENCODER__);
if(distance_remaining < 0){
move_is_done = true;
}
}
}
void set_expected_encoder_reads(int motor_speed){
int average_encoder_read_right = 0, average_encoder_read_left = 0, total_encoder_read_right = 0, total_encoder_read_left = 0;
int calculation_period = 3000;
prompt_bumpers();
reset_encoders();
set_motors_speed(motor_speed);
LCD_ClearAndPrint("Testing and setting encoder expected reads for %d ms...", calculation_period);
THREAD_MSleep(calculation_period);
total_encoder_read_right += ENCODER_Read(ENCODER_RIGHT);
total_encoder_read_left += ENCODER_Read(ENCODER_LEFT);
stop();
EXPECTED_ENCODER_READ_LEFT = (int)floor(total_encoder_read_left/(calculation_period/__PID_TIME_INTERVAL__));
EXPECTED_ENCODER_READ_RIGHT = (int)floor(total_encoder_read_right/(calculation_period/__PID_TIME_INTERVAL__));
LCD_ClearAndPrint("Done!\nExpected encoder reads are:\nRight: %d\nLeft: %d\nPress any bumper to continue!",EXPECTED_ENCODER_READ_RIGHT,EXPECTED_ENCODER_READ_LEFT);
prompt_bumpers();
reset_encoders();
}
int scan_for_ball(){
int dist;
int found=0;
min.d =2500;
set_light(LIT_RIGHT, LIT_GREEN);
set_motors_speed(SLOW_SPEED);
turn_relative(90);
while(!command_finish()){
update_sensor(SENSOR_US);
found |= check_ball(get_sensor_value(SENSOR_US), get_gyro_value());
}
//update robot angle
robot.t += 90;
turn_relative(-180);
while(!command_finish()){
update_sensor(SENSOR_US);
found |= check_ball(get_sensor_value(SENSOR_US), get_gyro_value());
}
//update robot angle
robot.t -= 180;
if(found == 1){
// Durante la scansione ho trovato un minimo
sleep(1);
printf("Start: %d -> Stop teorico: %d, Distanza teorica: %d\n", get_gyro_value(), min.t, min.d);
turn_relative_fb(min.t-get_gyro_value());
//update robot angle
robot.t = get_gyro_value();
printf("Stop reale: %d, Distanza reale: %d\n", robot.t, dist);
}else{
// Durante la scansione non ho trovato minimi
turn_relative_fb(90);
//while(!command_finish());
//update robot angle
robot.t = get_gyro_value();
}
set_light(LIT_RIGHT, LIT_OFF);
return found;
}
//this function makes the robot avoid the obstacle running for a small distance in another direction
//this direction depends on the current angle and position
void obstacle_avoid(){
int quadrant = robot.t / 90;
int right_left_n = robot.x / BORDER_X_MAX/2;
int up_down_n = robot.y / BORDER_Y_MAX/2;
int angles[4][2][2];
//first quadrant
angles[0][0][0]=45; //down left
angles[0][0][1]=-45; //down right
angles[0][1][0]=45; //up left
angles[0][1][1]=-45; //up right
//second quadrant
angles[1][0][0]=-45; //down left
angles[1][0][1]=-45; //down right
angles[1][1][0]=45; //up left
angles[1][1][1]=45; //up right
//third quadrant
angles[2][0][0]=-45; //down left
angles[2][0][1]=45; //down right
angles[2][1][0]=-45; //up left
angles[2][1][1]=45; //up right
//fourth quadrant
angles[3][0][0]=45; //down left
angles[3][0][1]=45; //down right
angles[3][1][0]=-45; //up left
angles[3][1][1]=-45; //up right
int angle=(robot.t+angles[quadrant][up_down_n][right_left_n])%360;
if(robot_rank ==0 && send_action_flag) send_action(next,angle,40,NORMAL_SPEED/2);
set_motors_speed(NORMAL_SPEED);
turn_absolute_fb(angle,get_cal_location(robot.x,robot.y));
robot.t=angle;
run_relative((int)40*360.0/18);
while(!command_finish());
robot.x= robot.x + 40*sin(robot.t*M_PI/180);
robot.y= robot.y + 40*cos(robot.t*M_PI/180);
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
}
int main(int argc,char** argv){
int mot_value[] = {200, 400, 600, 1000, 1200};
int us_value[NUM_TESTS];
int i;
// Init
init_gui();
init_motors();
init_sensor();
// US setup
set_motors_speed(NORMAL_SPEED);
printf("mot\tus\n");
for(i=0; i<NUM_TESTS; i++){
us_value[i] = do_test(mot_value[i]);
}
return 0;
}
void move(float distance, int motor_speed){
float distance_remaining = distance;
int encoder_error_right = 0, encoder_error_left = 0, total_error_right = 0, total_error_left = 0;
bool move_is_done = false;
set_motors_speed(motor_speed);
while(!move_is_done){
if(read_bumpers()){
stop_and_wait(250);
}else{
THREAD_MSleep(__PID_TIME_INTERVAL__);
int encoder_read_right = ENCODER_Read(ENCODER_RIGHT);
int encoder_read_left = ENCODER_Read(ENCODER_LEFT);
total_error_right += encoder_error_right;
total_error_left += encoder_error_left;
encoder_error_right = pid(encoder_read_right, MOTOR_RIGHT, EXPECTED_ENCODER_READ_RIGHT, encoder_error_right, total_error_right, motor_speed, __PID_PROPORTIONAL_GAIN__, __PID_INTEGRAL_GAIN__, __PID_DERIVATIVE_GAIN__, __PID_TIME_INTERVAL__);
encoder_error_left = pid(encoder_read_left, MOTOR_LEFT, EXPECTED_ENCODER_READ_LEFT, encoder_error_left, total_error_left, motor_speed, __PID_PROPORTIONAL_GAIN__, __PID_INTEGRAL_GAIN__, __PID_DERIVATIVE_GAIN__, __PID_TIME_INTERVAL__);
distance_remaining = distance_remaining - (encoder_read_right * __DISTANCE_PER_ENCODER__);
if(distance_remaining < 0){
move_is_done = true;
}
}
}
}
void turn90() {
int time = get_time();
set_motors_speed(0,HSPEED);
while(get_time() < time + TURN_TIME);
set_motors_speed(SPEED,SPEED);
}
void forward() {
set_motors_speed(DEFAULT_SPEED, DEFAULT_SPEED);
}
//return REACHED if position reached, OBSTACLE if stopped because of obstacle A CUSTOM STOP MESSAGE MUST BE SENT, CUSTOM_STOP_MESSAGE if stopped because a custom message was sent
//robot position is updated
//position is reached running for distance in steps <=200 (first the dist%200 than n steps dist/200)
//it sleeps 1s after each step
int go_to_position(pos_t target_pos, int margin, int speed){
int32_t tacho_dist, start_tacho, end_tacho;
int angle, dist;
int i;
if(robot_rank == 0) while(wait);
compute_polar_coord(target_pos, &dist, &angle,margin);
target_pos.x = robot.x + dist*sin(angle*M_PI/180);
target_pos.y = robot.y + dist*cos(angle*M_PI/180);
//start movement, in two steps if bigger than 200
int div = dist/200;
int mod = dist%200;
int step;
for(i = 0; i < div+1 ; i++){
if(i == div) step = mod;
else step = 200;
if(robot_rank == 0 && send_action_flag) send_action(next, angle, step, speed/2);
//vado alla posizione
set_motors_speed(SLOW_SPEED);
turn_absolute_fb(angle, get_cal_location(robot.x,robot.y));
//aggiorno posizione final;
robot.t = angle;//get_gyro_value();
printf("robot.t = %d\n",robot.t);
//save initial tacho counts
start_tacho=get_motorR_position();
set_motors_speed(speed);
run_relative((int)(step*360.0/18));
while(!command_finish()){
if(cancel){
//cancel = 0;
run_relative(0);
end_tacho = get_motorR_position();
tacho_dist = end_tacho-start_tacho;
dist = tacho_dist/20.0;
robot.x = robot.x + dist*sin(robot.t*M_PI/180);
robot.y = robot.y + dist*cos(robot.t*M_PI/180);
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
if(send_action_flag) send_cancel(next, dist);
// Send to ourself an action in order came closer
act_val.dist = cancel - dist;
act_val.angle = robot.t;
act++;
return CUSTOM_STOP_MESSAGE;
}
if(obstacle()){
run_relative(0);
end_tacho = get_motorR_position();
tacho_dist = end_tacho-start_tacho;
dist = tacho_dist/20.0;
printf("step %d dist %d\n",step,dist);
if(step-dist < STOP_DIST*18/360.0){
set_motors_speed(speed);
run_relative((int)((step-dist)*360.0/18));
return REACHED;
}
printf("tacho dist %ld dist %ld\n",tacho_dist,dist);
robot.x = robot.x + dist*sin(robot.t*M_PI/180);
robot.y = robot.y + dist*cos(robot.t*M_PI/180);
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
if(send_action_flag) send_cancel(next,dist);
// HERE WE MUST SEND A CUSTOM MESSAGE STOP TO FOLLOWING ROBOT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// HERE WE MUST SEND A CUSTOM MESSAGE STOP TO FOLLOWING ROBOT!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
return OBSTACLE;
}
}
sleep(1);
}
//update pos
robot.x = target_pos.x;
robot.y = target_pos.y;
printf("\n\n>>>>>>>>pos %d %d %d\n\n",robot.x,robot.y,robot.t);
return REACHED;
}
void drop_the_ball(){
set_motors_speed(NORMAL_SPEED);
set_motor_arm_position(MOTOR_ARM_DOWN);
while(!command_finish());
sleep(5);
}
