More work on locks

Makefile

@@ -6,7 +6,7 @@ BIN_DIR = bin/
 C_FILES = $(wildcard src/*.c)
 OBJ_FILES = $(addprefix $(OBJ_DIR),$(notdir $(C_FILES:.c=.o)))
 
-CFLAGS = $(INC_DIR) -Wall
+CFLAGS = $(INC_DIR) -Wall -g
 LDFLAGS = -lm -lbluetooth -pthread
 
 bin/testOsRobot: ev3c.a $(OBJ_FILES)

includes/init.h

@@ -6,6 +6,7 @@
 
 int init_robot(state *s);
 void nice_exit(state *s, int exitState);
+int init_locks(state *s);
 void signal_handler(int signalNumber);
 
 #endif

src/first_runner.c

@@ -20,27 +20,20 @@ int game_wrapper(state *s, mainpos *p)
     //If we have to wait until it's our turn
     if (s->role == ROLE_SECOND)
     {
-        //Let's wait for the starting message
-        pthread_mutex_lock(&(s->mutexSockUsage));
-
         char buf[100];
         int returnValue;
         while(1)
         {
             returnValue  = read_message_from_server(s, buf);
             if (returnValue == -1)
-            {
-                pthread_mutex_unlock(&(s->mutexSockUsage));
                 continue;
-            }
 
             if (buf[HEADER_TYPE] == MSG_NEXT)
                 break;
             else if (buf[HEADER_TYPE] == MSG_BALL) //Interpret the ball message
                 save_ball_position(s, buf);
 
         }
-        pthread_mutex_unlock(&(s->mutexSockUsage));
     }
 
     int (*strategy)(state*,mainpos*);
@@ -62,14 +55,27 @@ int game_wrapper(state *s, mainpos *p)
     s->gameStarted = TRAVELLING;
     pthread_mutex_unlock(&(s->mutexGameStarted));
 
-    init_main_positions(s, p);
-    strategy(s, p);
+    //init_main_positions(s, p);
+    //strategy(s, p);
+    printf("Test\n");
+    getchar();
+
+    printf("Test %d\n",s->ally);
+    position a = {1, 2};
+    s->curPos = a;
+    sleep(1);
+    a = (position) {2,3};
+    update_pos(s, a);
+    sleep(1);
+    a = (position) {.x = 4, .y = 3};
+    update_pos(s,a);
+
+    send_message(s, MSG_NEXT, s->ally);
 
     pthread_mutex_lock(&(s->mutexGameStarted));
     s->gameStarted = IMMOBILE;
     pthread_mutex_unlock(&(s->mutexGameStarted));
 
-
     return 0;
 }
 

src/init.c

@@ -50,6 +50,13 @@ int init_robot(state *s)
     }
 
     //Init position
+    returnValue = init_locks(s);
+    if (returnValue != 0)
+    {
+        log_this(s, "[%s] Unable to init locks\n", __FILE__);
+        return 1;
+    }
+
     init_pos(s);
 
     //Set game started
@@ -67,6 +74,19 @@ int init_robot(state *s)
     return 0;
 }
 
+int init_locks(state *s)
+{
+    if (pthread_mutex_init(&(s->mutexPosition), NULL) != 0)
+        return -1;
+
+    if (pthread_mutex_init(&(s->mutexGameStarted), NULL) != 0)
+        return -1;
+
+    if (pthread_mutex_init(&(s->mutexSockUsage), NULL) != 0)
+        return -1;
+
+    return 0;
+}
 
 
 /**

src/main.c

@@ -25,9 +25,6 @@ void game()
 {
     //Game function o_OOOOO_o
 
-    //Let's wait for the starting message
-    pthread_mutex_lock(&(s->mutexSockUsage));
-
     char buf[100];
     int returnValue;
     while(1)
@@ -40,8 +37,6 @@ void game()
             break;
     }
 
-    pthread_mutex_unlock(&(s->mutexSockUsage));
-
     load_game_params(s, buf);
     game_wrapper(s, p);
 

src/motors.c

@@ -203,11 +203,11 @@ int go_straight(state *s, int speed, int distance){
     // After each step we correct the direction of the robot
     do
     {
-    	int currentDistance = (correctDistance > STEPLENGTH) ? STEPLENGTH : correctDistance;
+        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);
+        turn(s, TURNING_SPEED, is_running_in_correct_angle(s));
+        correctDistance -= currentDistance;
 
     } while (correctDistance > 0);
 
@@ -221,16 +221,19 @@ int go_straight(state *s, int speed, int distance){
  * @param  position    Desired x,y position to go
  * @return          0 if everything is allright
  */
-int go_to_pos(state *s, position desiredposition){
+int go_to_pos(state *s, position desiredposition)
+{
     log_this(s, "\n[%s:go_to_pos] Going to x=%d , y=%d\n", __FILE__, desiredposition.x, desiredposition.y);
-	s->wantedPos=desiredposition;
-	go_to_pos_incomplete(s, s->wantedPos);
-	int pos_distance_diff = compute_distance(compute_relative_position(s->curPos,s->wantedPos));
-	if(pos_distance_diff>ERROR_DISTANCE_MARGIN){
-		log_this(s, "\n[%s:go_to_pos] Go to pos has finished with a significant distance error: err=%d... correct position\n", __FILE__,pos_distance_diff);
-		go_to_pos_incomplete(s, desiredposition);
-	}
-	return 0;
+    s->wantedPos=desiredposition;
+
+    go_to_pos_incomplete(s, s->wantedPos);
+
+    int pos_distance_diff = compute_distance(compute_relative_position(s->curPos,s->wantedPos));
+    if(pos_distance_diff>ERROR_DISTANCE_MARGIN){
+        log_this(s, "\n[%s:go_to_pos] Go to pos has finished with a significant distance error: err=%d... correct position\n", __FILE__,pos_distance_diff);
+        go_to_pos_incomplete(s, desiredposition);
+    }
+    return 0;
 }
 
 /**
@@ -239,25 +242,26 @@ int go_to_pos(state *s, position desiredposition){
  * @param  position    Desired x,y position to go
  * @return          0 if everything is allright
  */
-int go_to_pos_incomplete(state *s, position desiredposition){
-	log_this(s, "[%s:go_to_pos_incomplete] Go to pos departure destination  x=%d y=%d...\n", __FILE__, s->curPos.x,s->curPos.y);
-	log_this(s, "[%s:go_to_pos_incomplete] Go to pos desired destination  x=%d y=%d...\n", __FILE__, desiredposition.x,desiredposition.y);
-	position relativeposition=compute_relative_position(s->curPos,desiredposition);
-	int distancetodest=compute_distance(relativeposition);
-	log_this(s, "[%s:go_to_pos_incomplete] Distance relative to destination %d...\n", __FILE__, distancetodest);
-	int absoluteangle= -compute_angle(relativeposition);
-	log_this(s, "[%s:go_to_pos_incomplete] Absolute angle to destination (clockwise) to turn %d...\n", __FILE__, absoluteangle);
-	int relativeangle;
-	relativeangle=absoluteangle-s->angle;
+int go_to_pos_incomplete(state *s, position desiredposition)
+{
+    log_this(s, "[%s:go_to_pos_incomplete] Go to pos departure destination  x=%d y=%d...\n", __FILE__, s->curPos.x,s->curPos.y);
+    log_this(s, "[%s:go_to_pos_incomplete] Go to pos desired destination  x=%d y=%d...\n", __FILE__, desiredposition.x,desiredposition.y);
+    position relativeposition=compute_relative_position(s->curPos,desiredposition);
+    int distancetodest=compute_distance(relativeposition);
+    log_this(s, "[%s:go_to_pos_incomplete] Distance relative to destination %d...\n", __FILE__, distancetodest);
+    int absoluteangle= -compute_angle(relativeposition);
+    log_this(s, "[%s:go_to_pos_incomplete] Absolute angle to destination (clockwise) to turn %d...\n", __FILE__, absoluteangle);
+    int relativeangle;
+    relativeangle=absoluteangle-s->angle;
     log_this(s, "[%s:go_to_pos_incomplete] Relative angle to destination (clockwise) to turn %d...\n", __FILE__, relativeangle);
     int relativeAngleToTurnClockWise=clean_angle(relativeangle);
     log_this(s, "[%s:go_to_pos_incomplete] Relative cleaned angle to destination (clockwise) sent to turn function: %d...\n", __FILE__, relativeAngleToTurnClockWise);
 
     //need to be clockwise for the turn function so send - relative angle to the function
     turn(s, TURNING_SPEED, relativeAngleToTurnClockWise);
 
-	go_straight(s, MAX_WHEEL_SPEED, distancetodest);
-	return 0;
+    go_straight(s, MAX_WHEEL_SPEED, distancetodest);
+    return 0;
 }
 
 
@@ -335,15 +339,15 @@ int turn_imprecise(state *s, int speed, int angle){
  * @return the difference between the actual angle and the wanted angle, 0 if this difference is smaller than the error margin
  */
 int is_running_in_correct_angle(state *s){
-	int actualangle = gyro_angle(s);
-	//+- ERROR_M degrees is ok
+    int actualangle = gyro_angle(s);
+    //+- ERROR_M degrees is ok
     int angle_diff = clean_angle(s->angle - actualangle);
     //printf("Is running: %d\t%d\n", actualangle, angle_diff);
-	log_this(s, "[%s:is_running_correct_angle] Angle diff is %d\n", __FILE__, angle_diff);
-	if(abs(angle_diff) > ERROR_MARGIN){
-		return angle_diff;
-	}
-	return 0;
+    log_this(s, "[%s:is_running_correct_angle] Angle diff is %d\n", __FILE__, angle_diff);
+    if(abs(angle_diff) > ERROR_MARGIN){
+        return angle_diff;
+    }
+    return 0;
 }
 
 //Sweep motor
@@ -359,9 +363,9 @@ int is_running_in_correct_angle(state *s){
 int sweep(state *s, int speed, int angle)
 {
     log_this(s, "[%s:sweep] Sweeping for %d degrees\n", __FILE__, angle);
-	int cur_angle_sweep=ev3_get_position(s->sweepmotor);
-	int rel_sweep_angle = cur_angle_sweep + angle;
-	if (abs(rel_sweep_angle) > MAX_SWEEP_ANGLE) {
+    int cur_angle_sweep=ev3_get_position(s->sweepmotor);
+    int rel_sweep_angle = cur_angle_sweep + angle;
+    if (abs(rel_sweep_angle) > MAX_SWEEP_ANGLE) {
         log_this(s, "[%s:sweep] Sweep failed current sweep angle + desired angle exceed limit (%d(actual) --> %d(desired)) \n", __FILE__, cur_angle_sweep,cur_angle_sweep+ angle);
         return -1;
     }
@@ -431,15 +435,15 @@ int turn_compass(state *s, int speed, int angle){
  * @return the difference between the actual angle and the wanted angle, 0 if this difference is smaller than the error margin
  */
 int is_running_in_correct_angle_compass(state *s){
-	int actualangle = compass_angle(s);
-	//+- ERROR_M degrees is ok
+    int actualangle = compass_angle(s);
+    //+- ERROR_M degrees is ok
     int angle_diff = clean_angle(s->angle - actualangle);
     printf("Is running: %d\t%d\n", actualangle, angle_diff);
-	log_this(s, "[%s] Angle diff is %d\n", __FILE__, angle_diff);
-	if(abs(angle_diff) > ERROR_MARGIN){
-		return angle_diff;
-	}
-	return 0;
+    log_this(s, "[%s] Angle diff is %d\n", __FILE__, angle_diff);
+    if(abs(angle_diff) > ERROR_MARGIN){
+        return angle_diff;
+    }
+    return 0;
 }
 
 /**
@@ -454,9 +458,9 @@ int go_straight_compass(state *s, int speed, int distance){
     s->angle = compass_angle(s);
 
     // We divide the wanted distance in steps od STEPLENGTH
-	int nb_of_steps = distance / STEPLENGTH;
+    int nb_of_steps = distance / STEPLENGTH;
     int remaining_distance = distance % STEPLENGTH;
-	printf("STP: %d \t RD: %d\n", nb_of_steps, remaining_distance);
+    printf("STP: %d \t RD: %d\n", nb_of_steps, remaining_distance);
     int i;
     // After each step we correct the direction of the robot
     for (i=0; i<nb_of_steps; i++){

src/tester.c

@@ -1,5 +1,6 @@
 #include <stdio.h>
 #include <unistd.h>
+#include <pthread.h>
 
 #include "ev3c.h"
 #include "tester.h"
@@ -12,6 +13,8 @@
 #include "robot.h"
 #include "sensors.h"
 #include "logger.h"
+#include "threads.h"
+#include "init.h"
 
 void send_all_messages(state *s)
 {
@@ -36,6 +39,22 @@ void send_all_messages(state *s)
 void test_bluetooth(state *s)
 {
     s->sock = init_inet(s);
+    init_locks(s);
+
+    //Set game started
+    pthread_mutex_lock(&(s->mutexGameStarted));
+    s->gameStarted = IMMOBILE;
+    pthread_mutex_unlock(&(s->mutexGameStarted));
+
+    //Init Threads
+    if(pthread_create(&(s->threadPosition), NULL, (void *) position_thread, (void*) &s))
+    {
+        printf("[%s] Unable to create position thread\n", __FILE__);
+        exit(0);
+    }
+
+    game();
+    exit(0);
 
     char buffer[MSG_MAX_LEN];
     int readedBytes, result;
@@ -52,6 +71,9 @@ void test_bluetooth(state *s)
                     if (result == 0)
                     {
                         printf("Message START recieved and well interpreted\n");
+                        mainpos *p;
+                        game();
+
                         //send_all_messages(s);
 
             //            printf("NExt! %d\n", send_message(s, MSG_NEXT, s->ally));

src/threads.c

@@ -14,7 +14,6 @@ void position_thread(void *voidS)
 {
     state *s = (state *) voidS;
 
-    log_this(s, "[%s] Waiting for server\n");
     while(1)
     {
         pthread_mutex_lock(&(s->mutexGameStarted));
@@ -27,8 +26,7 @@ void position_thread(void *voidS)
         pthread_mutex_unlock(&(s->mutexGameStarted));
         sleep(2);
     }
-
-    log_this(s, "[%s] Starting to send position\n", __FILE__);
+    printf("WERE ARE\n");
 
     position currentPos;
     int i = 0;
@@ -38,9 +36,7 @@ void position_thread(void *voidS)
         currentPos = s->curPos;
         pthread_mutex_unlock(&(s->mutexPosition));
 
-        pthread_mutex_lock(&(s->mutexSockUsage));
         send_position(s, currentPos);
-        pthread_mutex_unlock(&(s->mutexSockUsage));
 
         if (++i == 8)
         {
@@ -56,8 +52,6 @@ void position_thread(void *voidS)
         sleep(2);
     }
 
-    log_this(s, "[%s] End thread position.\n", __FILE__);
-
     return;
 }