From 86938f1938f6711c6cad9ae4347e8f059aea7ffb Mon Sep 17 00:00:00 2001 From: Peter de Kok Date: Fri, 22 May 2015 13:35:34 +0200 Subject: 4th - with example of bitwise sensor changes --- Venus_Skeleton/Venus_Skeleton.ino | 177 ++++++++++++++++++++++++++++-------- Venus_Skeleton/calibration.wall-e.h | 6 ++ 2 files changed, 147 insertions(+), 36 deletions(-) create mode 100644 Venus_Skeleton/calibration.wall-e.h (limited to 'Venus_Skeleton') diff --git a/Venus_Skeleton/Venus_Skeleton.ino b/Venus_Skeleton/Venus_Skeleton.ino index f4cc57b..6b901c8 100644 --- a/Venus_Skeleton/Venus_Skeleton.ino +++ b/Venus_Skeleton/Venus_Skeleton.ino @@ -3,47 +3,75 @@ // - limit sendData to x per second // // - insert into calibration files -// const int calObstacleTurretMaxDist = 50; -// const int calObstacleTurretMinDist = 10; -// const int calIRLeftThreshold = 2; -// const int calIRRightThreshold = 2; -// #define OPERATION_MAX_CHANGE_TIME = 20000; +// // // - Make sure ENUM piDataType is correct and synced // - Change interpretData to resemble correct ENUM data // +#include + // ********************** // ** Calibration file ** // ********************** // Include Wally of Eve, (un)comment the right one. -#include "calibration.wally.h" +#include "calibration.wall-e.h" //#include "calibration.eve.h" + // ***************** // ** DEFINITIONS ** // ***************** // Pin configuration -#define PIN_SENS_COMPASS; -#define PIN_SENS_OBSTACLE_TURRET = 9; -#define PIN_SENS_IR_LEFT; -#define PIN_SENS_IR_RIGHT; -#define PIN_SENS_SAMPLE_TURRET; -#define PIN_SENS_SAMPLE_GRIPPER; -#define PIN_SENS_BEACON_TURRET; -#define PIN_SERVO_LEFT = 12; +#define PIN_SENS_COMPASS +#define PIN_SENS_OBSTACLE_TURRET 9 +#define PIN_SENS_IR_LEFT A1 +#define PIN_SENS_IR_RIGHT A0 +#define PIN_SENS_SAMPLE_TURRET +#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; +// Servos +Servo servoLeft; // Declare left servo signal +Servo servoRight; // Declare right servo signal + + // Raspberry Pi Data type declarations -enum piDataType {COMPASS, OBSTACLETURRET, IRLEFT, IRRIGHT, SAMPLETURRET, SAMPLEGRIPPER, BEACONTURRET, LAB, MOVETO, GRIPPER, TURRET}; +typedef enum { + PI_DATATYPE_COMPASS, + PI_DATATYPE_OBSTACLETURRET, + PI_DATATYPE_IRLEFT, + PI_DATATYPE_IRRIGHT, + PI_DATATYPE_SAMPLETURRET, + PI_DATATYPE_SAMPLEGRIPPER, + PI_DATATYPE_BEACONTURRET, + PI_DATATYPE_LAB, + PI_DATATYPE_MOVETO, + PI_DATATYPE_GRIPPER, + PI_DATATYPE_TURRET +} pi_datatype_t; // Operation modes -enum opMode {RANDOM, WAIT, INITIALSEQUENCE, RANDOM, MAPPING, GOTOSAMPLE, GRABSAMPLE, GOTOLABLOCATION, WAITFORLAB, FINDMAGNET, LABSEQUENCE}; +typedef enum { + OPMODE_RANDOM, + OPMODE_WAIT, + OPMODE_INITIALSEQUENCE, + OPMODE_RANDOM, + OPMODE_MAPPING, + OPMODE_GOTOSAMPLE, + OPMODE_GRABSAMPLE, + OPMODE_GOTOLABLOCATION, + OPMODE_WAITFORLAB, + OPMODE_FINDMAGNET, + OPMODE_LABSEQUENCE +} opmode_t; // Current operation mode -opMode operationMode = 0; +opmode_t operationMode = OPMODE_WAIT; unsigned long operationChange = 0; // Location values @@ -64,7 +92,7 @@ int currValTurret; // ********************** // sendData to Raspberry Pi -void sendData(int method, int data, int data1 = null, int data2 = null) { +void sendData(pi_datatype_t method, int data, int data1 = null, int data2 = null) { } @@ -74,21 +102,21 @@ void readData() { // if data received -> interpretate it // change operation mode + update timer - operationMode = WAIT; + operationMode = OPMODE_WAIT; operationChange = millis(); // for example: - interpretData(MOVETO, 0); + interpretData(PI_DATATYPE_MOVETO, 0); } -void interpretData(piDataType dataType, int message) { +void interpretData(pi_datatype_t dataType, int message) { switch(dataType) { - case MOVETO: + case PI_DATATYPE_MOVETO: break; - case GRIPPER: + case PI_DATATYPE_GRIPPER: break; - case TURRET: + case PI_DATATYPE_TURRET: break; case default: @@ -106,7 +134,7 @@ void interpretData(piDataType dataType, int message) { int sensCompass() { // average for last x ???????? int degree = 359; - sendData(COMPASS, degree); + sendData(PI_DATATYPE_COMPASS, degree); return degree; } @@ -114,26 +142,36 @@ int sensCompass() { int sensObstacleTurret() { int distance = 100; int turretAngle = 90; - sendData(OBSTACLETURRET, distance, turretAngle); + sendData(PI_DATATYPE_OBSTACLETURRET, distance, turretAngle); return distance; } // sensor IR, line detection boolean sensIRLeft() { + int i; + boolean inaccessible = false; + i = analogRead(PIN_SENS_IR_LEFT); + if (i > CAL_IR_LEFT_THRESHOLD) + inaccessible = true; + // Inaccessible terrain. True for inaccessible, false for accessible - boolean inaccessible = false; if(inaccessible) { stopMovement(); - sendData(IRLEFT, currValRobotX, currValRobotY, currValSensCompass); + 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; + // Inaccessible terrain. True for inaccessible, false for accessible - boolean inaccessible = false; if(inaccessible) { stopMovement(); - sendData(IRRIGHT, currValRobotX, currValRobotY, currValSensCompass); + sendData(PI_DATATYPE_IRRIGHT, currValRobotX, currValRobotY, currValSensCompass); } return inaccessible; } @@ -144,14 +182,14 @@ int sensSampleTurret() { if(sample) { // If a sample is detected, stop all servos, send data to the PI and wait for response stopAllServos(); - sendData(SAMPLETURRET, currValRobotX, currValRobotY, currValSensCompass, currValTurret); + sendData(PI_DATATYPE_SAMPLETURRET, currValRobotX, currValRobotY, currValSensCompass, currValTurret); } } int sensSampleGripper() { boolean sample = false; if(sample) { stopAllServos(); - sendData(SAMPLEGRIPPER, currValRobotX, currValRobotY, currValSensCompass); + sendData(PI_DATATYPE_SAMPLEGRIPPER, currValRobotX, currValRobotY, currValSensCompass); } } @@ -173,10 +211,14 @@ int sensLab() { // ************************ void stopMovement() { - + operationMode = OPMODE_WAIT; + servoLeft.detach(); + servoRight.detach(); } void stopAllServos() { - + operationMode = OPMODE_WAIT; + servoLeft.detach(); + servoRight.detach(); } // *********************** @@ -197,8 +239,14 @@ void loop() { sensBeaconTurret(); sensLab(); - if((operationChange + OPERATION_MAX_CHANGE_TIME) > millis() && operationMode != RANDOM) { - operationMode = RANDOM; + switch(operationMode) { + case OPMODE_WAIT: + stopMovement(); + break; + + default: + stopMovement(); + break; } } @@ -224,3 +272,60 @@ long microsecondsToCentimeters(long microseconds) // object we take half of the distance travelled. return microseconds / 29 / 2; } + + + + + + + + + + + + + + + + + +// 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(); +if (data.left_detected != val) { + data.left_detected = val; + // mark as changed + changedBits |= CHANGED_IR_LEFT; +} +// other sensors: same + +// 2. read whetehr you have hit a block / line sensor +if ((changedBits & (CHANGED_IR_LEFT | CHANGED_IR_RIGHT))) && + (data.left_detected || data.right_detected)) { + // panic! + stopMotor(); +} + +// 3. send to pi or other calculations (and clear bits) +sendData(PIN_IR_LEFT, data.left_detected); +// clear bits +changedBits &= ~CHANGED_IR_LEFT; \ No newline at end of file diff --git a/Venus_Skeleton/calibration.wall-e.h b/Venus_Skeleton/calibration.wall-e.h new file mode 100644 index 0000000..e667d56 --- /dev/null +++ b/Venus_Skeleton/calibration.wall-e.h @@ -0,0 +1,6 @@ +// WALLY configuration file + +#define calObstacleTurretMaxDist 50; +#define calObstacleTurretMinDist 10; +#define CAL_IR_LEFT_THRESHOLD 600 +#define CAL_IR_RIGHT_THRESHOLD 600 \ No newline at end of file -- cgit v1.2.1