diff options
Diffstat (limited to 'Venus_Skeleton')
-rw-r--r-- | Venus_Skeleton/Venus_Skeleton.ino | 50 |
1 files changed, 42 insertions, 8 deletions
diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index 6bb8a4a..82ebc3d 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -20,6 +20,8 @@ #include <HMC5883L.h> #include <Wire.h> #include <TrueRandom.h> +#include <IRremote.h> +#define SONY // ********************** // ** Calibration file ** @@ -65,6 +67,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; @@ -355,13 +362,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; @@ -372,8 +377,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) { @@ -384,6 +389,29 @@ void sensSampleGripper() { } // sensor IR, beacon detection and recognition +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) @@ -392,8 +420,8 @@ void 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) { @@ -853,6 +881,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); |