Merge

includes/config.h

@@ -46,15 +46,16 @@
 //Grabing constants
 #define GRAB_POSITION 23
 #define MAX_GRABBING_SPEED 550
-#define RELEASING_SPEED 20
+#define RELEASING_SPEED 100
 #define INIT_GRAB_POSITION -15
 
 
 //Wheel motors constants
-#define MAX_WHEEL_SPEED 1050
+#define MAX_WHEEL_SPEED 750
 #define WHEEL_DIAMETER 5.6
 #define STEPLENGTH 60
 #define TURNING_SPEED 30
+#define HIGH_TURNING_SPEED 200
 
 //Positions of of fixed elements
 #define CENTER_POSITION 0
@@ -239,7 +240,7 @@
 
 
 
-#define BALL_CATCH_DISTANCE 4
+#define BALL_CATCH_DISTANCE 6
 #define BALL_FOUND 0
 #define BALL_NOT_FOUND -1
 

src/first_runner.c

@@ -91,16 +91,18 @@ int beginner_small_stadium(state *s, mainpos *p)
     //Go to center
     log_this(s,"\n[%s:beginner_small_stadium] Going to ball area\n", __FILE__);
     go_to_pos(s, p->s_fr_ballarea);
-    sleep(2);
+    getchar();
+    //sleep(2);
 
     //release ball
     log_this(s,"\n[%s:beginner_small_stadium] Releasing ball\n", __FILE__);
-    release(s, RELEASING_SPEED);
-    send_message(s, MSG_BALL, s->ally);
+    //release(s, RELEASING_SPEED);
+    //send_message(s, MSG_BALL, s->ally);
 
 	//Go back a little
     log_this(s, "\n[%s:beginner_small_stadium] Going back a little\n", __FILE__);
 	go_straight(s, MAX_WHEEL_SPEED, -20);
+    getchar();
 
 	//Go to ending position
     log_this(s, "\n[%s:beginner_small_stadium] Going to the end\n", __FILE__);
@@ -126,11 +128,14 @@ int beginner_large_stadium(state *s, mainpos *p)
     //Dodge first obstacle
     log_this(s, "\n[%s:beginner_large_stadium] Dodging first obstacle \n",__FILE__);
 	go_to_pos(s, p->l_fr_dodgefirst);
+    getchar();
 
-    //Go to center and a 180
+    //Go to center and do a 180
     log_this(s, "\n[%s:beginner_large_stadium] Going to the center\n",__FILE__);
     go_to_pos(s, p->l_fr_ballarea);
-    turn(s, TURNING_SPEED, 180);
+    getchar();
+    turn(s, HIGH_TURNING_SPEED, 180);
+    getchar();
 
     //Release ball
     usleep(100000);

src/init.c

@@ -58,7 +58,7 @@ int init_robot(state *s)
     }
 
     init_pos(s);
-
+/*
     //Set game started
     pthread_mutex_lock(&(s->mutexGameStarted));
     s->gameStarted = IMMOBILE;
@@ -70,7 +70,7 @@ int init_robot(state *s)
         log_this(s, "[%s] Unable to create position thread\n", __FILE__);
         nice_exit(s, EXIT_FAILURE);
     }
-
+*/
     return 0;
 }
 

src/main.c

@@ -77,25 +77,33 @@ int main(int argc, char *argv[])
             //Run beginner
             case 1:
                 init_robot(s);
+                s->side = -1;
+                init_main_positions(s,p);
                 beginner_small_stadium(s,p);
-                look_for_ball(s);
                 break;
             //Test finisher
             case 2:
                 init_robot(s);
-                finisher_small_stadium(s, p);
+                s->side = -1;
+                init_main_positions(s,p);
+                beginner_large_stadium(s,p);
                 break;
             //Quicks tests
             case 3:
                 init_robot(s);
-                printf("cross compilation working");
-                //beginner_small_stadium(s,p);
-                init_robot(s);
-                look_for_ball(s);
+                look_for_ball_mecanical(s);
+                catch_ball(s);
                 break;
             case 4:
                 main2(0, NULL);
                 break;
+            case 5:
+                init_robot(s);
+                position pos = {-20, 10};
+                position pos2 = {-10, 80};
+                update_pos(s,pos);
+                go_to_pos(s, pos2);
+                break;
         }
     }
     //Close external ressources

src/motors.c

@@ -34,8 +34,8 @@ void init_motors(state *s){
     //Ramp smoothly to max speed
     ev3_set_ramp_up_sp(s->rightmotor, 500);
     ev3_set_ramp_up_sp(s->leftmotor, 500);
-    ev3_set_ramp_down_sp(s->rightmotor, 1000);
-    ev3_set_ramp_down_sp(s->leftmotor, 1000);
+    ev3_set_ramp_down_sp(s->rightmotor, 500);
+    ev3_set_ramp_down_sp(s->leftmotor, 500);
 
 }
 //Grab control functions
@@ -76,7 +76,7 @@ int release(state *s, int speed)
         log_this(s, "[%s:release] Releasing failed already in release position\n", __FILE__);
         return -1;
     }
-    ev3_set_ramp_up_sp(s->grabmotor, 10000);
+    ev3_set_ramp_up_sp(s->grabmotor, 500);
     ev3_set_speed_sp(s->grabmotor, speed);
     ev3_set_position_sp(s->grabmotor, -7);
     ev3_command_motor_by_name(s->grabmotor, "run-to-abs-pos");
@@ -100,7 +100,7 @@ int release(state *s, int speed)
 void set_wheels_speed(state *s, int speed){
     ev3_set_speed_sp(s->leftmotor, speed);
     ev3_set_speed_sp(s->rightmotor, speed);
-    //log_this(s, "[%s] Wheels' speed set to %d \n", __FILE__, speed);
+    log_this(s, "[%s] Wheels' speed set to %d \n", __FILE__, speed);
 }
 
 /**
@@ -111,7 +111,7 @@ void set_wheels_speed(state *s, int speed){
 void set_wheels_time(state *s, int time){
     ev3_set_time_sp(s->leftmotor, time);
     ev3_set_time_sp(s->rightmotor, time);
-    //log_this(s, "[%s] Wheels' time set to %d \n", __FILE__, time);
+    log_this(s, "[%s] Wheels' time set to %d \n", __FILE__, time);
 }
 
 /**
@@ -122,7 +122,7 @@ void set_wheels_time(state *s, int time){
 void set_wheels_pos(state *s, int pos){
     ev3_set_position_sp(s->leftmotor, pos);
     ev3_set_position_sp(s->rightmotor, pos);
-    //log_this(s, "[%s] Wheels' position set to %d \n", __FILE__, pos);
+    log_this(s, "[%s] Wheels' position set to %d \n", __FILE__, pos);
 }
 
 /**
@@ -163,7 +163,7 @@ int wheels_run_time(state *s, int speed, int time){
 * @return 0 if everything is alright
 */
 int wheels_run_pos(state *s, int speed, int pos){
-    //log_this(s, "[%s] Wheels running to relative position %d...\n", __FILE__, pos);
+    log_this(s, "[%s] Wheels running to relative position %d...\n", __FILE__, pos);
     set_wheels_speed(s, speed);
     set_wheels_pos(s, pos);
     command_wheels(s, RUN_TO_REL_POS);
@@ -197,18 +197,17 @@ int wheels_run_distance(state *s, int speed, int distance){
 int go_straight(state *s, int speed, int distance){
     log_this(s, "[%s:go_straight] Going straight for%d cm\n", __FILE__, distance);
     update_angle(s,gyro_angle(s));
-
     int correctDistance = distance*floor(100/97.5); // based on tests : 2,5cm error for 100cm
     // We divide the wanted distance in steps od STEPLENGTH
     // After each step we correct the direction of the robot
     do
     {
         int currentDistance = (correctDistance > STEPLENGTH) ? STEPLENGTH : correctDistance;
         log_this(s, "[%s:go_straight] Next step : %d\n",__FILE__, currentDistance);
-        wheels_run_distance(s, speed, currentDistance);
-        turn(s, TURNING_SPEED, is_running_in_correct_angle(s));
-        correctDistance -= currentDistance;
-
+    	wheels_run_distance(s, speed, currentDistance);
+        usleep(500000);
+    	turn(s, TURNING_SPEED, is_running_in_correct_angle(s));
+    	correctDistance -= currentDistance;
     } while (correctDistance > 0);
 
     return 0;

src/robot.c

@@ -32,11 +32,11 @@ int catch_ball(state* s)
     }
 
     //Run to the ball
-    go_straight(s, MAX_WHEEL_SPEED/10, distanceBall - BALL_CATCH_DISTANCE);
+    go_straight(s, MAX_WHEEL_SPEED/10, distanceBall + BALL_CATCH_DISTANCE);
 
     //Close the hook
     grab(s, MAX_GRABBING_SPEED);
-
+    sleep(1);
     int returnValue = is_ball_present(s);
     if(returnValue == BALL_FOUND)
     {

src/utilspositions.c

@@ -141,26 +141,28 @@ int sign(int a){
 **/
 void init_main_positions(state *s, mainpos *p){
 
-    int side = s->side;
-
+    int side = ((s->side==1)? 1:-1);
+    printf("%d\n",side);
     position s_fr_init={.x = S_FR_S_0_X,.y = S_FR_S_0_Y +  WHEELS_TO_END};
     position s_fr_ballareaposition = {.x = S_BA_0_X - 5,.y = S_BA_0_Y - WHEELS_TO_END + 5};
     position s_fr_endingposition = {.x = S_FR_E_1_X,.y = S_FR_E_1_Y};
+
     position l_fr_init = {.x = side*(L_FR_S_0_X + WHEELS_TO_END), .y = L_FR_S_0_Y + WHEELS_TO_END};
     position l_fr_dodgefirst = {.x = side*(L_O1_2_X + BIG_ARENA_MAX_X)/2, .y = L_O1_2_Y + L_FR_S_0_Y};
-    position l_fr_center = {.x = side*(L_BA_0_X + L_FR_S_0_X/2), .y = L_BA_0_Y + L_FR_S_0_Y};
+    position l_fr_center = {.x = side*(L_BA_1_X + ROBOT_WIDTH/2), .y = L_BA_1_Y + WHEELS_TO_END/2 };
     position l_fr_dodgesecond = {.x = side*L_FR_E_0_X , .y = L_O2_0_Y};
-    position l_fr_ending = {.x = side*L_FR_E_1_X, .y = L_FR_E_1_Y};
+    position l_fr_ending = {.x = side*(L_FR_E_1_X + ROBOT_WIDTH), .y = L_FR_E_1_Y };
 
 
     position s_sr_init={.x = S_SR_S_0_X + ROBOT_WIDTH,.y = S_SR_S_0_Y +  WHEELS_TO_END};
     position s_sr_ballareaposition = {.x = S_BA_1_X - 5,.y = S_BA_1_Y + WHEELS_TO_END + 5};
     position s_sr_endingposition = {.x = S_SR_E_1_X + ROBOT_WIDTH, .y = S_SR_E_1_Y - WHEELS_TO_END};
+
     position l_sr_init = {.x = side*(L_FR_S_0_X + WHEELS_TO_END), .y = L_FR_S_0_Y + WHEELS_TO_END};
     position l_sr_dodgefirst = {.x = side*(L_O1_2_X + BIG_ARENA_MAX_X)/2, .y = L_O1_2_Y + L_FR_S_0_Y};
     position l_sr_center = {.x = side*(L_BA_0_X + L_FR_S_0_X/2), .y = L_BA_0_Y + L_FR_S_0_Y};
     position l_sr_dodgesecond = {.x = side*L_FR_E_0_X , .y = L_O2_0_Y};
-    position l_sr_ending = {.x = side*L_FR_E_1_X, .y = L_FR_E_1_Y};
+    position l_sr_ending = {.x = side*L_FR_E_1_X + 1.5*ROBOT_WIDTH, .y = L_FR_E_1_Y + WHEELS_TO_END};
 
     p->s_fr_init=s_fr_init;
 	p->s_fr_ballarea=s_fr_ballareaposition;