latest commit

3 files changed, 183 insertions, 103 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 9b18248..a1a158c 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -41,15 +41,16 @@
 #define PIN_SENS_SAMPLE_GRIPPER
 #define PIN_SENS_BEACON_TURRET
 #define PIN_SERVO_LEFT 12
-#define PIN_SERVO_RIGHT = 13;
-#define PIN_SERVO_TURRET = 11;
+#define PIN_SERVO_RIGHT 13
+#define PIN_SERVO_TURRET 11
 
 // Servos
-Servo servoLeft;	// Declare left servo signal
-Servo servoRight;	// Declare right servo signal
+Servo servoLeft;
+Servo servoRight;
+Servo servoTurret;
 
-// Raspberry PI data variables
-sensor_data_t data;
+// Data variables
+data_t data;
 int dataToPiChangedBits = 0;
 
 // Current operation mode
@@ -59,13 +60,19 @@ unsigned long operationChange = 0;
 // Location values
 int currValRobotX = 0;
 int currValRobotY = 0;
+int LabX = 0;
+int LabY = 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
 
+// Number of directions the turret can measure from
+#define NUM_TURRET_DIRECTIONS 13
+int turretSequence[NUM_TURRET_DIRECTIONS] = {};
+
 // Sensor values
 int currValSensCompass = 0;
-int currValSensObstacleTurret = 0;
+int currValSensObstacleTurretLow = 0;
 int currValServoTurret = 0;
 
 // Actuator values
@@ -74,9 +81,76 @@ int currValDirection = 0;
 
 // Timers
 long timerMovement = 0;
+long timerInitialSequence = 0;
+long timerTurret = 0;
 
 // Counters
 int counterMovement = 0;
+int counterTurret = -1;
+
+
+// **********************
+// ** HELPER FUNCTIONS **
+// **********************
+
+// error helper
+void error(int errorCode) {
+	opMode(OPMODE_ERROR);
+	stopAllServos();
+	errorSequence(errorCode);
+}
+void errorSequence(int errorCode) {
+	while(1) {
+		// - move turret to middle (fast)
+		// - move turret to left (slow)
+		// - move turret to middle (slow)
+		delay(1000);
+		for(int i = 0; i < errorCode; i++) {
+			// - move turret to right (fast)
+			// - move turret to middle (fast)
+			delay(500);
+		}
+		delay(3000);
+	}
+}
+
+long microsecondsToInches(long microseconds)
+{
+  // According to Parallax's datasheet for the PING))), there are
+  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
+  // second).  This gives the distance travelled by the ping, outbound
+  // and return, so we divide by 2 to get the distance of the obstacle.
+  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
+  return microseconds / 74 / 2;
+}
+
+long microsecondsToCentimeters(long microseconds)
+{
+  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
+  // The ping travels out and back, so to find the distance of the
+  // object we take half of the distance travelled.
+  return microseconds / 29 / 2;
+}
+
+void initTurretSequence(int array, int numOfPositions) {
+	int sign = 1;
+	int x = 0;
+
+	for (int i = 0; i < numOfPositions; i++) {
+		array[i] = x;
+		x += sign * 2;
+
+		if(x > numOfPositions) {
+			sign = -1;
+			x -= 3;
+		}
+		if(x == numOfPositions) {
+			sign = -1;
+			x -= 1;
+		}
+	}
+}
+
 
 
 // **********************
@@ -131,48 +205,68 @@ int sensCompass() {
 }
 
 // sensor Ultrasound, distance measurement
-int sensObstacleTurret() {
-  int distance = 100;
-  int turretAngle = 90;
-  sendData(PI_DATATYPE_OBSTACLETURRET, distance, turretAngle);
-  return distance;
+long sensPing(int pin)
+{
+  long duration;
+	
+	// Sets pin to OUTPUT and send start signal: pulse (Low-High-Low for clean signal)
+  pinMode(pin, OUTPUT);
+  digitalWrite(pin, 0);
+  delayMicroseconds(2);
+  digitalWrite(pin, 255);
+  delayMicroseconds(5);
+  digitalWrite(pin, 0);
+	
+	// Sets pin to INPUT and measure time from the echo
+  pinMode(pin, INPUT);
+  duration = pulseIn(pin, 255);
+	
+	// Convert from us to cm
+  duration = microsecondsToCentimeters(duration);
+  return duration;
+}
+void sensObstacleTurret() {
+	
+	// BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT 
+	// (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners)
+	
+	if (timerTurret == 0) {
+		long distance = sensPing(PIN_SENS_OBSTACLE_TURRET);
+		if (data.obstacle_turret_distances[counterTurret] != distance) {
+			data.obstacle_turret_distances[counterTurret] = distance;
+			// mark as changed
+			dataToPiChangedBits |= CHANGED_SENS_OBSTACLETURRET;
+		}
+  }
 }
 
+
 // sensor IR, line detection
 bool sensIRLine(lr_t LorR) {
-  int i;
-	bool inaccessible = false;
-  
-	if (LorR == L) {
-	  i = analogRead(PIN_SENS_IR_LEFT);
-	  if (i > CAL_IR_LEFT_THRESHOLD)
-	    inaccessible = true;
 	
-		if (data.IR_left_detected != inaccessible) {
-			data.IR_left_detected = inaccessible;
-			// mark as changed
-			dataToPiChangedBits |= CHANGED_SENS_IRLEFT;
-		}
 	
-	  return inaccessible;
-	} 
+	// BE AWARE, LOCALISATION OF LINE IS y DISTANCE and z DEGREE FROM CENTER OF ROBOT,
+	// calculate by RPi??
 	
-	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;
-		}
+  int i;
+	bool inaccessible = false;
+	int pinNo = LorR == L ? PIN_SENS_IR_LEFT : PIN_SENS_IR_RIGHT;
+	int threshold = LorR == L ? CAL_IR_LEFT_THRESHOLD : CAL_IR_RIGHT_THRESHOLD;
+	bool *dataField = LorR == L ? &data.IR_left_detected : &data.IR_right_detected;
+	int changeBit = LorR == L ? CHANGED_SENS_IRLEFT : CHANGED_SENS_IRRIGHT;
+  
+  i = analogRead(pinNo);
+  if (i > threshold)
+    inaccessible = true;
 	
-	  return inaccessible;
+	if (*dataField != inaccessible) {
+		*dataField = inaccessible;
+		// mark as changed
+		dataToPiChangedBits |= changeBit;
 	}
 	
-	error(3);
-	return false;
+ 	return inaccessible;
 }
 
 // sensor IR, sample detection
@@ -227,6 +321,8 @@ void stopMovement() {
 void stopAllServos() {
 	servoLeft.detach();
 	servoRight.detach();
+	
+	timerTurret == 0;
 }
 
 bool moveTurnTo(int direction) {
@@ -263,6 +359,28 @@ void moveStraight(int direction, int distance) {
 	
 }
 
+void turnTurretTo(int deg) {
+	deg = deg + CAL_TURRET_DEGREE;
+	servoTurret.write(deg);
+}
+void turnTurretToNext() {
+	if(timerTurret == 0) {
+		counterTurret++;
+		if(counterTurret >= NUM_TURRET_DIRECTIONS) {
+			counterTurret == 0;
+		}
+		
+		int deg;
+	  deg = turretSequence[counterTurret] * 10;
+		
+		timerTurret = millis();
+	  turnTurretTo(deg)
+	}
+	if ((timerTurret + 100) < millis()) {
+		timerTurret = 0;
+	}
+}
+
 
 // *********************
 // ** LOGIC FUNCTIONS **
@@ -302,7 +420,7 @@ int checkBestRoute() {
 	
 	int direction = 0;
 	do {
-		direction = millis() % (NUM_DIRECTIONS * 2);
+		direction = micros() % (NUM_DIRECTIONS * 2);
 	} while (!directionArray[direction]);
 	
 	currValDirection = direction;
@@ -323,7 +441,7 @@ int eliminateDirections(int directionArray[]) {
 			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) )) {
+		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 ...
@@ -366,6 +484,10 @@ bool confirmSample() {
 	}
 }
 
+void initialSequence() {
+	moveStraight(0, CAL_INITIAL_DISTANCE);
+}
+
 // ***********************
 // ** ARDUINO FUNCTIONS **
 // ***********************
@@ -374,7 +496,16 @@ void setup() {
 	// initial communication between Arduino and RPi
 	// Send some values:
 	// - Number of possible directions
-  
+	
+	initTurretSequence(turretSequence, NUM_TURRET_DIRECTIONS);
+	
+	servoLeft.attach(servoLeftPin);
+	servoRight.attach(servoRightPin);
+	servoTurret.attach(servoTurretPin);
+	
+	turnTurretTo(0);
+	
+	delay(100); // Make sure all actuators are at their starting position
 }
 
 void loop() {
@@ -384,7 +515,7 @@ void loop() {
 	sensIRLine(R);
 	sensSampleTurret();
 	sensSampleGripper();
-	sensBeaconTurret();
+	
 	sensLab();
 
 	checkSample();
@@ -396,30 +527,19 @@ 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:
 			counterMovement = 0;
 			timerMovement = 0;
 			break;
 		case OPMODE_INITIALSEQUENCE:
-			
+			initialSequence();
 			break;
 		case OPMODE_RANDOM:
 			
 			break;
 		case OPMODE_MAPPING:
+			turnTurretToNext();
 			checkBestRoute();
 			moveTurnTo(currValDirection);
 			moveStraight(currValDirection, 1); // 1 is supposed to be some distance / time
@@ -431,7 +551,7 @@ void loop() {
 			
 			break;
 		case OPMODE_GOTOLABLOCATION:
-			
+			sensBeaconTurret();
 			break;
 		case OPMODE_WAITFORLAB:
 			
@@ -451,49 +571,6 @@ void loop() {
 	delay(20);
 }
 
-// **********************
-// ** HELPER FUNCTIONS **
-// **********************
-
-// error helper
-void error(int errorCode) {
-	opMode(OPMODE_ERROR);
-	stopAllServos();
-	errorSequence(errorCode);
-}
-void errorSequence(int errorCode) {
-	while(1) {
-		// - move turret to middle (fast)
-		// - move turret to left (slow)
-		// - move turret to middle (slow)
-		delay(1000);
-		for(int i = 0; i < errorCode; i++) {
-			// - move turret to right (fast)
-			// - move turret to middle (fast)
-			delay(500);
-		}
-		delay(3000);
-	}
-}
-
-long microsecondsToInches(long microseconds)
-{
-  // According to Parallax's datasheet for the PING))), there are
-  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
-  // second).  This gives the distance travelled by the ping, outbound
-  // and return, so we divide by 2 to get the distance of the obstacle.
-  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
-  return microseconds / 74 / 2;
-}
-
-long microsecondsToCentimeters(long microseconds)
-{
-  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
-  // The ping travels out and back, so to find the distance of the
-  // object we take half of the distance travelled.
-  return microseconds / 29 / 2;
-}
-
 
 
 
diff --git a/Venus_Skeleton/calibration_wall-e.h b/Venus_Skeleton/calibration_wall-e.h
index a7adf7a..462fa1b 100644
--- a/Venus_Skeleton/calibration_wall-e.h
+++ b/Venus_Skeleton/calibration_wall-e.h
@@ -4,4 +4,6 @@
 #define calObstacleTurretMinDist 10
 #define CAL_IR_LEFT_THRESHOLD 600
 #define CAL_IR_RIGHT_THRESHOLD 600
-#define CAL_SAMPLE_GRIPPER_THRESHOLD x
\ No newline at end of file
+#define CAL_SAMPLE_GRIPPER_THRESHOLD x
+#define CAL_INITIAL_DISTANCE x
+#define CAL_TURRET_DEGREE 7
\ No newline at end of file
diff --git a/Venus_Skeleton/dataTypes.h b/Venus_Skeleton/dataTypes.h
index 374f23b..9a77ae1 100644
--- a/Venus_Skeleton/dataTypes.h
+++ b/Venus_Skeleton/dataTypes.h
@@ -12,13 +12,14 @@ typedef enum {
 typedef struct {
 	// ir
 	bool IR_left_detected;
-	bool IR_right_detected; // usually an integer (16-bit)
+	bool IR_right_detected;
 	bool sample_turret_detected;
 	bool sample_gripper_detected;
-	int distance; // 16-bit
+	int distance;
+	long obstacle_turret_distances[NUM_TURRET_DIRECTIONS];
 	// ..
 	long timer; // 32-bit
-} sensor_data_t;
+} data_t;
 enum {
 	CHANGED_SENS_COMPASS = 1 << 0,
 	CHANGED_SENS_OBSTACLETURRET = 1 << 1,