// TODO // // - 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 #include #include #include #include // ********************** // ** Calibration file ** // ********************** #include "definitions.h" #include "dataTypes.h" // Include Wally or Eve, (un)comment the right one. #define ROVER "wall-e" #include "calibration_wall-e.h" //#include "calibration.eve.h" #include "comm.h" // set 0 to disable debugging // set 1 for debugging with Serial.print (default) // set 2 for debugging over RPi serial #ifndef DEBUG # define DEBUG 1 #endif #if DEBUG == 1 # define DEBUG_PRINT(str) Serial.print(str) #elif DEBUG == 2 # define DEBUG_PRINT(str) serial_print_debug(str) #else # define DEBUG_PRINT(str) do { } while (0) #endif // ***************** // ** DEFINITIONS ** // ***************** // Servos Servo servoLeft; Servo servoRight; Servo servoTurret; Servo servoGripper; // Compass HMC5883L compass; // Beacon IRrecv irrecv(PIN_SENS_BEACON_TURRET); decode_results results; // code send by IR beacon (defined in IRbeacon.ino) #define IRBEACON_CODE 0xa90 // Data variables data_t data; int dataToPiChangedBits = 0; bool RPiActive = true; unsigned long lastCommunication = 0; // Current operation mode opmode_t operationMode = OPMODE_WAIT; boolean initialized = false; unsigned long operationChange = 0; // Location values int currValRobotX = 0; int currValRobotY = 0; int LabX = 0; int LabY = 0; // Number of directions the turret can measure from int turretSequence[NUM_TURRET_DIRECTIONS] = {}; // Sensor values int currValSensCompass = 0; int currValSensObstacleTurretLow = 0; int currValServoTurret = 0; // Actuator values int currValTurret = 0; int currValDirection = 0; int currValDirectionDegree = 0; // Timers unsigned long timerMovementStart = 0; unsigned long timerMovementStop = 0; //unsigned long timerInitialSequence = 0; // TODO unused? /** * If set to 0, then the turret is not rotating and in a stable position. * Else the turret is apparently rotating and cannot be stopped. */ unsigned long timerTurret = 0; /** * Always read the sensors on the turret, unless disabled (for example, when * the measurements become unreliable due to a rotating robot). */ bool turretSensingEnabled = true; // Counters int counterInit = 0; int counterMovement = 0; /** * The orientation of the turret, in range 0..NUM_TURRET_DIRECTIONS-1. */ int counterTurret = 0; int counterTurretWait = 0; int counterSampleConfirm = 0; // ********************** // ** HELPER FUNCTIONS ** // ********************** // prototypes void opMode(opmode_t opmode); void stopAllServos(); int eliminateDirections(int directionArray[]); #if 0 // 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); } } #endif 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; } /** * Given the number of positions $n$, fills the lower half of the array with 0, * 2, 4, ..., $n-1$, then continues filling the upper half with $n-2$, $n-4$, * ..., 5, 3, 1. */ void initTurretSequence(int array[], int numOfPositions) { // for n=7, fill lower half of array with 0, 2, 4, 6 // for n=8, fill lower half of array with 0, 2, 4, 6 for (int i = 0; i < (numOfPositions + 1) / 2; i++) { array[i] = i * 2; } // for n=7, fill upper half of array with 5, 3, 1 // for n=8, fill upper half of array with 7, 5, 3, 1 int x = numOfPositions / 2 * 2 - 1; for (int i = (numOfPositions + 1) / 2; i < numOfPositions; i++) { array[i] = x; x -= 2; } } int findInArray(int n, int array[], int length) { for (int g = 0; g < length; g++) { if (array[g] == n) { return g; } } return -1; } // ********************** // ** PI COMMUNICATION ** // ********************** void interpretData(pi_datatype_t dataType, int message) { switch(dataType) { case PI_DATATYPE_MOVETO: break; case PI_DATATYPE_GRIPPER: break; case PI_DATATYPE_TURRET: break; default: // ignore ???? break; } } // receive data from Raspberry Pi // sequence = ... ????? void readData() { // if data received -> interpretate it // change operation mode + update timer operationMode = OPMODE_WAIT; operationChange = millis(); // for example: interpretData(PI_DATATYPE_MOVETO, 0); if (1 == 1) { lastCommunication = millis(); } } bool checkIfPiAlive() { return false; // CONNECT TO PI TO CHECK IF STILL ALIVE } bool checkRPiActive() { RPiActive = (lastCommunication + MAX_COMMUNICATION_DELAY) > millis() ? 1 : 0; if(!RPiActive) { RPiActive = checkIfPiAlive(); } return RPiActive; } // ********************** // ** SENSOR FUNCTIONS ** // ********************** // sensor Compass, orientation float sensMagnetometer() { MagnetometerRaw raw = compass.ReadRawAxis(); float heading = atan2(raw.YAxis, raw.XAxis); if(heading < 0){ heading += 2*PI; } float headingDegrees = heading * 180/M_PI; // Convert to angles headingDegrees = headingDegrees + 90; // Adjust angle to be relative to front of robot instead of X-direction of sensor if(headingDegrees>360){ headingDegrees=headingDegrees-360; } float north; if(headingDegrees<165){ // When heading is negative (left) of north with 15 degree margin north=headingDegrees-180; // Set angle between north and heading angle } else if(headingDegrees>195){ // When heading is positive (right) of north with 15 degree margin north=headingDegrees-180; // Set angle between north and heading angle } else{ // Else heading is towards north with 15 degree margin both sides north=0; // Set angle between north and heading angle to be 0 } return(north); // Returns angle difference } void sensCompass() { int degree; float readingtotal = 0; // take multiple samples and use the average for(int i = 0; i < NUM_COMPASS_CHECKS; i++){ readingtotal += sensMagnetometer(); } // if compass reading is sane, save it. degree = readingtotal / NUM_COMPASS_CHECKS; if (degree >= 0 && degree <= 360 && data.compass != degree) { data.compass = degree; // mark as changed dataToPiChangedBits |= CHANGED_SENS_COMPASS; } } // sensor Ultrasound, distance measurement 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 (turretSensingEnabled && 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 void sensIRLine(lr_t LorR) { // BE AWARE, LOCALISATION OF LINE IS y DISTANCE and z DEGREE FROM CENTER OF ROBOT, // calculate by RPi?? 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; if (*dataField != inaccessible) { *dataField = inaccessible; // mark as changed dataToPiChangedBits |= changeBit; } } // sensor IR, sample detection void sensSampleTurret() { // BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT // (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners) if (turretSensingEnabled && timerTurret == 0) { int i; bool sample = false; i = digitalRead(PIN_SENS_SAMPLE_TURRET); if (i == LOW) sample = true; if (data.sample_turret_detected[counterTurret] != sample) { data.sample_turret_detected[counterTurret] = sample; // mark as changed dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET; } } } void sensSampleGripper() { int i; bool sample = false; i = digitalRead(PIN_SENS_SAMPLE_GRIPPER); if (i == LOW) sample = true; if(data.sample_gripper_detected != sample) { data.sample_gripper_detected = sample; // Mark as changed dataToPiChangedBits |= CHANGED_SENS_SAMPLEGRIPPER; } } // sensor IR, beacon detection and recognition int sensIRRemote() { int matches = 0; for (int i = 0; i < 10; i++) { if (irrecv.decode(&results)) { // check for the parameters sent by IRbeacon.ino if (results.decode_type == SONY && results.bits == 12 && results.value == IRBEACON_CODE) { matches++; } irrecv.resume(); } } DEBUG_PRINT(matches); DEBUG_PRINT(" IRRemote matches\n"); // accept when half of the probes resulted in a match. return matches >= 5; } void sensBeaconTurret() { // BE AWARE, LOCALISATION OF OBSTACLE IS y DISTANCE FROM CENTER OF ROBOT // (creates triangle with centerOfRobot, centerOfSensor and Obstacle as its corners) if (turretSensingEnabled && timerTurret == 0) { bool beacon; beacon = sensIRRemote(); // SENSORDATA HERE if (data.beacon_detected[counterTurret] != beacon) { data.beacon_detected[counterTurret] = beacon; // mark as changed dataToPiChangedBits |= CHANGED_SENS_SAMPLETURRET; } } } // sensor IR, lab detection void sensLab() { // ... } // ************************ // ** ACTUATOR FUNCTIONS ** // ************************ void maneuver(int speedLeft, int speedRight){ speedLeft *= CAL_SERVO_LEFT; speedRight *= CAL_SERVO_RIGHT; servoLeft.writeMicroseconds(1500 + speedLeft); // Set left servo speed servoRight.writeMicroseconds(1500 - speedRight); // Set right servo speed } void stopMovement() { //DEBUG_PRINT(" Stop Movement\n"); maneuver(0, 0); timerMovementStart = 0; timerMovementStop = 0; // motor is stopped, allow sensor polling again. turretSensingEnabled = true; } void stopAllServos() { //DEBUG_PRINT(" Stop All Servos\n"); maneuver(0, 0); timerTurret = 0; timerMovementStart = 0; timerMovementStop = 0; } /** * Maps directions in the range [0..12] to [0..360] and [12..24] to [0..-360]. */ int directionToDegree(int direction) { int deg = (direction % NUM_DIRECTIONS) * 360 / NUM_DIRECTIONS; // lower half is to the left (so negative, counter-clockwise) return direction < NUM_DIRECTIONS ? -deg : deg; } /** * Rotates the robot to a given direction relative to robot. If the desired * rotation is 0, nothing happens. * If the robot is still making it's turn, calculate it's current orientation. * If the timer is finished, stop movement and reset all ultrasound values. * * @returns true when the rotation is completed. */ bool moveTurnTo(int degree) { if (degree % 360 == 0) { // already in the right orientation, no need to rotate. return true; } // when rotating, the data collected with sensors on the turret becomes // useless as the orientation is not fixed. turretSensingEnabled = false; // if movementtimer is not started, start timer or something and initiate movement // else continue movement and return false // if movementtimer is expired, stop movement. if (!timerMovementStop) { DEBUG_PRINT("\n Start turn move: "); DEBUG_PRINT(degree); DEBUG_PRINT("deg - "); counterMovement++; // timer = now() + direction * formula timerMovementStart = millis(); timerMovementStop = timerMovementStart + CAL_MOVE_TURN * abs(degree); // DO A COMPASS CHECK maneuver(0,0); if (degree < 0) { DEBUG_PRINT(" left\n"); // turn left maneuver(-100,100); } if (degree > 0) { DEBUG_PRINT(" right\n"); // Turn right maneuver(100,-100); } } else { int deg = (int)(CAL_MOVE_TURN * (millis() - timerMovementStart)); deg %= 360; if (degree < 0) { deg *= -1; } // +360 to ensure that the angle stays positive data.robot_curr_deg = (data.robot_curr_deg + deg + 360) % 360; } if (timerMovementStop < millis()) { DEBUG_PRINT("\n Stop turn move"); stopMovement(); // reset the ultrasound measurements for (int k = 0; k < NUM_TURRET_DIRECTIONS; k++) { data.obstacle_turret_distances[k] = OBSTACLE_FAR_AWAY; } // rotation is completed. return true; } // in other cases the rotation is not completed yet. return false; } void moveStraight(int direction, float distance) { if (direction != 0 && direction != NUM_DIRECTIONS) { return; } // Convert distance into drive time float drivetime = distance / CAL_MOVE_STRAIGHT; if (!timerMovementStop) { //DEBUG_PRINT(" Start straight move: "); //DEBUG_PRINT(distance); //DEBUG_PRINT(" - "); counterMovement++; // timer = now() + direction * formula timerMovementStart = millis(); timerMovementStop = millis() + drivetime; // If input is positive drive forward if(distance > 0) { //DEBUG_PRINT(" fw\n"); //maneuver(100, 100); } // If input is negative drive backwards if(distance < 0){ //DEBUG_PRINT(" bw\n"); //maneuver(-100, -100); } } else { float dist = (millis() - timerMovementStart) * CAL_MOVE_STRAIGHT; data.robot_curr_x += dist * sin((data.robot_curr_deg/180.0)*PI); data.robot_curr_y += dist * cos((data.robot_curr_deg/180.0)*PI); } if (timerMovementStop < millis()) { stopMovement(); counterMovement = 0; } } void turnTurretTo(int deg) { deg = deg + CAL_TURRET_DEGREE; servoTurret.write(deg); } void turnTurretToNext() { if(timerTurret == 0) { counterTurret = (counterTurret + 1) % NUM_TURRET_DIRECTIONS; counterTurretWait++; int deg; // turret can rotate in range 0..120 degree. For the lower half of the // counterTurret, rotate from left to right (0, 2, 4, ...), then back // from right to left (.., 5, 3, 1). deg = turretSequence[counterTurret] * 120 / (NUM_TURRET_DIRECTIONS - 1); timerTurret = millis(); turnTurretTo(deg); } // If the turret is finished with rotating, mark it as idle. if ((timerTurret + 100) < millis()) { timerTurret = 0; } } // ********************* // ** LOGIC FUNCTIONS ** // ********************* void opMode(opmode_t opmode) { DEBUG_PRINT("\nOPMODE"); DEBUG_PRINT(opmode); DEBUG_PRINT("\n\n"); if(operationMode != opmode) { counterSampleConfirm = 0; } operationMode = opmode; counterTurretWait = 0; } /** * Given the current known state (including sensor data), find out whether the * robot can still move forward. */ void checkFreePath() { // Stop moving if the IR line sensor found a black spot if ((dataToPiChangedBits & (CHANGED_SENS_IRLEFT | CHANGED_SENS_IRRIGHT)) && (data.IR_left_detected || data.IR_right_detected)) { // Left or Right IR sensor sees inaccessible terrain opMode(OPMODE_WAIT); DEBUG_PRINT(" NO FREE PATH: left-"); DEBUG_PRINT(data.IR_left_detected); DEBUG_PRINT(" - right-"); DEBUG_PRINT(data.IR_right_detected); // no need to check for other data, we cannot move anyway... return; } // Ultrasound // Only check for obstacles for turret measurements in the middle, that is, // ignore measurements from the far left and right positions. if (// (dataToPiChangedBits & CHANGED_SENS_OBSTACLETURRET) && (turretSequence[counterTurret] >= (NUM_TURRET_DIRECTIONS / 6)) && (turretSequence[counterTurret] < (NUM_TURRET_DIRECTIONS - (NUM_TURRET_DIRECTIONS / 6))) && (data.obstacle_turret_distances[counterTurret] < OBSTACLE_TOO_CLOSE)) { // Ultrasound sensor sees inaccessible terrain opMode(OPMODE_WAIT); DEBUG_PRINT(" NO FREE PATH: Obstacle turret index: "); DEBUG_PRINT(counterTurret); DEBUG_PRINT(" - "); DEBUG_PRINT(data.obstacle_turret_distances[counterTurret]); } } int checkBestRoute(int preferedDirection) { // 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 if (counterMovement) { return 0; } // 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] = {}; for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) { if (preferedDirection == -1) { directionArray[i] = 1; } else { directionArray[i] = 1 + abs((int)(pow(2, abs((int)(abs(((NUM_DIRECTIONS - abs(i - NUM_DIRECTIONS)) % NUM_DIRECTIONS) - preferedDirection) - (float)NUM_DIRECTIONS / 2))))); } // limit to first 180deg if( (i % NUM_DIRECTIONS) > (NUM_DIRECTIONS / 2) ) { directionArray[i] = 0; } } if (!eliminateDirections(directionArray)) { // We are screwed!!! Basically robot is dead or trapped. return false; while(1) { delay(1000); } } int direction = 0; int sumDirectionPriority = 0; int randValue = 0; for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) { sumDirectionPriority += directionArray[i]; } do { randValue = TrueRandom.random(sumDirectionPriority); for (int i = 0; i < (NUM_DIRECTIONS * 2); i++) { randValue -= directionArray[i]; if (randValue < 0) { direction = i--; break; } } } while (!directionArray[direction]); currValDirection = 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++) { // 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_right_detected && ( (i < ((float)NUM_DIRECTIONS / 4.0)) || (i > ((float)NUM_DIRECTIONS / 4.0)*2.0) )) { directionArray[i] = 0; } // ULTRASOUND PANIC! Check the obstacle_turret_distances array to see if there are unallowed directions if ((i < (float)NUM_DIRECTIONS / 4.0) || ((i > (((float)NUM_DIRECTIONS / 4.0)*3.0)) && (i < (((float)NUM_DIRECTIONS / 4.0)*5.0))) || (i > (((float)NUM_DIRECTIONS / 4.0)*7.0))) { // Incremental value for turret directions per robot direction float kinc = ((((float)NUM_TURRET_DIRECTIONS - 1) * 3.0) / (float)NUM_DIRECTIONS); // left most turret direction for this robot direction int kinit = ((int)((float)(i % NUM_DIRECTIONS) + (float)NUM_DIRECTIONS / 6.0) % NUM_DIRECTIONS) * kinc - kinc; // for all turret directions relative to this robot direction, check obstacle sensor data for (int k = kinit; k <= (kinit + 2*kinc); k++) { if (k < 0) k = 0; if (data.obstacle_turret_distances[findInArray(k, turretSequence, NUM_TURRET_DIRECTIONS)] < OBSTACLE_TOO_CLOSE) { directionArray[i] = 0; } } } //DEBUG_PRINT(" "); //DEBUG_PRINT(directionArray[i]); 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; } void checkSample() { // 37 is an arbitrary magic number that specifies the acceptable distance // for attempting to get the sample. This ensures that the robot does not // hit a hill or something. if ((dataToPiChangedBits & (CHANGED_SENS_SAMPLETURRET | CHANGED_SENS_SAMPLEGRIPPER)) && (data.sample_turret_detected[counterTurret] || data.sample_gripper_detected) && (data.obstacle_turret_distances[counterTurret] > 37)) { DEBUG_PRINT("\n -- FOUND SAMPLE?\n"); // One of the turret sees a sample (or the lab) opMode(OPMODE_CHECKSAMPLE); // If sample is straight ahead, position the turret to the front such // that the IR sensor can get a confirmation. if (data.sample_gripper_detected) { // If turretSequence has odd number of entries, there is a straight forward position at the NUM/4 index. // See initTurretSequence function for more information. counterTurret = NUM_TURRET_DIRECTIONS / 4; } } } void confirmSample(int turretVal) { // TODO use turretVal if (!counterSampleConfirm) { stopAllServos(); // Turret direction in degrees calculated from forward. (negative is left, positive is right) int checkTurretDeg = (int)((turretSequence[counterTurret] - ((NUM_TURRET_DIRECTIONS - 1) / 2)) * 10 * -1); int sign = checkTurretDeg < 0 ? -1 : 1; // Calculate angle the robot needs to turn to point in direction of possible sample. int checkTurretDegAbs = 90 - abs(checkTurretDeg); int x = CAL_SAMPLE_TURRET_MAX_DIST * cos((checkTurretDegAbs/180.0)*PI); int y = CAL_SAMPLE_TURRET_MAX_DIST * sin((checkTurretDegAbs/180.0)*PI) + CAL_TURRET_TO_CENTER_DIST; int currValDirectionDegree = (int)((atan2(x, y)/PI) * 180.0); currValDirectionDegree += (x == 0 ? 0 : 10); currValDirectionDegree *= sign; DEBUG_PRINT("\n currvaldegi = "); DEBUG_PRINT(currValDirectionDegree); counterSampleConfirm++; } else { DEBUG_PRINT("\n currvaldege = "); DEBUG_PRINT(currValDirectionDegree); // Check if a sample was seen by the gripperSampleDetector and the movement to drive the grippers over it are done if (!counterMovement && counterSampleConfirm == 2) { // Pick it up DEBUG_PRINT("\n---GRAB SAMPLE"); opMode(OPMODE_GRABSAMPLE); } // Check if a sample is seen by the gripperSampleDetector if (data.sample_gripper_detected && (counterSampleConfirm == 1 || counterSampleConfirm == 3)) { stopMovement(); DEBUG_PRINT("\nsample detected"); currValDirectionDegree = 0; counterSampleConfirm = 2; } // Check if movement has stopped (turn and forward), without seeing a sample (second turn). -> False positive if (!counterMovement && counterSampleConfirm == 3) { DEBUG_PRINT("\n---FALSE POSITIVE"); opMode(OPMODE_WAIT); } // Check if movement has stopped (turn and forward), without seeing a sample (first turn). -> Turn other way if (!counterMovement && counterSampleConfirm == 1) { DEBUG_PRINT("\ncheck other side"); int sign = currValDirectionDegree < 0 ? 1 : -1; currValDirectionDegree = 60 * sign; counterSampleConfirm = 3; } } } void initialSequence() { do { moveStraight(0, CAL_INITIAL_DISTANCE1); } while(timerMovementStop); // Set x and y value of robot to (0, 0) for lab/starting location. data.robot_curr_x = 0; data.robot_curr_y = 0; do { moveStraight(0, CAL_INITIAL_DISTANCE2); } while(timerMovementStop); opMode(OPMODE_WAIT); } void calculateOrientation() { // current + compass } // *********************** // ** ARDUINO FUNCTIONS ** // *********************** void setup() { // initial communication between Arduino and RPi // Send some values: // - Number of possible directions Wire.begin(); #ifdef DEBUG Serial.begin(9600); #endif DEBUG_PRINT("\n\n\n** ROVER "); DEBUG_PRINT(ROVER); DEBUG_PRINT(" **\n\n"); initTurretSequence(turretSequence, NUM_TURRET_DIRECTIONS); for(int i = 0; i < NUM_TURRET_DIRECTIONS; i++) { DEBUG_PRINT(turretSequence[i]); DEBUG_PRINT(" "); } DEBUG_PRINT("\n\n"); // initialise compass communication compass = HMC5883L(); // Gauss scale 1.3 int error = compass.SetScale(1.3); if(error != 0){ DEBUG_PRINT(compass.GetErrorText(error)); DEBUG_PRINT("\n"); } error = compass.SetMeasurementMode(Measurement_Continuous); if(error != 0){ 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); servoGripper.attach(PIN_SERVO_GRIPPER); turnTurretTo(0); delay(100); // Make sure all actuators are at their starting position } void loop() { // read out sensors // x sensCompass(); sensObstacleTurret(); sensIRLine(L); sensIRLine(R); sensSampleTurret(); sensSampleGripper(); //sensLab(); // processes sensors, setting the next action (opmode) checkSample(); checkFreePath(); // calculateOrientation(); // calculateLocationOffset(); // finally execute the next action switch(operationMode) { case OPMODE_WAIT: stopMovement(); if (!initialized) { // whenever the initial sequence to locate the robot has not happened, // keep performing that sequence opMode(OPMODE_INITIALSEQUENCE); } else { // goto standalone algorithm when idle opMode(OPMODE_MAPPING); } turnTurretToNext(); break; case OPMODE_INITIALSEQUENCE: initialSequence(); initialized = true; break; case OPMODE_RANDOM: break; case OPMODE_MAPPING: turnTurretToNext(); checkBestRoute(0); if (moveTurnTo(directionToDegree(currValDirection))) { currValDirection = 0; } moveStraight(currValDirection, 100.0); // find distance break; case OPMODE_CHECKSAMPLE: // Sequence: // - Turn a little further then turret direction or until sample detected // - Drive forward 15 cm (either to pick up, or try a little further) // - Turn other way for 60deg or until sample detected // - Drive forward 15 cm (either to pick up, or to finish sequence) // - Either a false positive or a success confirmSample(counterTurret); turnTurretToNext(); if (moveTurnTo(currValDirectionDegree)) { DEBUG_PRINT("\nMoveTurnToComplete"); currValDirectionDegree = 0; } moveStraight(currValDirectionDegree, 15.0); break; case OPMODE_GRABSAMPLE: // Close grippers to calibration value (sample sensor should not see grippers) break; case OPMODE_GOTOLABLOCATION: sensBeaconTurret(); // check if sample still there. // calculate prefered direction to (0, 0) break; case OPMODE_WAITFORLAB: break; case OPMODE_FINDMAGNET: break; case OPMODE_LABSEQUENCE: break; default: break; } // mark all data as read dataToPiChangedBits = 0; // Delay for stability. delay(2); }