some changes again

3 files changed, 123 insertions, 57 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 60df676..c51e4ba 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -63,7 +63,7 @@ unsigned long lastCommunication = 0;
 
 // Current operation mode
 opmode_t operationMode = OPMODE_WAIT;
-opmode_t operationModeDefault = OPMODE_WAIT;
+opmode_t operationModeDefault = OPMODE_INITIALSEQUENCE;
 unsigned long operationChange = 0;
 
 // Location values
@@ -85,11 +85,13 @@ int currValTurret = 0;
 int currValDirection = 0;
 
 // Timers
-long timerMovement = 0;
+long timerMovementStart = 0;
+long timerMovementStop = 0;
 long timerInitialSequence = 0;
 long timerTurret = 0;
 
 // Counters
+int counterInit = 0;
 int counterMovement = 0;
 int counterTurret = -1;
 int counterTurretWait = 0;
@@ -208,12 +210,10 @@ bool checkIfPiAlive() {
 }
 
 bool checkRPiActive() {
-	RPiActive = ((lastCommunication + MAX_COMMUNICATION_DELAY) > millis());
-	
+	RPiActive = (lastCommunication + MAX_COMMUNICATION_DELAY) > millis() ? 1 : 0;
 	if(!RPiActive) {
 		RPiActive = checkIfPiAlive();
 	}
-	
 	return RPiActive;
 }
 
@@ -408,14 +408,16 @@ void maneuver(int speedLeft, int speedRight){
 void stopMovement() {
 	DEBUG_PRINT(" Stop Movement\n");
 	maneuver(0, 0);
-	timerMovement = 0;
+	timerMovementStart = 0;
+	timerMovementStop = 0;
 }
 
 void stopAllServos() {
 	DEBUG_PRINT(" Stop All Servos\n");
 	maneuver(0, 0); 
 	timerTurret = 0;
-	timerMovement = 0;
+	timerMovementStart = 0;
+	timerMovementStop = 0;
 }
 
 bool moveTurnTo(int direction) {
@@ -423,35 +425,48 @@ bool moveTurnTo(int direction) {
 		return true;
 	}
 	
+	// Make sure turret doesn't move
+	timerTurret = 1;
+	
 	// if movementtimer is not started, start timer or something and initiate movement
 	// else continue movement and return false
 	// if movementtimer is expired, stop movement.
 	
-	if (!timerMovement) {
-		DEBUG_PRINT("\n  Start turn move: ");
+	if (!timerMovementStop) {
+		DEBUG_PRINT("  Start turn move: ");
 		DEBUG_PRINT(direction);
 		DEBUG_PRINT(" - ");
 		counterMovement++;
 		// timer = now() + direction * formula
-		timerMovement = millis() + (direction % NUM_DIRECTIONS) * CAL_MOVE_TURN * (360 / NUM_DIRECTIONS);
+		timerMovementStart = millis();
+		timerMovementStop = millis() + (direction % NUM_DIRECTIONS) * CAL_MOVE_TURN * (360 / NUM_DIRECTIONS);
 		
 		// DO A COMPASS CHECK
 		
 		maneuver(0,0);
 		if(direction < NUM_DIRECTIONS){
-			DEBUG_PRINT(" left\n\n");
+			DEBUG_PRINT(" left\n");
 			// turn left
+			int sign = -1;
 			maneuver(-100,100);
 		}
 		if(direction >= NUM_DIRECTIONS){
-			DEBUG_PRINT(" right\n\n");
+			DEBUG_PRINT(" right\n");
 			// Turn right
+			int sign = 1;
 			maneuver(100,-100);
 		}
+	} else {
+		int deg = ((int)((millis() - timerMovementStart) * CAL_MOVE_TURN * sign)) % 360;
+		data.robot_curr_deg = (data.robot_curr_deg + deg + 360*3) % 360;
 	}
-	if (timerMovement < millis()) {
+	
+	if (timerMovementStop < millis()) {
 		stopMovement();
 		currValDirection = 0;
+		for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) {
+			data.obstacle_turret_distances[k] = 300;
+		}
 	}
 }
 
@@ -461,32 +476,38 @@ void moveStraight(int direction, float distance) {
 	}
 	
 	// Convert distance into drive time
-	float drivetime = distance / 0.0154;
+	float drivetime = distance / CAL_MOVE_STRAIGHT;
 	
-	if (!timerMovement) {
-		DEBUG_PRINT("\n  Start straight move: ");
+	if (!timerMovementStop) {
+		DEBUG_PRINT("  Start straight move: ");
 		DEBUG_PRINT(distance);
 		DEBUG_PRINT(" - ");
 		counterMovement++;
 		// timer = now() + direction * formula
-		timerMovement = millis() + drivetime;
+		timerMovementStart = millis();
+		timerMovementStop = millis() + drivetime;
 		
 		// If input is positive drive forward
 		if(distance > 0) {
-			DEBUG_PRINT(" fw\n\n");
+			DEBUG_PRINT(" fw\n");
 			maneuver(100, 100);
 		}
 		// If input is negative drive backwards
 		if(distance < 0){
-			DEBUG_PRINT(" bw\n\n");
+			DEBUG_PRINT(" bw\n");
 			maneuver(-100, -100);
 		}
+	} else {
+		float dist = (millis() - timerMovementStart) * CAL_MOVE_STRAIGHT;
+		data.robot_curr_x += dist * sin((data.robot_curr_deg/180.0)*PI);
+		data.robot_curr_y += dist * cos((data.robot_curr_deg/180.0)*PI);
 	}
 	
-	if (timerMovement < millis()) {
+	if (timerMovementStop < millis()) {
 		stopMovement();
 		counterMovement = 0;
 	}
+	
 }
 
 void turnTurretTo(int deg) {
@@ -496,9 +517,7 @@ void turnTurretTo(int deg) {
 void turnTurretToNext() {
 	if(timerTurret == 0) {
 		counterTurret++;
-		DEBUG_PRINT("Turret: ");
-		DEBUG_PRINT(counterTurret);
-		DEBUG_PRINT("\n");
+		counterTurretWait++;
 		if(counterTurret >= NUM_TURRET_DIRECTIONS) {
 			counterTurret = 0;
 		}
@@ -524,6 +543,7 @@ void opMode(opmode_t opmode) {
 	DEBUG_PRINT(opmode);
 	DEBUG_PRINT("\n\n");
 	operationMode = opmode;
+	counterTurretWait = 0;
 }
 
 void checkFreePath() {
@@ -539,9 +559,9 @@ void checkFreePath() {
 	}
 	
 	// Ultrasound
-	if ((dataToPiChangedBits & CHANGED_SENS_OBSTACLETURRET) && 
-			(counterTurret >= (NUM_TURRET_DIRECTIONS / 4)) && 
-			(counterTurret < (NUM_TURRET_DIRECTIONS - (NUM_TURRET_DIRECTIONS / 4))) &&
+	if (// (dataToPiChangedBits & CHANGED_SENS_OBSTACLETURRET) && 
+			(counterTurret >= (NUM_TURRET_DIRECTIONS / 6)) && 
+			(counterTurret < (NUM_TURRET_DIRECTIONS - (NUM_TURRET_DIRECTIONS / 6))) &&
 			(data.obstacle_turret_distances[counterTurret] < OBSTACLE_TOO_CLOSE)) {
 		// Ultrasound sensor sees inaccessible terrain
 		opMode(OPMODE_WAIT);
@@ -563,22 +583,44 @@ int checkBestRoute(int preferedDirection) {
 	// 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++) {
+	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)));
+			directionArray[i] = 100 + abs((int)(100 * (float)(abs(((NUM_DIRECTIONS - abs(i - NUM_DIRECTIONS)) % NUM_DIRECTIONS) - preferedDirection) - (float)NUM_DIRECTIONS / 2)));
+		}
+		
+		// limit to first 180deg
+		if( (i % NUM_DIRECTIONS) > (NUM_DIRECTIONS / 2) ) {
+			directionArray[i] = 0;
 		}
 	}
 	
 	if (!eliminateDirections(directionArray)) {
 		// We are screwed!!! Basically robot is dead or trapped.
 		return false;
+		while(1) {
+			delay(1000);
+		}
 	}
 	
 	int direction = 0;
+	int sumDirectionPriority = 0;
+	int randValue = 0;
+	
+	for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) {
+		sumDirectionPriority += directionArray[i];
+	}
+	
 	do {
-		direction = TrueRandom.random(NUM_DIRECTIONS * 2);
+		randValue = TrueRandom.random(sumDirectionPriority);
+		for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) {
+			randValue -= directionArray[i];
+			if (randValue < 0) {
+				direction = i--;
+				break;
+			}
+		}
 	} while (!directionArray[direction]);
 	
 	currValDirection = direction;
@@ -654,7 +696,19 @@ bool confirmSample() {
 }
 
 void initialSequence() {
-	moveStraight(0, CAL_INITIAL_DISTANCE);
+	do {
+		moveStraight(0, CAL_INITIAL_DISTANCE1);
+	} while(timerMovementStop);
+	// Set x and y value of robot to (0, 0) for lab/starting location.
+	data.robot_curr_x = 0;
+	data.robot_curr_y = 0;
+	do {
+		moveStraight(0, CAL_INITIAL_DISTANCE2);
+	}	while(timerMovementStop);
+}
+
+void calculateOrientation() {
+	// current + compass
 }
 
 // ***********************
@@ -702,15 +756,13 @@ void setup() {
 	
 	turnTurretTo(0);
 	
-	lastCommunication = millis();
-	
 	delay(100); // Make sure all actuators are at their starting position
 }
 
 void loop() {
 	
 	//sensCompass();
-	//sensObstacleTurret();
+	sensObstacleTurret();
 	sensIRLine(L);
 	sensIRLine(R);
 	//sensSampleTurret();
@@ -720,60 +772,65 @@ void loop() {
 	
 	//checkSample();
 	checkFreePath();
-	
-	// calculateLocation();
+
+	// calculateOrientation();
+	// calculateLocationOffset();
 	
 	if (checkRPiActive()) {
+		operationModeDefault = (operationModeDefault == OPMODE_INITIALSEQUENCE ? OPMODE_INITIALSEQUENCE : OPMODE_WAIT);
 		//sendData(PI_DATATYPE_ALL, 'lala', 'lala');
 		//readData();
 	} else {
 		// STANDALONE ALGORITHM
+		operationModeDefault = (operationModeDefault == OPMODE_INITIALSEQUENCE ? OPMODE_INITIALSEQUENCE : OPMODE_MAPPING);
 	}
-		dataToPiChangedBits = 0; // Clear changed bits
+	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);
-		}
-		
+		timerMovementStart = 0;
+		timerMovementStop = 0;
+		opMode(operationModeDefault);
+		turnTurretToNext();
 		break;
+		
 		case OPMODE_INITIALSEQUENCE:
 		initialSequence();
+		operationModeDefault = OPMODE_WAIT;
 		break;
+		
 		case OPMODE_RANDOM:
-			
 		break;
+		
 		case OPMODE_MAPPING:
 		turnTurretToNext();
-		checkBestRoute(-1);
+		checkBestRoute(0);
 		moveTurnTo(currValDirection);
-		moveStraight(currValDirection, 20.0); // 1.0 is supposed to be some distance
+		moveStraight(currValDirection, 30.0);
 		break;
+		
 		case OPMODE_CHECKSAMPLE:
-			
 		break;
-		case OPMODE_GRABSAMPLE:
-			
+		
+		case OPMODE_GRABSAMPLE:	
 		break;
+		
 		case OPMODE_GOTOLABLOCATION:
 		sensBeaconTurret();
 		break;
+		
 		case OPMODE_WAITFORLAB:
-			
 		break;
+		
 		case OPMODE_FINDMAGNET:
-			
 		break;
+		
 		case OPMODE_LABSEQUENCE:
-			
 		break;
+		
 		default:
-		// ??
 		break;
 	}
 	
diff --git a/Venus_Skeleton/calibration_wall-e.h b/Venus_Skeleton/calibration_wall-e.h
index 61848f3..2c88c38 100644
--- a/Venus_Skeleton/calibration_wall-e.h
+++ b/Venus_Skeleton/calibration_wall-e.h
@@ -6,13 +6,22 @@
 #define CAL_IR_RIGHT_THRESHOLD 600
 #define CAL_SAMPLE_GRIPPER_THRESHOLD 1
 #define CAL_SAMPLE_TURRET_THRESHOLD 1
-#define CAL_INITIAL_DISTANCE 30.0
-#define CAL_TURRET_DEGREE 7
+
+// Distance to drive during initial sequence. (to (x, y) (0, 0))
+#define CAL_INITIAL_DISTANCE1 60.0
+// Distance to drive during initial sequence (to starting position - one robot further then the other, to prevent collissions)
+#define CAL_INITIAL_DISTANCE2 20.0
+
+#define CAL_TURRET_DEGREE 10
 
 // Left wheel calibration factor   WALL-E: 1.4 EVE: 1.0
 #define CAL_SERVO_LEFT 1.4
 // Right wheel calibration factor  WALL-E: 1.0 EVE: 1.1
 #define CAL_SERVO_RIGHT 1.0
 
+// Movement calibration
+// Turn calibration (ms per degree)
 #define CAL_MOVE_TURN 6.05
+// Straight calibration ((1/ms) per cm)
+#define CAL_MOVE_TURN 0.0154
 
diff --git a/Venus_Skeleton/dataTypes.h b/Venus_Skeleton/dataTypes.h
index 9021dc6..f19eaf5 100644
--- a/Venus_Skeleton/dataTypes.h
+++ b/Venus_Skeleton/dataTypes.h
@@ -20,8 +20,8 @@ typedef struct {
 	int robot_x[NUM_TURRET_DIRECTIONS];
 	int robot_y[NUM_TURRET_DIRECTIONS];
 	int robot_deg[NUM_TURRET_DIRECTIONS];
-	int robot_curr_x;
-	int robot_curr_y;
+	float robot_curr_x;
+	float robot_curr_y;
 	int robot_curr_deg;
 } data_t;
 enum {