Changed some shit:

Added a direction array with variable size, started implemented this.
Rest of skeleton is coming along great.
IR Left / Right might be done.

2 files changed, 215 insertions, 62 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 6b901c8..e5c0869 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -1,14 +1,21 @@
 // TODO
 //
 // - limit sendData to x per second
-//
+// - errorSequence()
 // - insert into calibration files
-// 		
-// 
 // - Make sure ENUM piDataType is correct and synced
 // - Change interpretData to resemble correct ENUM data
 // 
 
+// *****************
+// ** ERROR CODES **
+// *****************
+// 
+// 0		no error
+// 1		Error: Sensor Compass error
+// 2		Error: Sensor Obstacle detection on the turret
+// 3		Error: Sensor IR Line detection
+
 #include <Servo.h>
 
 // **********************
@@ -25,6 +32,7 @@
 // *****************
 
 // Pin configuration
+#
 #define PIN_SENS_COMPASS
 #define PIN_SENS_OBSTACLE_TURRET 9
 #define PIN_SENS_IR_LEFT A1
@@ -40,6 +48,30 @@
 Servo servoLeft;	// Declare left servo signal
 Servo servoRight;	// Declare right servo signal
 
+// Types
+
+// Sensor data
+typedef struct {
+	// ir
+	bool IR_left_detected;
+	bool IR_right_detected; // usually an integer (16-bit)
+	int distance; // 16-bit
+	// ..
+	long timer; // 32-bit
+} sensor_data_t;
+
+enum {
+	CHANGED_SENS_COMPASS = 1 << 0,
+	CHANGED_SENS_OBSTACLETURRET = 1 << 1,
+	CHANGED_SENS_IRLEFT = 1 << 2,
+	CHANGED_SENS_IRRIGHT = 1 << 3,
+	CHANGED_SENS_SAMPLETURRET = 1 << 4,
+	CHANGED_SENS_SAMPLEGRIPPER = 1 << 5
+};
+
+sensor_data_t data;
+int dataToPiChangedBits = 0;
+
 
 // Raspberry Pi Data type declarations
 typedef enum {
@@ -78,6 +110,9 @@ unsigned long operationChange = 0;
 int currValRobotX;
 int currValRobotY;
 
+// Number of directions the robot can choose from: ((x-1) * (360/x)) degree. Preferably a multiple of 4, I think.
+#define NUM_DIRECTIONS 12
+
 // Sensor values
 int currValSensCompass;
 int currValSensObstacleTurret;
@@ -119,7 +154,7 @@ void interpretData(pi_datatype_t dataType, int message) {
     case PI_DATATYPE_TURRET:
     
     break;
-    case default:
+    default:
 		// ignore ????
     break;
   }
@@ -147,33 +182,40 @@ int sensObstacleTurret() {
 }
 
 // sensor IR, line detection
-boolean sensIRLeft() {
+bool sensIRLine(char LorR) {
   int i;
-	boolean inaccessible = false;
-  i = analogRead(PIN_SENS_IR_LEFT);
-  if (i > CAL_IR_LEFT_THRESHOLD)
-    inaccessible = true;
+	bool inaccessible = false;
+  
+	if (LorR == 'L') {
+	  i = analogRead(PIN_SENS_IR_LEFT);
+	  if (i > CAL_IR_LEFT_THRESHOLD)
+	    inaccessible = true;
 	
-  // Inaccessible terrain. True for inaccessible, false for accessible
-  if(inaccessible) {
-    stopMovement();
-    sendData(PI_DATATYPE_IRLEFT, currValRobotX, currValRobotY, currValSensCompass);
-  }
-  return inaccessible;
-}
-int sensIRRight() {
-  int i;
-	boolean inaccessible = false;
-  i = analogRead(PIN_SENS_IR_RIGHT);
-  if (i > CAL_IR_RIGHT_THRESHOLD)
-    inaccessible = true;
+		if (data.IR_left_detected != inaccessible) {
+			data.IR_left_detected = inaccessible;
+			// mark as changed
+			dataToPiChangedBits |= CHANGED_SENS_IRLEFT;
+		}
 	
-  // Inaccessible terrain. True for inaccessible, false for accessible
-  if(inaccessible) {
-    stopMovement();
-    sendData(PI_DATATYPE_IRRIGHT, currValRobotX, currValRobotY, currValSensCompass);
-  }
-  return inaccessible;
+	  return inaccessible;
+	} 
+	
+	if (LorR == 'R') {
+	  i = analogRead(PIN_SENS_IR_RIGHT);
+	  if (i > CAL_IR_RIGHT_THRESHOLD)
+	    inaccessible = true;
+	
+		if (data.IR_right_detected != inaccessible) {
+			data.IR_right_detected = inaccessible;
+			// mark as changed
+			dataToPiChangedBits |= CHANGED_SENS_IRRIGHT;
+		}
+	
+	  return inaccessible;
+	}
+	
+	error(3);
+	return false;
 }
 
 // sensor IR, sample detection
@@ -186,7 +228,19 @@ int sensSampleTurret() {
   }
 }
 int sensSampleGripper() {
-  boolean sample = false;
+  int i;
+	bool sample = false;
+	
+	i = false; // sensor readout here
+	if (i > CAL_SAMPLE_GRIPPER_THRESHOLD) 
+		sample = true;
+	
+	if(data.sample_gripper != sample) {
+		data.sample_gripper = sample;
+		// Mark as changed
+		dataToPiChangedBits |= CHANGED_SENS_SAMPLEGRIPPER;
+	}
+	
   if(sample) {
     stopAllServos();
     sendData(PI_DATATYPE_SAMPLEGRIPPER, currValRobotX, currValRobotY, currValSensCompass);
@@ -220,34 +274,125 @@ void stopAllServos() {
 	servoLeft.detach();
 	servoRight.detach();
 }
+bool turnTo(int direction) {
+	if(direction == 0 || direction == NUM_DIRECTIONS) {
+		return true;
+	}
+	
+	// if movementtimer is not started, start timer or something and initiate movement
+	// else continue movement and return false
+	// if movementtimer is expired, stop movement and return true.
+	
+}
+
+// *********************
+// ** LOGIC FUNCTIONS **
+// *********************
+
+void checkFreePath() {
+	if ((dataToPiChangedBits & (CHANGED_SENS_IRLEFT | CHANGED_SENS_IRRIGHT))) && (data.IR_left_detected || data.IR_right_detected)) {
+			// Left or Right IR sensor sees inaccessible terrain
+			stopMovement();
+	}
+	
+	// Ultrasound
+	
+}
+
+int checkBestRoute() {
+	// IMPLEMENT SOME TIMER HERE...
+	// DON'T RUN EVERY TIME, TOO MUCH JITTER THEN
+	// possibly use time instead of boolean values, to remember them, default value = default time, possible to let pi change this
+	// possibly change directionArray to global variable
+	
+	// Array with all directions (for example with 12 directions):
+	// Bottom half: ([00-11] % 12) * (360/12) degrees to the left
+	// Top half: 		([12-23] % 12) * (360/12) degrees to the right
+	int directionArray[NUM_DIRECTIONS * 2] = {};
+	
+	if (!eliminateDirections(directionArray)) {
+		// We are screwed!!! Basically robot is dead or trapped.
+		return false;
+	}
+	
+	int direction = 0;
+	do {
+		direction = millis() % (NUM_DIRECTIONS * 2);
+	} while (!directionArray[direction]);
+	
+	return direction;
+}
+
+int eliminateDirections(int directionArray[]) {
+	int numPossibleDirections = 0;
+	// Implement PI's choices (if still possible)
+	// int numPossibleDirectionsPi = 0;
+	for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
+		directionArray[i] = 1;
+	}
+	
+	for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
+		// LINE PANIC! If a line is detected on the left, only a right turn is allowed between 90deg and 180deg
+		if (data.IR_left_detected && ( (i < ((float)NUM_DIRECTIONS / 4.0) + NUM_DIRECTIONS) || (i > ((float)NUM_DIRECTIONS / 4.0) * 2.0 + NUM_DIRECTIONS)) )) {
+			directionArray[i] = 0;
+		}
+		// LINE PANIC! If a line is detected on the right, only a left turn is allowed between 90deg and 180deg
+		if (data.IR_left_detected && ( (i < ((float)NUM_DIRECTIONS / 4.0)) || (i > ((float)NUM_DIRECTIONS / 4.0)*2.0) )) {
+			directionArray[i] = 0;
+		}
+		// ULTRASOUND PANIC! If ...
+		
+		
+		if (directionArray[i]) {
+			numPossibleDirections++;
+			// Implement PI's choices (if still possible)
+			// if(PIarray[i]) {numPossibleDirectionsPi++;}
+		}
+	}
+
+	// Implement PI's choices (if still possible)
+	// if (numPossibleDirectionsPi) {
+		//for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
+			// if(!PIarray[i]) {directionArray[i] = 0}
+		// }
+	// }
+	
+	return numPossibleDirections;
+}
 
 // ***********************
 // ** ARDUINO FUNCTIONS **
 // ***********************
 
 void setup() {
+	// initial communication between Arduino and RPi
+	// Send some values:
+	// - Number of possible directions
   
 }
 
 void loop() {
 	sensCompass();
 	sensObstacleTurret();
-	sensIRLeft();
-	sensIRRight();
+	sensIRLine('L');
+	sensIRLine('R');
 	sensSampleTurret();
 	sensSampleGripper();
 	sensBeaconTurret();
 	sensLab();
 	
-	switch(operationMode) {
-		case OPMODE_WAIT:
-		stopMovement();
-		break;
-		
-		default:
-		stopMovement();
-		break;
-	}
+	checkFreePath();
+	
+	// calculateLocation();
+	
+	sendData();
+	dataToPiChangedBits = 0; // Clear changed bits
+	readData();
+	
+	// Operations (probably in a switch or something, according to operation modes)
+	turnTo(checkBestRoute());
+	// driveStraight(someDistanceOrTime);
+	
 	
 }
 
@@ -255,6 +400,12 @@ void loop() {
 // ** HELPER FUNCTIONS **
 // **********************
 
+// error helper
+error(int errorCode) {
+	stopAllServos();
+	//errorSequence();
+}
+
 long microsecondsToInches(long microseconds)
 {
   // According to Parallax's datasheet for the PING))), there are
@@ -289,25 +440,26 @@ long microsecondsToCentimeters(long microseconds)
 
 
 
-// define types
-typedef struct {
-	// ir
-	bool left_detected;
-	bool right_detected; // usually an integer (16-bit)
-	int distance; // 16-bit
-	// ..
-	long timer; // 32-bit
-} sensor_data_t;
-
-enum {
-	CHANGED_IR_LEFT = 1 << 0,
-	CHANGED_IR_RIGHT = 1 << 1,
-	CHNAGED_JDHCHDUCDH = 1 << 2,
-		CHANGED_IDJDHD = 1 << 3
-};
-
-sensor_data_t data;
-int changedBits = 0;
+// // define types
+// typedef struct {
+// 	// ir
+// 	bool left_detected;
+// 	bool right_detected; // usually an integer (16-bit)
+// 	int distance; // 16-bit
+// 	// ..
+// 	long timer; // 32-bit
+// } sensor_data_t;
+// 
+// enum {
+// 	CHANGED_IR_LEFT = 1 << 0,
+// 	CHANGED_IR_RIGHT = 1 << 1,
+// 	CHNAGED_JDHCHDUCDH = 1 << 2,
+// 		CHANGED_IDJDHD = 1 << 3
+// };
+// 
+// sensor_data_t data;
+// int changedBits = 0;
+// 
 
 // 1. gather sensor data -< sets changedBits and data
 val = readFromSensor();
diff --git a/Venus_Skeleton/calibration.wall-e.h b/Venus_Skeleton/calibration.wall-e.h
index e667d56..a7adf7a 100644
--- a/Venus_Skeleton/calibration.wall-e.h
+++ b/Venus_Skeleton/calibration.wall-e.h
@@ -1,6 +1,7 @@
 // WALLY configuration file
 
-#define calObstacleTurretMaxDist 50;
-#define calObstacleTurretMinDist 10;
+#define calObstacleTurretMaxDist 50
+#define calObstacleTurretMinDist 10
 #define CAL_IR_LEFT_THRESHOLD 600
-#define CAL_IR_RIGHT_THRESHOLD 600
\ No newline at end of file
+#define CAL_IR_RIGHT_THRESHOLD 600
+#define CAL_SAMPLE_GRIPPER_THRESHOLD x
\ No newline at end of file