All the lab works are here!
Dependencies: ISR_Mini-explorer mbed
Fork of VirtualForces by
Diff: main.cpp
- Revision:
- 29:224e9e686f7b
- Parent:
- 28:f884979a02fa
- Child:
- 30:95d8d3e2b81b
--- a/main.cpp Wed Apr 26 10:55:54 2017 +0000 +++ b/main.cpp Wed Apr 26 16:58:57 2017 +0000 @@ -8,8 +8,8 @@ void fill_initial_log_values(); //generate a position randomly and makes the robot go there while updating the map void randomize_and_map(); -//make the robot do a pi flip -void do_a_flip(); +//make the robot do a pi/2 flip +void do_half_flip(); //go the the given position while updating the map void go_to_point_with_angle(float target_x, float target_y, float target_angle); //Updates sonar values @@ -126,11 +126,14 @@ Y=DEFAULT_Y; - for (int i = 0; i<20; i++) { - randomize_and_map(); - wait(2); + for (int i = 0; i<10; i++) { + randomize_and_map(); + //print_final_map(); + print_final_map_with_robot_position(); + } + while(1){ print_final_map(); - //print_final_map_with_robot_position(); + print_final_map_with_robot_position(); } } @@ -163,14 +166,17 @@ go_to_point_with_angle(target_x, target_y, target_angle); } -void do_a_flip(){ + +void do_half_flip(){ Odometria(); - float theta_plus_pi=rad_angle_check(theta+pi); - while(abs(theta_plus_pi-theta)>0.01){ - leftMotor(1,500); - rightMotor(1,500); - wait(0.2); + float theta_plus_h_pi=theta+pi/2;//theta is between -pi and pi + if(theta_plus_h_pi > pi) + theta_plus_h_pi=-(2*pi-theta_plus_h_pi); + leftMotor(0,100); + rightMotor(1,100); + while(abs(theta_plus_h_pi-theta)>0.05){ Odometria(); + // pc.printf("\n\r diff=%f", abs(theta_plus_pi-theta)); } leftMotor(1,0); rightMotor(1,0); @@ -203,34 +209,15 @@ //pc.printf("\n\r rightMm=%f", rightMm); //if in dangerzone - if(frontMm < 100 || leftMm <100 || rightMm <100){ + if(frontMm < 120 || leftMm <120 || rightMm <120){ leftMotor(1,0); rightMotor(1,0); update_sonar_values(leftMm, frontMm, rightMm); - //TODO Giorgos maybe you can also test the do_a_flip() function + //TODO Giorgos maybe you can also test the do_half_flip() function Odometria(); //do a flip TODO keep_going=false; - target_angle=theta+pi; - target_y=Y; - target_x=X; - alpha = atan2((target_y-Y),(target_x-X))-theta; - alpha = atan(sin(alpha)/cos(alpha)); - rho = dist(X, Y, target_x, target_y); - beta = -alpha-theta+target_angle; - //keep going until the flip is done - do{ - Odometria(); - dt = t.read(); - t.reset(); - t.start(); - compute_angles_and_distance(target_x, target_y, target_angle); //Compute the angles and the distance from target - compute_linear_angular_velocities(); //Using the angles and distance, compute the velocities needed (linear & angular) - leftMotor(1,angular_left); - rightMotor(1,angular_right); - //Timer stuff - t.stop(); - }while(d2>1 && (abs(target_angle-theta)>0.01)); + do_half_flip(); }else{ //if not in danger zone continue as usual update_sonar_values(leftMm, frontMm, rightMm); @@ -337,12 +324,12 @@ for (int x= 0; x<NB_CELL_WIDTH; x++) { currProba=log_to_proba(map[x][y]); if ( currProba < 0.5) { - pc.printf(" 0 "); + pc.printf(" "); } else { if(currProba==0.5) pc.printf(" . "); else - pc.printf(" + "); + pc.printf(" X "); } } pc.printf("\n\r"); @@ -374,12 +361,12 @@ else{ currProba=log_to_proba(map[x][y]); if ( currProba < 0.5) - pc.printf(" 0 "); + pc.printf(" "); else{ if(currProba==0.5) pc.printf(" . "); else - pc.printf(" + "); + pc.printf(" X "); } } }