modified: Makefile

	modified:   include/sensors.h
	modified:   include/types.h
	new file:   include/utils.h
	modified:   include/vehicle.h
	modified:   src/main.c
	modified:   src/sensors.c
	new file:   src/utils.c
	modified:   src/vehicle.c

Makefile

@@ -1,6 +1,6 @@
 CC=gcc
-# libs third party
-LIBS=-pthread `allegro-config --libs`
+# libs third party: pthread/allegro/math
+LIBS=-pthread `allegro-config --libs` -lm
 
 # Headers
 IDIR=include

include/sensors.h

@@ -5,11 +5,12 @@
 
 #include "vehicle.h"
 
-// struct dstsens_;
-// struct cam_;
-// struct gps_;
-
+/**** REAL-TIME TASKS ****/
 void *getFrame(vehicle_t *myVehicle);
 void *getRangefinder(vehicle_t *myVehicle);
+/************************/
 
+void analyzeCameraFrame(vehicle_t *myVehicle);
+/* analyze the camera frame
+ */
 #endif

include/types.h

@@ -1,22 +1,28 @@
 #ifndef TYPE_H
 #define TYPE_H 
 
+#define NUM_BEAMS 4
+#define HRES 16
+#define VRES 9
+
+
 typedef enum tlState_ {RED_LIGHT, ORANGE_LIGHT, GREEN_LIGHT} tlState_t;
 
 typedef struct  
 {
 	char	visible;
-	int		posX;
-	int 	posY;
-	double 	*distances;
+	int		x;		// x pixel position
+	int 	y;		// y pixel position
+	double 	distances[NUM_BEAMS];
 } dstsens_t;
 
 typedef struct 
 {
-	int		resX;
-	int		ResolutionY;
-	double	posX;
-	double	posY;
+	unsigned	resH;
+	unsigned	resV;
+	unsigned	x;		// x position
+	unsigned	y;		// y position
+	unsigned 	image[HRES][VRES];	// image
 } cam_t;
 
 typedef struct 
@@ -29,8 +35,8 @@ typedef struct {
 	double		mass;
 	double		friction;
 	double 		controlU;
-	double		posXr;	 // real (or granted) position
-	double		posYr; 	// real (or granted) position
+	double		xr;	 // real (or granted) position
+	double		yr; 	// real (or granted) position
 	double		orientation;
 	double		velocity;
 	dstsens_t	dsensor;

include/utils.h

@@ -0,0 +1,18 @@
+#ifndef UTILS_H
+#define UTILS_H value
+
+/*RESOLUTION CONSTANTS*/
+#define W 640
+#define H 480
+/*BITS FOR COLORS*/
+#define N_COL 8
+
+void initGridMap(const char n_blocks);
+/* Inits grid map
+ */
+
+void initRandomMap(const int street_w, const int numRoads);
+/* Inits a random map, calls initGrid and fills some blocks
+ */
+
+#endif

include/vehicle.h

@@ -4,6 +4,26 @@
 #ifndef VEHICLE_H
 #define VEHICLE_H 
 
+/***** Real-Time Task *****/
 void *drive(vehicle_t *myVehicle);
+/* drives the car, do the planning
+ * based on distance sensor information and raw camera info.  
+ * Period: 20 ms
+ */
+/**************************/
+
+void initVehicle(vehicle_t *myV);
+/* Inits vehicle position
+ */
+
+void moveVehicle(vehicle_t *myVehicle);
+/*  
+ */
+void brakeVehicle(vehicle_t *myVehicle);
+/* 
+ */
+void accelerateVehicle(vehicle_t *myVehicle);
+/* 
+ */
 
 #endif

src/main.c

@@ -1,13 +1,10 @@
 #include <stdio.h>
 #include <allegro.h>
+
 #include "types.h"
 #include "vehicle.h"
+#include "utils.h"
 
-/*RESOLUTION CONSTANTS*/
-#define W 800
-#define H 600
-/*BITS FOR COLORS*/
-#define N_COL 8
 /*NUMBER OF BLOCKS*/
 char n_blocks;
 
@@ -20,15 +17,7 @@ void initialize_graphics()
 	clear_to_color(screen, 8);		//grey background
 
 	/*Initializing a simple random map*/
-	int n_blocks_x, n_blocks_y;
-	n_blocks_x = W/(2*n_blocks);
-	n_blocks_y = H/(2*n_blocks);
-	int i,j;
-	for(i=0; i<n_blocks_x; i++){
-		for(j=0; j<n_blocks_y; j++){
-			rectfill(screen,(i*20+5),(j*15+10),(i*15+5),(j*20+10),10);
-		}
-	}
+	initGridMap(n_blocks);	// TODO by Ari
 }
 
 int main(int argc, char const *argv[])
@@ -39,6 +28,12 @@ int main(int argc, char const *argv[])
 
 	initialize_graphics();
 
+	/**** Testing Cam *****/
+	vehicle_t v1;
+	initVehicle(&v1);
+	getFrame(&v1);
+	/**********************/
+
 	while(1){}
 
 	allegro_exit();

src/sensors.c

@@ -1,5 +1,65 @@
+#include <allegro.h>
+#include <math.h>
 #include "sensors.h"
 #include "types.h"
 
-void *getFrame(vehicle_t *myVehicle);
-void *getRangefinder(vehicle_t *myVehicle);
+void initCam(cam_t *myCam) {
+	myCam->resH = HRES;
+	myCam->resV = VRES;
+}
+
+void *getFrame(vehicle_t *myVehicle) {
+/* provides periodically frames acquired by camera: 33 ms
+ * code:
+ */	
+	// Camera global position
+	unsigned x0 = myVehicle->camera.x;
+	unsigned y0 = myVehicle->camera.y;
+
+	// Camera res a local variable
+	unsigned hRes = myVehicle->camera.resH; 
+	unsigned vRes = myVehicle->camera.resV;
+
+	// Taking picture
+	unsigned i, j;
+	unsigned x, y;
+	for (i = 0; i < hRes; ++i) {
+		for(j = 0; j < vRes; j++) {
+			x = x0 - hRes/2 + i;
+			y = y0 - vRes/2 + j;
+			myVehicle->camera.image[i][j] = getpixel(screen, x, y);
+		}
+	}
+
+}
+
+#define SMIN 10
+#define SMAX 100
+#define STEP 1
+
+void *getRangefinder(vehicle_t *myVehicle) {
+/* function to provide periodically informations acquired 
+ * by the distance sensor. Period: 20 ms
+ * code: 
+ */
+	int c;
+	int x, y;
+	int d = SMIN;
+	int x0 = myVehicle->xr;
+	int y0 = myVehicle->yr;
+
+	double alpha = myVehicle->orientation;
+
+	do {
+		x = x0 + d * cos(alpha);
+		y = y0 + d * cos(alpha);
+		c = getpixel(screen, x, y);
+		d = d + STEP;
+	} while ((d <= SMAX) && (c == 8));
+}
+
+void  analyzeCameraFrame(vehicle_t *myVehicle) {
+/* Analyzes the camera frame looking for a traffic light
+ * code:
+ */
+}

src/utils.c

@@ -0,0 +1,33 @@
+#include <allegro.h>
+#include <stdlib.h>
+#include <time.h>
+#include "utils.h"
+
+void initGridMap(const char n_blocks){
+	char n_blocks_x, n_blocks_y;
+	int i, j;
+
+	int block_w = 40;
+	int street_w = 20;
+	int someCoeff = block_w + street_w;
+
+	n_blocks_x = W / (2 * n_blocks);
+	n_blocks_y = H / (2 * n_blocks);
+
+	for(i = 0; i < n_blocks_x; i++){
+		for(j = 0; j < n_blocks_y; j++){
+			// rectfill(screen, (i * 20 + 5), (j * 15 + 10), 
+			// 				 (i * 15 + 5), (j * 20 + 10), 10);
+			// rectfill(screen, (j * 20), (i * 20), 
+			// 				 (j * 20 + block_w), 
+			// 				 (i * 20 + block_w), 10);
+			rectfill(screen, (j * someCoeff), (i * someCoeff), 
+							 (j * someCoeff + block_w), 
+							 (i * someCoeff + block_w), 10);
+		}
+	}	
+	// street_w = 20(j+1) - (20j + block_w) = 20j + 20 - 20j - block_w
+	// street_w = 20 - block_w;
+	// street_w + block_w = 20 = someCoeff;
+}
+

src/vehicle.c

@@ -3,4 +3,16 @@
 
 void *drive(vehicle_t *myVehi) {
 
+}
+
+void initVehicle(vehicle_t *myV) {
+	/* inits vehicle variables
+	 * code:
+	 */
+	myV->xr = 15;
+	myV->yr = 16;
+	myV->camera.x = myV->xr + 5;
+	myV->camera.y = myV->yr;
+	myV->camera.resH = 16;
+	myV->camera.resV = 9;
 }