beacon & sample detection implemented

1 files changed, 43 insertions, 10 deletions

diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino
index 5873726..f766a75 100644
--- a/Venus_Skeleton/Venus_Skeleton.ino
+++ b/Venus_Skeleton/Venus_Skeleton.ino
@@ -28,6 +28,8 @@
 #include <HMC5883L.h>
 #include <Wire.h>
 #include <TrueRandom.h>
+#include <IRremote.h>
+#define SONY
 
 // **********************
 // ** Calibration file **
@@ -55,6 +57,11 @@ Servo servoGripper;
 // Compass
 HMC5883L compass;
 
+// Beacon
+IRrecv irrecv(RECV_PIN);
+decode_results results;
+int LCode = 2704 ; // code send by beacon TBA
+
 // Data variables
 data_t data;
 int dataToPiChangedBits = 0;
@@ -348,13 +355,11 @@ void sensSampleTurret() {
 		int i;
 		bool sample = false;
 		
-		i = false; // SENSORDATA HERE
-		if (i > CAL_SAMPLE_TURRET_THRESHOLD) 
+		i = digitalRead(PIN_SENS_SAMPLE_TURRET);
+		if (i == LOW) 
 			sample = true;
 		
 		if (data.sample_turret_detected[counterTurret] != sample) {
-			opMode(OPMODE_WAIT);
-			stopAllServos();
 			data.sample_turret_detected[counterTurret] = sample;
 			// mark as changed
 			dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET;
@@ -365,8 +370,8 @@ void sensSampleGripper() {
 	int i;
 	bool sample = false; 
 	
-	i = false; // SENSORDATA HERE
-	if (i > CAL_SAMPLE_GRIPPER_THRESHOLD) 
+	i = digitalRead(PIN_SENS_SAMPLE_GRIPPER);
+	if (i == LOW) 
 		sample = true;
 	
 	if(data.sample_gripper_detected != sample) {
@@ -377,7 +382,29 @@ void sensSampleGripper() {
 }
 
 // sensor IR, beacon detection and recognition
-int sensBeaconTurret() {
+int sensIRRemote() {
+	int x = 0;
+	int LabFound = LOW;
+	for (int i = 1; i <= 10; i++)
+	{
+		x = 0;
+		do
+		{
+			if(irrecv.decode(&results))
+			{
+				irrecv.resume();
+			}
+			x++;
+			if( x == 5 )
+			{
+				LabFound = HIGH;
+				return LabFound;
+			}
+		}     while(results.value, DEC == LCode);
+	}
+	return LabFound;
+}
+void sensBeaconTurret() {
 	// BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT 
 	// (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners)
 	
@@ -385,8 +412,8 @@ int sensBeaconTurret() {
 		int i;
 		bool beacon = false;
 		
-		i = false; // SENSORDATA HERE
-		if (i > CAL_SAMPLE_TURRET_THRESHOLD) 
+		i = sensIRRemote(); // SENSORDATA HERE
+		if (i == HIGH) 
 			beacon = true;
 		
 		if (data.beacon_detected[counterTurret] != beacon) {
@@ -786,7 +813,7 @@ bool confirmSample(int turretVal) {
 		if (!counterMovement && counterSampleConfirm == 1) {
 			int sign = currValDirectionDegree < 0 ? 1 : -1;
 			currValDirectionDegree = 60 * sign;
-			counterSampleConfirm = 3
+			counterSampleConfirm = 3;
 		}
 	}
 }
@@ -845,6 +872,12 @@ void setup() {
 		DEBUG_PRINT(compass.GetErrorText(error));
 	}
 	
+	// Input pins
+	pinMode(PIN_SENS_SAMPLE_TURRET, INPUT);
+	pinMode(PIN_SENS_SAMPLE_GRIPPER, INPUT);
+	
+	irrecv.enableIRIn(); // Start the receiver IMPORTANT
+	
 	servoLeft.attach(PIN_SERVO_LEFT);
 	servoRight.attach(PIN_SERVO_RIGHT);
 	servoTurret.attach(PIN_SERVO_TURRET);