Ultrasound no path created, updated checkBestRoute for prefered direction & impemented truerandom library

Fixed checkBestRoute protocol (to actualy not just drive straight)

1 files changed, 102 insertions, 29 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index d784255..8877af6 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -27,6 +27,8 @@
 #include <Servo.h>
 #include <HMC5883L.h>
 #include <Wire.h>
+#include <Serial.h>
+#include <TrueRandom.h>
 
 // **********************
 // ** Calibration file **
@@ -57,9 +59,12 @@ HMC5883L compass;
 // Data variables
 data_t data;
 int dataToPiChangedBits = 0;
+bool RPiActive = true;
+unsigned long lastCommunication = 0;
 
 // Current operation mode
 opmode_t operationMode = OPMODE_WAIT;
+opmode_t operationModeDefault = OPMODE_WAIT;
 unsigned long operationChange = 0;
 
 // Location values
@@ -88,7 +93,7 @@ long timerTurret = 0;
 // Counters
 int counterMovement = 0;
 int counterTurret = -1;
-int initCounter = 0;
+int counterTurretWait = 0;
 
 
 // **********************
@@ -161,7 +166,10 @@ void initTurretSequence(int array[], int numOfPositions) {
 
 // sendData to Raspberry Pi
 void sendData(pi_datatype_t method, int data, int data1, int data2, int data3) {
-  
+	if (1 == 1) {
+		lastCommunication = millis();
+	}
+	
 }
 
 void interpretData(pi_datatype_t dataType, int message) {
@@ -191,8 +199,24 @@ void readData() {
 	operationChange = millis();
 	// for example:
 	interpretData(PI_DATATYPE_MOVETO, 0);
+	if (1 == 1) {
+		lastCommunication = millis();
+	}
 }
 
+bool checkIfPiAlive() {
+	return false; // CONNECT TO PI TO CHECK IF STILL ALIVE
+}
+
+bool checkRPiActive() {
+	RPiActive = (lastCommunication + MAX_COMMUNICATION_DELAY) > millis());
+	
+	if(!RPiActive) {
+		RPiActive = checkIfPiAlive();
+	}
+	
+	return RPiActive;
+}
 
 // **********************
 // ** SENSOR FUNCTIONS **
@@ -385,13 +409,14 @@ void maneuver(int speedLeft, int speedRight){
 void stopMovement() {
 	DEBUG_PRINT(" Stop Movement\n");
 	maneuver(0, 0);
-	timerTurret == 0;
+	timerMovement = 0;
 }
 
 void stopAllServos() {
 	DEBUG_PRINT(" Stop All Servos\n");
 	maneuver(0, 0); 
-	timerTurret == 0;
+	timerTurret = 0;
+	timerMovement = 0;
 }
 
 bool moveTurnTo(int direction) {
@@ -461,6 +486,7 @@ void moveStraight(int direction, float distance) {
 	
 	if (timerMovement < millis()) {
 		stopMovement();
+		counterMovement = 0;
 	}
 }
 
@@ -471,8 +497,11 @@ void turnTurretTo(int deg) {
 void turnTurretToNext() {
 	if(timerTurret == 0) {
 		counterTurret++;
+		DEBUG_PRINT("Turret: ");
+		DEBUG_PRINT(counterTurret);
+		DEBUG_PRINT("\n");
 		if(counterTurret >= NUM_TURRET_DIRECTIONS) {
-			counterTurret == 0;
+			counterTurret = 0;
 		}
 		
 		int deg;
@@ -499,18 +528,32 @@ void opMode(opmode_t opmode) {
 }
 
 void checkFreePath() {
+	// Line detection
 	if ((dataToPiChangedBits & (CHANGED_SENS_IRLEFT | CHANGED_SENS_IRRIGHT)) && (data.IR_left_detected || data.IR_right_detected)) {
 		// Left or Right IR sensor sees inaccessible terrain
 		opMode(OPMODE_WAIT);
 		stopMovement();
-		DEBUG_PRINT(" NO FREE PATH!! ");
+		DEBUG_PRINT(" NO FREE PATH: left-");
+		DEBUG_PRINT(data.IR_left_detected);
+		DEBUG_PRINT(" - right-");
+		DEBUG_PRINT(data.IR_right_detected);
 	}
 	
 	// Ultrasound
-	
+	if ((dataToPiChangedBits & CHANGED_SENS_OBSTACLETURRET) && 
+			(counterTurret >= (NUM_TURRET_DIRECTIONS / 4)) && 
+			(counterTurret < (NUM_TURRET_DIRECTIONS - (NUM_TURRET_DIRECTIONS / 4))) &&
+			(data.obstacle_turret_distances[counterTurret] < OBSTACLE_TOO_CLOSE)) {
+		// Ultrasound sensor sees inaccessible terrain
+		opMode(OPMODE_WAIT);
+		stopMovement();
+
+		DEBUG_PRINT("   NO FREE PATH: Obstacle turret-");
+		DEBUG_PRINT(data.obstacle_turret_distances[counterTurret]);
+	}
 }
 
-int checkBestRoute() {
+int checkBestRoute(int preferedDirection) {
 	// 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
 	if (counterMovement) {
@@ -521,6 +564,13 @@ int checkBestRoute() {
 	// 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] = {};
+	for (int i = 0; i < NUM_DIRECTIONS * 2; i++) {
+		if (preferedDirection == -1) {
+			directionArray[i] = 1;
+		} else {
+			directionArray[i] = 10 + abs((int)(10 * (float)(abs(((NUM_DIRECTIONS - abs(i - NUM_DIRECTIONS)) % NUM_DIRECTIONS) - preferedDirection) - (float)NUM_DIRECTIONS / 2)));
+		}
+	}
 	
 	if (!eliminateDirections(directionArray)) {
 		// We are screwed!!! Basically robot is dead or trapped.
@@ -529,7 +579,7 @@ int checkBestRoute() {
 	
 	int direction = 0;
 	do {
-		direction = micros() % (NUM_DIRECTIONS * 2);
+		direction = TrueRandom.random(NUM_DIRECTIONS * 2);
 	} while (!directionArray[direction]);
 	
 	currValDirection = direction;
@@ -540,9 +590,6 @@ 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
@@ -553,9 +600,23 @@ int eliminateDirections(int directionArray[]) {
 		if (data.IR_right_detected && ( (i < ((float)NUM_DIRECTIONS / 4.0)) || (i > ((float)NUM_DIRECTIONS / 4.0)*2.0) )) {
 			directionArray[i] = 0;
 		}
-		// ULTRASOUND PANIC! If ...
-		
+		// ULTRASOUND PANIC! Check the obstacle_turret_distances array to see if there are unallowed directions
+		if ((i < (float)NUM_DIRECTIONS / 4.0) || ((i > (((float)NUM_DIRECTIONS / 4.0)*3.0)) && (i < (((float)NUM_DIRECTIONS / 4.0)*5.0))) || (i > (((float)NUM_DIRECTIONS / 4.0)*7.0))) {
+			// Incremental value for turret directions per robot direction
+			float kinc = ((((float)NUM_TURRET_DIRECTIONS - 1) * 3.0) / (float)NUM_DIRECTIONS);
+			// left most turret direction for this robot direction
+			int kinit = ((int)((float)(k % NUM_DIRECTIONS) + (float)NUM_DIRECTIONS / 6.0) % NUM_DIRECTIONS) * kinc - kinc;
+			// for all turret directions relative to this robot direction, check obstacle sensor data
+			for (int k = kinit; k <= (kinit + 2*kinc); k++) {
+				if (k < 0) k = 0;
+				if (data.obstacle_turret_distances[k] > OBSTACLE_TOO_CLOSE) {
+					directionArray[i] = 0;
+				}
+			}
+		}
 		
+		DEBUG_PRINT(" ");
+		DEBUG_PRINT(directionArray[i]);
 		if (directionArray[i]) {
 			numPossibleDirections++;
 			// Implement PI's choices (if still possible)
@@ -616,6 +677,11 @@ void setup() {
 	DEBUG_PRINT(" **\n\n");
 	
 	initTurretSequence(turretSequence, NUM_TURRET_DIRECTIONS);
+	for(int i = 0; i < NUM_TURRET_DIRECTIONS; i++) {
+		DEBUG_PRINT(turretSequence[i]);
+		DEBUG_PRINT(" ");
+	}
+	DEBUG_PRINT("\n\n");
 	
 	// initialise compass communication
 	compass = HMC5883L();
@@ -637,33 +703,45 @@ void setup() {
 	
 	turnTurretTo(0);
 	
+	lastCommunication = millis();
+	
 	delay(100); // Make sure all actuators are at their starting position
 }
 
 void loop() {
 	
-	sensCompass();
-	sensObstacleTurret();
+	//sensCompass();
+	//sensObstacleTurret();
 	sensIRLine(L);
 	sensIRLine(R);
-	sensSampleTurret();
-	sensSampleGripper();
+	//sensSampleTurret();
+	//sensSampleGripper();
 	
-	sensLab();
+	//sensLab();
 	
 	//checkSample();
 	checkFreePath();
 	
 	// calculateLocation();
 	
-	//sendData(PI_DATATYPE_ALL, 'lala', 'lala');
-	dataToPiChangedBits = 0; // Clear changed bits
-	//readData();
+	if (checkRPiActive()) {
+		//sendData(PI_DATATYPE_ALL, 'lala', 'lala');
+		//readData();
+	} else {
+		// STANDALONE ALGORITHM
+	}
+		dataToPiChangedBits = 0; // Clear changed bits
 	
 	switch(operationMode) {
 		case OPMODE_WAIT:
 		counterMovement = 0;
 		timerMovement = 0;
+		
+		// Make sure obstacle_turret_distances array is filled with new data before moving on
+		if (counterTurretWait == NUM_TURRET_DIRECTIONS) {
+			opMode(operationModeDefault);
+		}
+		
 		break;
 		case OPMODE_INITIALSEQUENCE:
 		initialSequence();
@@ -673,7 +751,7 @@ void loop() {
 		break;
 		case OPMODE_MAPPING:
 		turnTurretToNext();
-		checkBestRoute();
+		checkBestRoute(-1);
 		moveTurnTo(currValDirection);
 		moveStraight(currValDirection, 20.0); // 1.0 is supposed to be some distance
 		break;
@@ -700,11 +778,6 @@ void loop() {
 		break;
 	}
 	
-	if (!initCounter) {
-		opMode(OPMODE_MAPPING);
-		initCounter++;
-	}
-	
 	// Delay for stability.
-	delay(20);
+	delay(2);
 }
\ No newline at end of file