#include #define USE_ROLLINGAVERAGE #define USE_DERIVATIVE // Use of USE_BANG_BANG will override USE_ROLLINGAVERAGE and USE_DERIVATIVE #define USE_BANG_BANG Servo RightServo; Servo Left_Servo; const int ledPin = 13; const int LE_RefDesignMin = 100; const int LE_RefDesignMax = 250; const int LE_PeakDesignMin = 300; const int LE_PeakDesignMax = 1000; const int RE_RefDesignMin = 100; const int RE_RefDesignMax = 320; const int RE_PeakDesignMin = 370; const int RE_PeakDesignMax = 1000; const int RightBumperPin = 3; const int Left_BumperPin = 2; int RightBumperState = 0; int Left_BumperState = 0; // The right and left side ambient photocells are used to get a sense // of the ambient light and reflective properties of the surface. Their // main function is to stop the robot from attending to bright ambient // light, such as from a window, and heading towards it. const int RightAmbPin = A4; const int Left_AmbPin = A3; // These values are the raw inputs from the 'ambient' photocells int RightAmbVal = 0; int Left_AmbVal = 0; const int RightEyePin = A5; const int Left_EyePin = A2; // These values are the raw inputs from the 'eye' photocells int RightEyeVal = 0; int Left_EyeVal = 0; // Stuff for the running inertial values based on the incoming xxxxxEyeVals int i = 0 ; const int ArrayLength = 4 ; // 20msec * 25 appx .5sec int RightEyeArray [ArrayLength] ; int Left_EyeArray [ArrayLength] ; float RightEyeComputedVal = 0.0; float Left_EyeComputedVal = 0.0; // if USE_DERIVATIVE, the reloads are used to fill the array with the new higher value. int reloadRight = 0 ; int reload_Left = 0 ; // Sometimes, drive behaviours can be put in simple talk-aboutable // terms like those in the enums below. // It is the responsibility of the motor driver routine, with help // from the motor drive values constants below, to implement the // requested behavior, and that includes comsuming time with the // delay() call. typedef enum { park, ahead_quarter, ahead_half, ahead_full, back_quarter, back_half, back_full, right_turn_forward_full, right_turn_forward_half, right_turn_forward_quarter, backup_to_the_right_full, left_turn_forward_full, left_turn_forward_half, left_turn_forward_quarter, backup_to_the_left_full, right_rotate_fast, right_rotate_slow, left_rotate_fast, left_rotate_slow } drive_behavior ; void drive (drive_behavior behavior) ; // Motor Drive values were empirically determined. const int halt = 90; const int RaheadFull = 0; const int RaheadHalf = 50; const int RaheadQuarter = 65; const int RreverseQuarter = 115; const int RreverseHalf = 130; const int RreverseFull = 180; const int LaheadFull = 180; const int LaheadHalf = 130; const int LaheadQuarter = 115; const int LreverseQuarter = 65; const int LreverseHalf = 50; const int LreverseFull = 0; // The values sent to the servos. int RightSpeed ; int Left_Speed ; // These are the reference values that are overwritten // during calibration. They are initialized to extremes. int RE_Low_Ref = 2000 ; int LE_Low_Ref = 2000 ; int RA_MIN = 2000 ; int LA_MIN = 2000 ; int RE_HighRef = 0 ; int LE_HighRef = 0 ; int RightEyePeak = 0 ; int Left_EyePeak = 0 ; int RA_MAX = 0 ; int LA_MAX = 0 ; // These will be the final reference values used in calculations int RE_Ref = 0 ; int LE_Ref = 0 ; // The time requierd to spin around at left_rotate_slow is 3550, so // the 7000 value is enough to // 1. spin around once to sense the floor, then // 2. sense the peak LASER values (but you must *give* it some LASER. int calibrated = 7000 ; // Used in BANG_BANG int Rthresh = 0 ; int Lthresh = 0 ; // Proportional control is a y=mx + b thing. float mR = 0 ; float mL = 0 ; float bR = 0 ; float bL = 0 ; int firsttimethrough = 1 ; void setup() { RightServo.attach(9); Left_Servo.attach(10); Serial.begin(9600); pinMode(ledPin, OUTPUT); pinMode(RightBumperPin, INPUT); pinMode(Left_BumperPin, INPUT); drive(park) ; // Start with motors at speed zero. } void loop() { // get the state of the outside world: RightBumperState = digitalRead(RightBumperPin); Left_BumperState = digitalRead(Left_BumperPin); RightEyeVal = analogRead(RightEyePin); Left_EyeVal = analogRead(Left_EyePin); RightAmbVal = analogRead(RightAmbPin); Left_AmbVal = analogRead(Left_AmbPin); // --------------------------- Calibrate --------------------------- if (calibrated >= 0 ) { // ----------------- Calibrate ambient values ------------------ if (calibrated >= 3500 ) { // The 100 check is so that it doesn't seize low power-on values for the reference // when the loop is just starting if ((RightEyeVal > 100) && (RightEyeVal < RE_Low_Ref)) {RE_Low_Ref = RightEyeVal ;} if (RightEyeVal > RE_HighRef) {RE_HighRef = RightEyeVal ;} if ((Left_EyeVal > 100) && (Left_EyeVal < LE_Low_Ref)) {LE_Low_Ref = Left_EyeVal ;} if (Left_EyeVal > LE_HighRef) {LE_HighRef = Left_EyeVal ;} if (RightAmbVal < RA_MIN) {RA_MIN = RightAmbVal ;} if (Left_AmbVal < LA_MIN) {LA_MIN = Left_AmbVal ;} if (RightAmbVal > RA_MAX) {RA_MAX = RightAmbVal ;} if (Left_AmbVal > LA_MAX) {LA_MAX = Left_AmbVal ;} RE_Ref = RE_HighRef ; LE_Ref = LE_HighRef ; drive(left_rotate_slow) ; } // ----------------- Calibrate the peak values ----------------- else // calibrating peak values ... expecte the user to shine the LASER onto the floor // in front of the robot. { drive(park) ; if (RightEyePeak < RightEyeVal) { RightEyePeak = RightEyeVal ;} if (Left_EyePeak < Left_EyeVal) { Left_EyePeak = Left_EyeVal ;} } calibrated = calibrated - 20 ; } //--------------------------------------------------------------- // Done with calibration. Now stay in this loop. else { if (firsttimethrough == 1) { for (i=0 ; i < ArrayLength; i++) { RightEyeArray[i] = RE_Ref; Left_EyeArray[i] = LE_Ref; } // Ensure values are within operation design limits // Were one so inclined, simple LED indicators could show // if any of these 4 values had to be forced. if (LE_Ref < LE_RefDesignMin) LE_Ref = LE_RefDesignMin ; if (RE_Ref < RE_RefDesignMin) RE_Ref = RE_RefDesignMin ; if (LE_Ref > LE_RefDesignMax) LE_Ref = LE_RefDesignMax ; if (RE_Ref > RE_RefDesignMax) RE_Ref = RE_RefDesignMax ; if (Left_EyePeak < LE_PeakDesignMin) Left_EyePeak = LE_PeakDesignMin ; if (RightEyePeak < RE_PeakDesignMin) RightEyePeak = RE_PeakDesignMin ; if (Left_EyePeak > LE_PeakDesignMax) Left_EyePeak = LE_PeakDesignMax ; if (RightEyePeak > RE_PeakDesignMax) RightEyePeak = RE_PeakDesignMax ; mL = float(90/(float(Left_EyePeak) - float(LE_Ref))) ; mR = float(90/(float(RightEyePeak) - float(RE_Ref))) ; bL = mL*LE_Ref ; bR = mR*RE_Ref ; firsttimethrough = 0 ; } // Constrain the input values within design limits. if (Left_EyeVal < LE_RefDesignMin) Left_EyeVal = LE_RefDesignMin ; if (RightEyeVal < RE_RefDesignMin) RightEyeVal = RE_RefDesignMin ; if (Left_EyeVal > LE_PeakDesignMax) Left_EyeVal = LE_PeakDesignMax ; if (RightEyeVal > RE_PeakDesignMax) RightEyeVal = RE_PeakDesignMax ; #ifdef USE_DERIVATIVE reloadRight = (RightEyeVal > RightEyeComputedVal) ? 1 : 0 ; reload_Left = (Left_EyeVal > Left_EyeComputedVal) ? 1 : 0 ; #else reloadRight = 0 ; reload_Left = 0 ; #endif Left_EyeComputedVal = 0.0 ; RightEyeComputedVal = 0.0 ; // When using USE_DERIVATIVE, if the incoming light value is > the // current average, load the entire array with that new value. // That's the Poor Man's derivative, a way to respond immediately // to changes. Just toss out the average. // Otherwise push the new value onto the end, pop off the oldest // value, and take an average. #ifdef USE_ROLLINGAVERAGE for (i=0 ; i < (ArrayLength-1); i++) { RightEyeComputedVal = RightEyeComputedVal + RightEyeArray[i] ; Left_EyeComputedVal = Left_EyeComputedVal + Left_EyeArray[i] ; RightEyeArray[i] = (reloadRight) ? RightEyeVal : RightEyeArray[i+1] ; Left_EyeArray[i] = (reload_Left) ? Left_EyeVal : Left_EyeArray[i+1] ; } RightEyeArray[ArrayLength-1] = RightEyeVal ; Left_EyeArray[ArrayLength-1] = Left_EyeVal ; RightEyeComputedVal = float ((RightEyeComputedVal + RightEyeVal)/ArrayLength) ; Left_EyeComputedVal = float ((Left_EyeComputedVal + Left_EyeVal)/ArrayLength) ; #else RightEyeComputedVal = RightEyeVal; Left_EyeComputedVal = Left_EyeVal; #endif // This stops it going into reverse if it gets dark. There are no bumpers/switches on the back. if (RightEyeComputedVal < RE_Ref) { RightEyeComputedVal = RE_Ref ; } if (Left_EyeComputedVal < LE_Ref) { Left_EyeComputedVal = LE_Ref ; } // Check the bumpers (switches) first. // What should happen is that both wheels reverse on any bumper, // but the side which bumped should back off more. if ( (RightBumperState == LOW) && (Left_BumperState == LOW)) { drive(back_full) ; } else if (RightBumperState == LOW) { drive(backup_to_the_left_full) ; } else if (Left_BumperState == LOW) { drive(backup_to_the_right_full) ; } // That's the bumpers. Now check the ambient light. // If it's ambiently bright on the on left AND right ... else if ((Left_AmbVal >= (LA_MIN+50)) && (RightAmbVal >= (RA_MIN+50))) { drive(backup_to_the_left_full) ; } //------------------------------------------------------------------ // In this section the motors are driven by a value calculated, and // not a straight threshold as are the canned behaviors. // The canned behaviors never led to smooth operation so I added this // variable drive bit. else // calculate motor values { // SENSOR DATA on 2015.08.17 // | left right // ---------------------------------+------------------------ // Kinda dark | 3 and 7 // LASER on the left side | 286 and 42 // LASER on the right side | 29 and 270 // LASER in the middle | 114-120 and 140-171 // Room lights on | 176-183 and 248-254 // Room lights plus LASER in middle | 262-273 and 203-380 // Now compute the speed values proportionally w.r.t the reference values such that // when xxxxxEyeComputedVal is close to XE_ref, the output is close to 0, and // as xxxxxEyeComputedVal gets closer to xxxxxEyePeak, the output is close to 90. // Then // Right Eye data drives the left motor. // Left Eye data drives the right motor. Left_Speed = int ((RightEyeComputedVal*mR) - bR) ; RightSpeed = int ((Left_EyeComputedVal*mL) - bL) ; // Now a bit of finessing. First, don't actually let the Speed exceed 90: if (RightSpeed > 90) { RightSpeed = 90; } if (Left_Speed > 90) { Left_Speed = 90; } // If the left is a small value and the right has some LASER thing going, // then reverse the left: if ((Left_Speed < 10) && (RightSpeed > 10)) { Left_Speed = -10; } // If the right is a small value and the left has some LASER thing going, // then reverese the right: else if ((RightSpeed < 10) && (Left_Speed > 10)) { RightSpeed = -10; } else // they are both small values, so make them zero, same as "park": { if (Left_Speed < 10) { Left_Speed = 0; } if (RightSpeed < 10) { RightSpeed = 0; } } #ifdef USE_BANG_BANG Rthresh = RE_Ref + 10 ; Lthresh = LE_Ref + 10 ; // Bang-bang control. Just go when the light is a wee bit above threshold. if ((RightEyeComputedVal > Rthresh) && (Left_EyeComputedVal > Lthresh)) { RightSpeed = 90 ; Left_Speed = 90 ; } else if (RightEyeComputedVal > Rthresh) { RightSpeed = -90 ; // 90 - -90 = 180, which is full speed reverse on the right Left_Speed = 90 ; // 90 + 90 = 180, which is full speed ahead on the left } else if (Left_EyeComputedVal > Lthresh) { RightSpeed = 90 ; // 90 - +90 = 0, which is full speed ahead on the right Left_Speed = -90 ; // 90 + -90 = 0, which is full speed reverse on the left } else { RightSpeed = 0 ; Left_Speed = 0 ; } #endif // Finally convert the values to L and R servo sense. // Lspeed above 90 is forward motion. // Rspeed below 90 is forward motion. Left_Speed = 90 + Left_Speed ; RightSpeed = 90 - RightSpeed ; RightServo.write(RightSpeed) ; Left_Servo.write(Left_Speed) ; delay (20) ; // Calls to 'drive' with the enumerated values consume time, // but direct servo writes do not. So do it here. } // end of else calculate motor values } } //--------------------------------------------------------------- void drive (drive_behavior behavior) { if (behavior == ahead_quarter) { RightServo.write(RaheadQuarter) ; Left_Servo.write(LaheadQuarter) ; } else if (behavior == ahead_half) { RightServo.write(RaheadHalf) ; Left_Servo.write(LaheadHalf) ; } else if (behavior == ahead_full) { RightServo.write(RaheadFull) ; Left_Servo.write(LaheadFull) ; delay (80); } else if (behavior == back_quarter) { RightServo.write(RreverseQuarter) ; Left_Servo.write(LreverseQuarter) ; } else if (behavior == back_half) { RightServo.write(LreverseHalf) ; Left_Servo.write(LreverseHalf) ; } else if (behavior == back_full) { RightServo.write(RreverseFull); Left_Servo.write(LreverseFull); delay (980); } else if (behavior == right_turn_forward_full) { RightServo.write(halt) ; Left_Servo.write(LaheadFull) ; } else if (behavior == right_turn_forward_half) { RightServo.write(halt) ; Left_Servo.write(LaheadFull) ; } else if (behavior == right_turn_forward_quarter) { RightServo.write(halt) ; Left_Servo.write(LaheadQuarter) ; } else if (behavior == backup_to_the_right_full) { RightServo.write(RreverseQuarter); Left_Servo.write(LreverseFull); delay (980); } else if (behavior == left_turn_forward_full) { RightServo.write(RaheadFull) ; Left_Servo.write(halt) ; } else if (behavior == left_turn_forward_half) { RightServo.write(RaheadFull) ; Left_Servo.write(halt) ; } else if (behavior == left_turn_forward_quarter) { RightServo.write(RaheadQuarter) ; Left_Servo.write(halt) ; } else if (behavior == backup_to_the_left_full) { RightServo.write(RreverseFull); Left_Servo.write(LreverseQuarter); delay (980); } else if (behavior == right_rotate_fast) { RightServo.write(RreverseFull) ; Left_Servo.write(LaheadFull) ; } else if (behavior == right_rotate_slow) { RightServo.write(RreverseQuarter) ; Left_Servo.write(LaheadQuarter) ; } else if (behavior == left_rotate_fast) { RightServo.write(RaheadFull) ; Left_Servo.write(LreverseFull) ; } else if (behavior == left_rotate_slow) { RightServo.write(RaheadQuarter) ; Left_Servo.write(LreverseQuarter) ; } // park is the default behavior else { RightServo.write(halt); Left_Servo.write(halt); } delay(20) ; }