Last changes

Filled in quite a bit last few days

1 files changed, 122 insertions, 32 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index e5c0869..9b4dd54 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -55,6 +55,8 @@ typedef struct {
 	// ir
 	bool IR_left_detected;
 	bool IR_right_detected; // usually an integer (16-bit)
+	bool sample_turret_detected;
+	bool sample_gripper_detected;
 	int distance; // 16-bit
 	// ..
 	long timer; // 32-bit
@@ -89,16 +91,15 @@ typedef enum {
 } pi_datatype_t;
 // Operation modes
 typedef enum {
-	OPMODE_RANDOM, 
-	OPMODE_WAIT, 
-	OPMODE_INITIALSEQUENCE, 
-	OPMODE_RANDOM, 
-	OPMODE_MAPPING, 
-	OPMODE_GOTOSAMPLE, 
-	OPMODE_GRABSAMPLE, 
-	OPMODE_GOTOLABLOCATION, 
-	OPMODE_WAITFORLAB, 
-	OPMODE_FINDMAGNET, 
+	OPMODE_WAIT,
+	OPMODE_INITIALSEQUENCE,
+	OPMODE_RANDOM,
+	OPMODE_MAPPING,
+	OPMODE_CHECKSAMPLE,
+	OPMODE_GRABSAMPLE,
+	OPMODE_GOTOLABLOCATION,
+	OPMODE_WAITFORLAB,
+	OPMODE_FINDMAGNET,
 	OPMODE_LABSEQUENCE
 } opmode_t;
 
@@ -107,19 +108,26 @@ opmode_t operationMode = OPMODE_WAIT;
 unsigned long operationChange = 0;
 
 // Location values
-int currValRobotX;
-int currValRobotY;
+int currValRobotX = 0;
+int currValRobotY = 0;
 
 // 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;
-int currValServoTurret;
+int currValSensCompass = 0;
+int currValSensObstacleTurret = 0;
+int currValServoTurret = 0;
 
 // Actuator values
-int currValTurret;
+int currValTurret = 0;
+int currValDirection = 0;
+
+// Timers
+long timerMovement = 0;
+
+// Counters
+int counterMovement = 0;
 
 
 // **********************
@@ -223,6 +231,7 @@ int sensSampleTurret() {
   boolean sample = false;
   if(sample) {
 		// If a sample is detected, stop all servos, send data to the PI and wait for response
+		opMode(OPMODE_WAIT);
     stopAllServos();
     sendData(PI_DATATYPE_SAMPLETURRET, currValRobotX, currValRobotY, currValSensCompass, currValTurret);
   }
@@ -235,16 +244,13 @@ int sensSampleGripper() {
 	if (i > CAL_SAMPLE_GRIPPER_THRESHOLD) 
 		sample = true;
 	
-	if(data.sample_gripper != sample) {
-		data.sample_gripper = sample;
+	if(data.sample_gripper_detected != sample) {
+		data.sample_gripper_detected = sample;
 		// Mark as changed
 		dataToPiChangedBits |= CHANGED_SENS_SAMPLEGRIPPER;
 	}
 	
-  if(sample) {
-    stopAllServos();
-    sendData(PI_DATATYPE_SAMPLEGRIPPER, currValRobotX, currValRobotY, currValSensCompass);
-  }
+  return sample;
 }
 
 // sensor IR, beacon detection and recognition
@@ -265,34 +271,63 @@ int sensLab() {
 // ************************
 
 void stopMovement() {
-	operationMode = OPMODE_WAIT;
 	servoLeft.detach();
 	servoRight.detach();
 }
+
 void stopAllServos() {
-	operationMode = OPMODE_WAIT;
 	servoLeft.detach();
 	servoRight.detach();
 }
-bool turnTo(int direction) {
-	if(direction == 0 || direction == NUM_DIRECTIONS) {
+
+bool moveTurnTo(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.
+	// if movementtimer is expired, stop movement.
+	
+	if (!timerMovement) {
+		counterMovement++;
+		// timer = now() + direction * formula
+		timerMovement = millis() + (direction % NUM_DIRECTIONS) * 200;
+		
+		
+		// DO A COMPASS CHECK
+		// movePivot('R', somespeed);
+		
+		
+	}
+	if (timerMovement < millis()) {
+		stopMovement();
+		currValDirection = 0;
+	}
+}
+
+void moveStraight(int direction) {
+	if (direction != 0 && direction != NUM_DIRECTIONS) {
+		return;
+	}
+	
 	
 }
 
+
 // *********************
 // ** LOGIC FUNCTIONS **
 // *********************
 
+void opMode(opmode_t opmode) {
+	operationMode = opmode;
+}
+
 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();
+		// Left or Right IR sensor sees inaccessible terrain
+		opMode(OPMODE_WAIT);
+		stopMovement();
 	}
 	
 	// Ultrasound
@@ -304,6 +339,9 @@ int checkBestRoute() {
 	// 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
+	if (counterMovement) {
+		return 0;
+	}
 	
 	// Array with all directions (for example with 12 directions):
 	// Bottom half: ([00-11] % 12) * (360/12) degrees to the left
@@ -320,6 +358,7 @@ int checkBestRoute() {
 		direction = millis() % (NUM_DIRECTIONS * 2);
 	} while (!directionArray[direction]);
 	
+	currValDirection = direction;
 	return direction;
 }
 
@@ -360,6 +399,26 @@ int eliminateDirections(int directionArray[]) {
 	return numPossibleDirections;
 }
 
+void checkSample() {
+	if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER))) && (data.sample_turret_detected || data.sample_gripper_detected)) {
+		// One of the turret sees a sample (or the lab)
+		opMode(OPMODE_CHECKSAMPLE);
+		stopAllServos();
+		sensSampleTurret();
+		sensSampleGripper();
+	}
+}
+
+bool confirmSample() {
+	if (data.sample_turret_detected || data.sample_gripper_detected) {
+		if (!data.sample_gripper_detected) {
+			// Turn robot to sample
+			// If still no sample_gripper_detected return false
+		}
+		// What is distance
+	}
+}
+
 // ***********************
 // ** ARDUINO FUNCTIONS **
 // ***********************
@@ -380,7 +439,8 @@ void loop() {
 	sensSampleGripper();
 	sensBeaconTurret();
 	sensLab();
-	
+
+	checkSample();
 	checkFreePath();
 	
 	// calculateLocation();
@@ -389,9 +449,38 @@ void loop() {
 	dataToPiChangedBits = 0; // Clear changed bits
 	readData();
 	
+	
+	// OPMODE_WAIT,
+	// OPMODE_INITIALSEQUENCE,
+	// OPMODE_RANDOM,
+	// OPMODE_MAPPING,
+	// OPMODE_CHECKSAMPLE,
+	// OPMODE_GRABSAMPLE,
+	// OPMODE_GOTOLABLOCATION,
+	// OPMODE_WAITFORLAB,
+	// OPMODE_FINDMAGNET,
+	// OPMODE_LABSEQUENCE
+	
+	switch(operationMode) {
+		case OPMODE_WAIT:
+			
+			break;
+		// ...
+		case OPMODE_MAPPING:
+			checkBestRoute();
+			moveTurnTo(currValDirection);
+			moveStraight(currValDirection, SOMEDISTANCE!!!!!!!!!!!!!);
+			break;
+		case OPMODE_CHECKSAMPLE:
+			
+			break;
+	}
 	// Operations (probably in a switch or something, according to operation modes)
-	turnTo(checkBestRoute());
-	// driveStraight(someDistanceOrTime);
+	// Opmode WAIT: set counterMovement to zero
+	//							set timerMovement to zero
+	checkBestRoute();
+	moveTurnTo(currValDirection);
+	moveStraight(currValDirection, SOMEDISTANCE!!!!!!!!!!!!!);
 	
 	
 }
@@ -402,6 +491,7 @@ void loop() {
 
 // error helper
 error(int errorCode) {
+	opMode(OPMODE_WAIT);
 	stopAllServos();
 	//errorSequence();
 }