Document turret orientation, fix off-by-one

1 files changed, 19 insertions, 13 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index e50709f..87b8449 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -119,7 +119,10 @@ bool turretSensingEnabled = true;
 // Counters
 int counterInit = 0;
 int counterMovement = 0;
-int counterTurret = -1;
+/**
+ * The orientation of the turret, in range 0..NUM_TURRET_DIRECTIONS-1.
+ */
+int counterTurret = 0;
 int counterTurretWait = 0;
 int counterSampleConfirm = 0;
 
@@ -585,11 +588,8 @@ void turnTurretTo(int deg) {
 }
 void turnTurretToNext() {
 	if(timerTurret == 0) {
-		counterTurret++;
+		counterTurret = (counterTurret + 1) % NUM_TURRET_DIRECTIONS;
 		counterTurretWait++;
-		if(counterTurret >= NUM_TURRET_DIRECTIONS) {
-			counterTurret = 0;
-		}
 		
 		int deg;
 		deg = turretSequence[counterTurret] * 10;
@@ -632,6 +632,8 @@ void checkFreePath() {
 	}
 	
 	// Ultrasound
+	// Only check for obstacles for turret measurements in the middle, that is,
+	// ignore measurements from the far left and right positions.
 	if (// (dataToPiChangedBits & CHANGED_SENS_OBSTACLETURRET) && 
 			(counterTurret >= (NUM_TURRET_DIRECTIONS / 6)) && 
 			(counterTurret < (NUM_TURRET_DIRECTIONS - (NUM_TURRET_DIRECTIONS / 6))) &&
@@ -749,18 +751,21 @@ int eliminateDirections(int directionArray[]) {
 }
 
 void checkSample() {
-	if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected[counterTurret] || data.sample_gripper_detected) && (data.obstacle_turret_distances[counterTurret] > 37)) {
+	// 37 is an arbitrary magic number that specifies the acceptable distance
+	// for attempting to get the sample. This ensures that the robot does not
+	// hit a hill or something.
+	if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) &&
+		(data.sample_turret_detected[counterTurret] || data.sample_gripper_detected) &&
+		(data.obstacle_turret_distances[counterTurret] > 37)) {
 		DEBUG_PRINT("\n  -- FOUND SAMPLE?\n");
 		// One of the turret sees a sample (or the lab)
 		opMode(OPMODE_CHECKSAMPLE);
 		
-		// If sample straight ahead, set counterTurret to 6
+		// If sample is straight ahead, position the turret to the front such
+		// that the IR sensor can get a confirmation.
 		if (data.sample_gripper_detected) {
-			counterTurret = 3;
+			counterTurret = NUM_TURRET_DIRECTIONS / 2;
 		}
-		sensSampleTurret();
-		sensSampleGripper();
-		sensObstacleTurret();
 	}
 }
 
@@ -888,6 +893,7 @@ void setup() {
 
 void loop() {
 	
+	// read out sensors
 	// x sensCompass();
 	sensObstacleTurret();
 	sensIRLine(L);
@@ -897,14 +903,14 @@ void loop() {
 	
 	//sensLab();
 
+	// processes sensors, setting the next action (opmode)
 	checkSample();
 	checkFreePath();
 
 	// calculateOrientation();
 	// calculateLocationOffset();
 	
-	// TODO integrate pi communications
-	
+	// finally execute the next action
 	switch(operationMode) {
 		case OPMODE_WAIT:
 		counterMovement = 0;