modified: include/utils.h

	leaving city added:   src/main.c
	leaving city + bug fux:   src/pathplanner.c
	bug fixed:   src/sensors.c
	leaving city added:   src/utils.c
	leaving city added:   src/vehicle.c

include/utils.h

@@ -68,4 +68,6 @@ char cmpPnts(int *A, int *B, const int N);
 
 void chooseCarDelete(int *r, vehicle_t *c, rt_task_par_t *p);
 
+void checkWhosOut(int *r, vehicle_t *c, rt_task_par_t *p);
+
 #endif

src/main.c

@@ -66,6 +66,13 @@ int main(int argc, char const *argv[])
 			drawNCars(carCounter);	
 		}
 
+		checkWhosOut(&readyCar, cars, carsPrms);
+		if (readyCar > -1) {
+			task_terminate(&carsPrms[readyCar]);
+			carCounter--;
+			drawNCars(carCounter);	
+		}
+
 	} while(scan != KEY_ESC);
 	/***************************************/
 

src/pathplanner.c

@@ -232,16 +232,19 @@ void check2turnAround(vehicle_t *c, imfeatures_t *imf){
 	if (c->cam.image[(imf->stCorner.x[0] + 5) * HRES + 				// check if the color between the corners is the street color
 		(imf->stCorner.y[0]/2 + imf->stCorner.y[1]/2)] == BLOCK_COL)
 	{	
-
-		c->planner.w0 = fabs(imf->stCorner.y[0] - imf->stCorner.y[1]) / 2	// compute x0 to generate a half circle trajectory
-						- imf->stCorner.y[1] + HRES / 2;	
-		c->planner.e.a = c->planner.w0; 								// radio
-		c->planner.alpha = M_PI / 2;									// the circle starts at 90 degress
-		c->planner.tmpX = c->xr;										// catching the current position
-		c->planner.tmpY = c->yr;
-		c->theta_old = c->theta;										// the current theta
-		c->turn = TURN_180;												// flaggin the type of steering
-		c->isTime2Steer = 1;											// enabling to execute the steering
+		c->planner.w0 = fabs(imf->stCorner.y[0] - imf->stCorner.y[1]);	// compute x0 to generate a half circle trajectory
+		printf("y[0] %d, y[1] %d\n", imf->stCorner.y[0], imf->stCorner.y[1]);
+		if (c->planner.w0 > (STREET_W - 5.0)) {								// 5 is a tolerance
+			c->planner.w0 /= 2.0;												// compute x0 to generate a half circle trajectory
+			c->planner.w0 -= (imf->stCorner.y[1] - HRES / 2);					// compute x0 to generate a half circle trajectory
+			c->planner.e.a = c->planner.w0; 								// radio
+			c->planner.alpha = M_PI / 2;									// the circle starts at 90 degress
+			c->planner.tmpX = c->xr;										// catching the current position
+			c->planner.tmpY = c->yr;
+			c->theta_old = c->theta;										// the current theta
+			c->turn = TURN_180;												// flaggin the type of steering
+			c->isTime2Steer = 1;											// enabling to execute the steering
+		}
 	}
 }
 
@@ -255,6 +258,9 @@ char pathPlanner(vehicle_t *c) {
 	/***** FRONT DISTANCE RESPONSE *****/
 	if (c->ds.dsts[MID_DST] < DST_FROM_FRONTOBSTACLE) {
 		c->Vr = 0;	// Brakes before hit front wall or front block
+		if (c->yr < STREET_W / 2 + 5) {
+			stopppedByTL = 2;
+		}
 		/***********************************/
 	} else { 
 		c->Vr = V_REF;							// it needs to return going again
@@ -293,6 +299,8 @@ char pathPlanner(vehicle_t *c) {
 			steer(c, &imf);					// steers
 		/***********************************/
 	}
+
+
 	return stopppedByTL;
 }
 

src/sensors.c

@@ -27,9 +27,9 @@ void display(int *im, const int x0, const int y0) {
 	j = 0;
 
 	for (p = im; p < (im + VRES * HRES); ++p) {
-		if (*p != getpixel(screen, x0 + i, y0 + j)) {	// if the curren pixel is equal than the previous
+		if (*p != getpixel(screen, x0 + j, y0 + i)) {	// if the curren pixel is equal than the previous
 			pthread_mutex_lock(&screen_lock);			// it's not neccesary to redraw
-			putpixel(screen, x0 + i, y0 + j, *p);
+			putpixel(screen, x0 + j, y0 + i, *p);
 			pthread_mutex_unlock(&screen_lock);
 		}
 
@@ -63,7 +63,8 @@ void *getFrame(vehicle_t *c) {
 		for(j = 0; j < HRES; j++) {
 			x = (-VRES / 2 + i) * cost - (-HRES / 2 + j) * sint + x0; 
 			y = (-VRES / 2 + i) * sint + (-HRES / 2 + j) * cost + y0; 
-			if ((x < 0) || (y < 0)) 
+
+			if ((x <= 0) || (y <= 0)) 
 				c->cam.image[i * HRES + j] = BLOCK_COL;
 			else 
 				c->cam.image[i * HRES + j] = getpixel(screen, x, y);
@@ -162,7 +163,7 @@ void  analyzeCameraFrame(vehicle_t *c, imfeatures_t *imft) {
 		}
 	}
 
-	// display(bwTL, c->id * 100, 620);
+	display(c->cam.image, W - HRES - 10, H - VRES - 100);
 	RosenfeldPfaltz(bwTL, &imft->TLcenter, 0);
 	fastHarrisRobertCornerDetection(&bwStreet);
 	imft->stCorner = bwStreet.ft;

src/utils.c

@@ -28,10 +28,22 @@ void chooseCarDelete(int *r, vehicle_t *c, rt_task_par_t *p) {
 	for (i = 0; i < MAX_CARS; i++) {
 		if(p[i].isActive) {
 
-			if (c[i].isStopped) *r = i;
+			if (c[i].isStopped == 1) *r = i;
 
 			if (*r == -1) *r = i;
 		}
 	}
 
-}
+}
+
+void checkWhosOut(int *r, vehicle_t *c, rt_task_par_t *p) {
+	*r = -1;
+	int i;
+	for (i = 0; i < MAX_CARS; i++) {
+		if(p[i].isActive && (c[i].isStopped == 2)) {
+			*r = i;
+		}
+	}
+}
+
+

src/vehicle.c

@@ -98,7 +98,12 @@ void *moveVehicle(void *myV) {
 	/** Reading Sensors **/
 	getRangefinder(&c->ds, RANGEFINDER_3_BEAMS);
 	getFrame(c);
-	stoppedByTL = pathPlanner(c);
+
+	if (c->isStopped == 2)
+		c->Vr = 10;
+	else 
+		stoppedByTL = pathPlanner(c);
+
 
 	xd = c->xr;
 	yd = c->yr;
@@ -125,7 +130,8 @@ void *moveVehicle(void *myV) {
 		pthread_mutex_unlock(&screen_lock);
 	} else {
 		/** RED LIGHTS display at the back of the car when it's stopped **/
-		c->isStopped = !stoppedByTL;
+		if (stoppedByTL == 0) c->isStopped = 1;
+		else if (stoppedByTL == 2) c->isStopped = 2;
 		pthread_mutex_lock(&screen_lock);
 		circlefill(screen, c->xr - c->l/4 * cos(c->theta), c->yr - c->l/4 * sin(c->theta), c->w / 2 - 3, CAR_STOPPED);
 		pthread_mutex_unlock(&screen_lock);	
@@ -243,7 +249,7 @@ char generateRandomPositionAroundCity(vehicle_t *c) {
 			/* Bottom Side of screen */
 			theta = 1;
 			nb = (H - STREET_W) / blkLen;		// Number of blocks along y axis
-			c->theta += M_PI;					// 270 	deg
+			c->theta = 3 * M_PI / 2;					// 270 	deg
 			c->yr += nb * blkLen - STREET_W;
 			/*					|________ where counting from bottom
 			 */