From 289f6213ba9e82b15e0cc14d9e828f10b9aed9f1 Mon Sep 17 00:00:00 2001 From: Peter de Kok Date: Sat, 23 May 2015 02:30:33 +0200 Subject: Changed some shit: Added a direction array with variable size, started implemented this. Rest of skeleton is coming along great. IR Left / Right might be done. --- Venus_Skeleton/Venus_Skeleton.ino | 270 ++++++++++++++++++++++++++++-------- Venus_Skeleton/calibration.wall-e.h | 7 +- 2 files changed, 215 insertions(+), 62 deletions(-) (limited to 'Venus_Skeleton') diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index 6b901c8..e5c0869 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -1,14 +1,21 @@ // TODO // // - limit sendData to x per second -// +// - errorSequence() // - insert into calibration files -// -// // - Make sure ENUM piDataType is correct and synced // - Change interpretData to resemble correct ENUM data // +// ***************** +// ** ERROR CODES ** +// ***************** +// +// 0 no error +// 1 Error: Sensor Compass error +// 2 Error: Sensor Obstacle detection on the turret +// 3 Error: Sensor IR Line detection + #include // ********************** @@ -25,6 +32,7 @@ // ***************** // Pin configuration +# #define PIN_SENS_COMPASS #define PIN_SENS_OBSTACLE_TURRET 9 #define PIN_SENS_IR_LEFT A1 @@ -40,6 +48,30 @@ Servo servoLeft; // Declare left servo signal Servo servoRight; // Declare right servo signal +// Types + +// Sensor data +typedef struct { + // ir + bool IR_left_detected; + bool IR_right_detected; // usually an integer (16-bit) + int distance; // 16-bit + // .. + long timer; // 32-bit +} sensor_data_t; + +enum { + CHANGED_SENS_COMPASS = 1 << 0, + CHANGED_SENS_OBSTACLETURRET = 1 << 1, + CHANGED_SENS_IRLEFT = 1 << 2, + CHANGED_SENS_IRRIGHT = 1 << 3, + CHANGED_SENS_SAMPLETURRET = 1 << 4, + CHANGED_SENS_SAMPLEGRIPPER = 1 << 5 +}; + +sensor_data_t data; +int dataToPiChangedBits = 0; + // Raspberry Pi Data type declarations typedef enum { @@ -78,6 +110,9 @@ unsigned long operationChange = 0; int currValRobotX; int currValRobotY; +// 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; @@ -119,7 +154,7 @@ void interpretData(pi_datatype_t dataType, int message) { case PI_DATATYPE_TURRET: break; - case default: + default: // ignore ???? break; } @@ -147,33 +182,40 @@ int sensObstacleTurret() { } // sensor IR, line detection -boolean sensIRLeft() { +bool sensIRLine(char LorR) { int i; - boolean inaccessible = false; - i = analogRead(PIN_SENS_IR_LEFT); - if (i > CAL_IR_LEFT_THRESHOLD) - inaccessible = true; + bool inaccessible = false; + + if (LorR == 'L') { + i = analogRead(PIN_SENS_IR_LEFT); + if (i > CAL_IR_LEFT_THRESHOLD) + inaccessible = true; - // Inaccessible terrain. True for inaccessible, false for accessible - if(inaccessible) { - stopMovement(); - sendData(PI_DATATYPE_IRLEFT, currValRobotX, currValRobotY, currValSensCompass); - } - return inaccessible; -} -int sensIRRight() { - int i; - boolean inaccessible = false; - i = analogRead(PIN_SENS_IR_RIGHT); - if (i > CAL_IR_RIGHT_THRESHOLD) - inaccessible = true; + if (data.IR_left_detected != inaccessible) { + data.IR_left_detected = inaccessible; + // mark as changed + dataToPiChangedBits |= CHANGED_SENS_IRLEFT; + } - // Inaccessible terrain. True for inaccessible, false for accessible - if(inaccessible) { - stopMovement(); - sendData(PI_DATATYPE_IRRIGHT, currValRobotX, currValRobotY, currValSensCompass); - } - return inaccessible; + return inaccessible; + } + + 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; + } + + return inaccessible; + } + + error(3); + return false; } // sensor IR, sample detection @@ -186,7 +228,19 @@ int sensSampleTurret() { } } int sensSampleGripper() { - boolean sample = false; + int i; + bool sample = false; + + i = false; // sensor readout here + if (i > CAL_SAMPLE_GRIPPER_THRESHOLD) + sample = true; + + if(data.sample_gripper != sample) { + data.sample_gripper = sample; + // Mark as changed + dataToPiChangedBits |= CHANGED_SENS_SAMPLEGRIPPER; + } + if(sample) { stopAllServos(); sendData(PI_DATATYPE_SAMPLEGRIPPER, currValRobotX, currValRobotY, currValSensCompass); @@ -220,34 +274,125 @@ void stopAllServos() { servoLeft.detach(); servoRight.detach(); } +bool turnTo(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. + +} + +// ********************* +// ** LOGIC FUNCTIONS ** +// ********************* + +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(); + } + + // Ultrasound + +} + +int checkBestRoute() { + // IMPLEMENT SOME TIMER HERE... + // 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 + + // Array with all directions (for example with 12 directions): + // Bottom half: ([00-11] % 12) * (360/12) degrees to the left + // Top half: ([12-23] % 12) * (360/12) degrees to the right + int directionArray[NUM_DIRECTIONS * 2] = {}; + + if (!eliminateDirections(directionArray)) { + // We are screwed!!! Basically robot is dead or trapped. + return false; + } + + int direction = 0; + do { + direction = millis() % (NUM_DIRECTIONS * 2); + } while (!directionArray[direction]); + + return direction; +} + +int eliminateDirections(int directionArray[]) { + int numPossibleDirections = 0; + // Implement PI's choices (if still possible) + // int numPossibleDirectionsPi = 0; + for (int i = 0; i < NUM_DIRECTIONS * 2; i++) { + directionArray[i] = 1; + } + + for (int i = 0; i < NUM_DIRECTIONS * 2; i++) { + // LINE PANIC! If a line is detected on the left, only a right turn is allowed between 90deg and 180deg + if (data.IR_left_detected && ( (i < ((float)NUM_DIRECTIONS / 4.0) + NUM_DIRECTIONS) || (i > ((float)NUM_DIRECTIONS / 4.0) * 2.0 + NUM_DIRECTIONS)) )) { + 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) )) { + directionArray[i] = 0; + } + // ULTRASOUND PANIC! If ... + + + if (directionArray[i]) { + numPossibleDirections++; + // Implement PI's choices (if still possible) + // if(PIarray[i]) {numPossibleDirectionsPi++;} + } + } + + // Implement PI's choices (if still possible) + // if (numPossibleDirectionsPi) { + //for (int i = 0; i < NUM_DIRECTIONS * 2; i++) { + // if(!PIarray[i]) {directionArray[i] = 0} + // } + // } + + return numPossibleDirections; +} // *********************** // ** ARDUINO FUNCTIONS ** // *********************** void setup() { + // initial communication between Arduino and RPi + // Send some values: + // - Number of possible directions } void loop() { sensCompass(); sensObstacleTurret(); - sensIRLeft(); - sensIRRight(); + sensIRLine('L'); + sensIRLine('R'); sensSampleTurret(); sensSampleGripper(); sensBeaconTurret(); sensLab(); - switch(operationMode) { - case OPMODE_WAIT: - stopMovement(); - break; - - default: - stopMovement(); - break; - } + checkFreePath(); + + // calculateLocation(); + + sendData(); + dataToPiChangedBits = 0; // Clear changed bits + readData(); + + // Operations (probably in a switch or something, according to operation modes) + turnTo(checkBestRoute()); + // driveStraight(someDistanceOrTime); + } @@ -255,6 +400,12 @@ void loop() { // ** HELPER FUNCTIONS ** // ********************** +// error helper +error(int errorCode) { + stopAllServos(); + //errorSequence(); +} + long microsecondsToInches(long microseconds) { // According to Parallax's datasheet for the PING))), there are @@ -289,25 +440,26 @@ long microsecondsToCentimeters(long microseconds) -// define types -typedef struct { - // ir - bool left_detected; - bool right_detected; // usually an integer (16-bit) - int distance; // 16-bit - // .. - long timer; // 32-bit -} sensor_data_t; - -enum { - CHANGED_IR_LEFT = 1 << 0, - CHANGED_IR_RIGHT = 1 << 1, - CHNAGED_JDHCHDUCDH = 1 << 2, - CHANGED_IDJDHD = 1 << 3 -}; - -sensor_data_t data; -int changedBits = 0; +// // define types +// typedef struct { +// // ir +// bool left_detected; +// bool right_detected; // usually an integer (16-bit) +// int distance; // 16-bit +// // .. +// long timer; // 32-bit +// } sensor_data_t; +// +// enum { +// CHANGED_IR_LEFT = 1 << 0, +// CHANGED_IR_RIGHT = 1 << 1, +// CHNAGED_JDHCHDUCDH = 1 << 2, +// CHANGED_IDJDHD = 1 << 3 +// }; +// +// sensor_data_t data; +// int changedBits = 0; +// // 1. gather sensor data -< sets changedBits and data val = readFromSensor(); diff --git a/Venus_Skeleton/calibration.wall-e.h b/Venus_Skeleton/calibration.wall-e.h index e667d56..a7adf7a 100644 --- a/Venus_Skeleton/calibration.wall-e.h +++ b/Venus_Skeleton/calibration.wall-e.h @@ -1,6 +1,7 @@ // WALLY configuration file -#define calObstacleTurretMaxDist 50; -#define calObstacleTurretMinDist 10; +#define calObstacleTurretMaxDist 50 +#define calObstacleTurretMinDist 10 #define CAL_IR_LEFT_THRESHOLD 600 -#define CAL_IR_RIGHT_THRESHOLD 600 \ No newline at end of file +#define CAL_IR_RIGHT_THRESHOLD 600 +#define CAL_SAMPLE_GRIPPER_THRESHOLD x \ No newline at end of file -- cgit v1.2.1