From cefb51ff2a7309b2f003eadc781b8ac6527652ae Mon Sep 17 00:00:00 2001 From: MadTooler Date: Fri, 26 Oct 2018 00:30:21 -0500 Subject: [PATCH] Example updates for release 0.0.95 --- examples/RaceTrack/RaceTrackRobotsV2/Config.h | 47 ++ .../RaceTrack/RaceTrackRobotsV2/IRcodes.h | 20 + .../RaceTrackRobotsV2/RaceTrackRobotsV2.ino | 161 ++++ .../RaceTrackScoreBoard.ino | 702 ++++++++++++++++++ .../Level_1/BSA_Course/BSA_Course.ino | 72 ++ .../Level_2/GLC_GetIRcodes/GLC_GetIRcodes.ino | 57 ++ .../Level_2/GLC_IRcontrol/Config.h | 53 ++ .../Level_2/GLC_IRcontrol/GLC_IRcontrol.ino | 186 +++++ .../Level_2/GLC_IRcontrol/IRcodes.h | 76 ++ .../Level_2/GLC_IRcontrol/Tiles.ino | 48 ++ .../Level_2/GLC_IRcontrol/bDecode.ino | 323 ++++++++ .../Level_2/GLC_IRcontrol/beeps.ino | 91 +++ .../ScoutBotics/Level_2/GLC_IRcontrol/m1.ino | 170 +++++ .../ScoutBotics/Level_2/GLC_IRcontrol/m2.ino | 449 +++++++++++ .../ScoutBotics/Level_2/GLC_IRcontrol/m3.ino | 357 +++++++++ .../ScoutBotics/Level_2/GLC_IRcontrol/m4.ino | 202 +++++ .../ScoutBotics/Level_2/GLC_IRcontrol/m5.ino | 222 ++++++ .../Level_2/GLC_IRcontrol/utilities.ino | 188 +++++ .../Level_2/GLC_SampleIRcodes/DirectTV_RC23.h | 78 ++ .../Level_2/GLC_SampleIRcodes/DirectTV_RC73.h | 78 ++ .../GLC_SampleIRcodes/GLC_SampleIRcodes.ino | 80 ++ .../Level_2/GLC_SampleIRcodes/MTA.h | 79 ++ .../Level_2/GLC_SampleIRcodes/Sanyo_FXML.h | 79 ++ examples/Xtra/RepeatAfterMe/RepeatAfterMe.ino | 118 +++ extras/Change.log | 20 +- extras/responsa/SBL2_2018.txt | 156 ++++ library.properties | 4 +- src/GobbitLineCommand.cpp | 30 +- src/GobbitLineCommand.h | 10 +- src/config.h | 8 +- 30 files changed, 4141 insertions(+), 23 deletions(-) create mode 100644 examples/RaceTrack/RaceTrackRobotsV2/Config.h create mode 100644 examples/RaceTrack/RaceTrackRobotsV2/IRcodes.h create mode 100644 examples/RaceTrack/RaceTrackRobotsV2/RaceTrackRobotsV2.ino create mode 100644 examples/RaceTrack/RaceTrackScoreBoard/RaceTrackScoreBoard.ino create mode 100644 examples/ScoutBotics/Level_1/BSA_Course/BSA_Course.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_GetIRcodes/GLC_GetIRcodes.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/Config.h create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/GLC_IRcontrol.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/IRcodes.h create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/Tiles.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/bDecode.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/beeps.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/m1.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/m2.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/m3.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/m4.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/m5.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_IRcontrol/utilities.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_SampleIRcodes/DirectTV_RC23.h create mode 100644 examples/ScoutBotics/Level_2/GLC_SampleIRcodes/DirectTV_RC73.h create mode 100644 examples/ScoutBotics/Level_2/GLC_SampleIRcodes/GLC_SampleIRcodes.ino create mode 100644 examples/ScoutBotics/Level_2/GLC_SampleIRcodes/MTA.h create mode 100644 examples/ScoutBotics/Level_2/GLC_SampleIRcodes/Sanyo_FXML.h create mode 100644 examples/Xtra/RepeatAfterMe/RepeatAfterMe.ino create mode 100644 extras/responsa/SBL2_2018.txt diff --git a/examples/RaceTrack/RaceTrackRobotsV2/Config.h b/examples/RaceTrack/RaceTrackRobotsV2/Config.h new file mode 100644 index 0000000..f50fe9d --- /dev/null +++ b/examples/RaceTrack/RaceTrackRobotsV2/Config.h @@ -0,0 +1,47 @@ +// Choose your Motor Driver... +// To load default settings for either an Ardumoto version 14 or 20, or an Adafruit v2.3, +// uncomment only the following motor driver define that matches. +// If none are uncommented, Ardumoto v14 values will be used. +// +// DO NOT UNCOMMENT MORE THAN ONE +//#define ARDUMOTO_14 +//#define ARDUMOTO_20 +//#define ADAFRUIT_MS + +// choose your lane with either 'R' for right or 'L' for left +#define LANE_CHOICE 'R' + +// What voltage is your battery (whole numbers only) +#define BATTERY_VOLTS 9 + +// **** comment out define LEFT_LANE if using right lane +//#define LEFT_LANE + +// If you will use an IO pin to power the IR receiver +5v +// then declare the define to the pin number. +// Otherwise, comment it at out. +// This is a shortcut for low mAmp power where you may have available IO pins and +// where the +5v pin is already used and you do not have a breadboard or splitter on hand. +#define IR_POWER_PIN A0 // comment out to disable + +#define RECV_PIN A1 // pin your IR receiver is using + +#define PIEZO_PIN A2 // piezo beeper/sensor pin + +#define CALIBRATE_SPEED 0 // speed 0-100 for line follow calibration mode. 0 uses default values. + +const int min_speed = -35; // speed -100 to 100 for line follow motor speed PID. -100 is the default, but + // a little higher might work better here + +const int max_speed = 100; // speed -100 to 100 for line follow motor speed PID. 100 is the default. + +#define FOLLOWLINE_MODE 1 // set followLine mode 0,1,2,3. 1 or 2 are probably best use here. + +// set these values ONLY if using battery monitoring +// to disable, comment out the define ANALOG_DIVIDER_PIN +//#define ANALOG_DIVIDER_PIN A5 // analog pin your voltage divider is connected +#define CUTOFF 9 // cut off voltage... for example, for a 3 cell lithium, set at 3.0v x 3 cells = 9 +#define SMALL_RES_KOHMS 33 // K ohm value of your small resistor in the divider +#define LARGE_RES_KOHMS 100 // K ohm value of your small resistor in the divider + + diff --git a/examples/RaceTrack/RaceTrackRobotsV2/IRcodes.h b/examples/RaceTrack/RaceTrackRobotsV2/IRcodes.h new file mode 100644 index 0000000..d4741c1 --- /dev/null +++ b/examples/RaceTrack/RaceTrackRobotsV2/IRcodes.h @@ -0,0 +1,20 @@ +// IR command values are unique to this setup. +// While these are currently sent and received in the NEC format of IR is used for now, +// it may be best to switch to a shorter/smaller bit value code for faster communication. + +//#ifdef LEFT_LANE +#if (LANE_CHOICE == 'L') + // Left lane IR commands + #define STOP 0x2BB27000 + #define BACKUP 0x2BB27002 + #define BUMP_IN 0x2BB27003 +#else + // Right lane IR commands + #define STOP 0x2BB28000 + #define BACKUP 0x2BB28002 + #define BUMP_IN 0x2BB28003 +#endif + +// Common IR commands +#define GO_RACE 0x2BB26001 +#define ALL_STOP 0x2BB29999 diff --git a/examples/RaceTrack/RaceTrackRobotsV2/RaceTrackRobotsV2.ino b/examples/RaceTrack/RaceTrackRobotsV2/RaceTrackRobotsV2.ino new file mode 100644 index 0000000..058fbb5 --- /dev/null +++ b/examples/RaceTrack/RaceTrackRobotsV2/RaceTrackRobotsV2.ino @@ -0,0 +1,161 @@ +/* 10/25/2018 + + This program is intended for the Gobbit robot with a line sensor and infrared receiver to be used + on a lined course with either a scoreboard start/finish line or simple IR remote control for both + staging robots at the start line, telling them when to start, and when to stop. + + NOTE: The wiring of the Gobbit here matches the wiring used in the ScoutBotics Level 2 IRcontrol example. + + SEE Config.h TAB TO CONFIRM SETTINGS AND LANE BEFORE UPLOADING + + SEE IRcodes.h if you need to change the working IR command codes + + If a scoreboard is not used with the RaceTrackScoreBoard program, then change the IR codes to match + those on an IR remote, such as a tv or cable remote. + + "IRremote" library is also needed. + + For GobbitLineCommand function definitions, see the GobbitLineCommand "GLC_Functions" example sketch. + + ** Further documentation is not currently available. ** + + It may be better to revise and use a shorter IR code format for improved speed and timing. + +*/ + +#include "Config.h" +#include "IRcodes.h" + +#include +IRrecv irrecv(RECV_PIN); +decode_results results; + +#include +GobbitLineCommand MyBot; + +int go = -1; +byte moving = 0; +void setup() { + + // most of the setup notes and calls are specifically for the Gobbit robot with other possible configurations in mind, and the GobbitLineCommand library usage + + MyBot.setMinMaxSpeeds(min_speed, max_speed); + + // If IR_POWER_PIN was defined, this will set the pin to power the IR receiver, +#ifdef IR_POWER_PIN + pinMode(IR_POWER_PIN, OUTPUT); + digitalWrite(IR_POWER_PIN, HIGH); +#endif + + // ArduMoto v14 +#ifdef ARDUMOTO_14 + MyBot.setQTRpins(2, 4, 5, 6, 7, 8, A3, A4); // ** + MyBot.setLeftMotorPinsDirPWM(12, 9); // on ardumoto v14, these were 12,3; use a jumper from 3 to 9 + MyBot.setRightMotorPinsDirPWM(13, 10); // on ardumoto v14, these were 13,11; use a jumper from 11 to 10 +#endif + + // ArduMoto v20 +#ifdef ARDUMOTO_20 + MyBot.setQTRpins(5, 6, 7, 8, 12, 13, A3, A4); // ** + MyBot.setLeftMotorPinsDirPWM(4, 10); // on ardumoto v20, these were 4,11; use a jumper from 11 to 10 + MyBot.setRightMotorPinsDirPWM(2, 9); // on ardumoto v20, these were 2,3; use a jumper from 3 to 9 +#endif + +#if (BATTERY_VOLTS < 10) + // use these for 9v, not above + // PID tunes here vary from the default due to pin changes and larger overhead timing of processor cycles. + // See the "ArdumotoDefaults.h" or the "AdafruitMSDefaults.h" files in the library "src" folder to view or change the default values. + MyBot.setPID(0.1, 0.00, 2); // default at 9v is 0.15, .002, 411.1v is 0.15, 0.002, 4 + MyBot.setPIDcoarse(0.3, 0.001, 4.4); // default at 9v is 0.2, 0.001, 3.4 + MyBot.setPIDfineRange(0.6); // default at 9v is 0.04 +#endif + +#if (BATTERY_VOLTS > 9) + // PID tunes here should be uncommented if the default values are to be improved for course specific. + // See the "ArdumotoDefaults.h" or the "AdafruitMSDefaults.h" files in the library "src" folder to view or change the default values. + MyBot.setPID(0.1, 0.00, 2); // default at 11.1v is 0.15, 0.002, 4 + MyBot.setPIDcoarse(0.2, 0.001, 4.4); // default at 11.1v is 0.2, 0.001, 4.4 + MyBot.setPIDfineRange(0.6); // default at 11.1v is 0.05 +#endif + + MyBot.setBatteryVolts(BATTERY_VOLTS); + + MyBot.beginGobbit(); // initializes robot with settings + + MyBot.calibrateLineSensor(CALIBRATE_SPEED); // calibrate the sensor + + irrecv.enableIRIn(); // Start the IR receiver + +#ifdef ANALOG_DIVIDER_PIN + MyBot.checkBattery(ANALOG_DIVIDER_PIN, CUTOFF, SMALL_RES_KOHMS, LARGE_RES_KOHMS); +#endif + +} + +void loop() { + +#ifdef ANALOG_DIVIDER_PIN + // check battery status, if enabled, and halt the program if the voltage is too low + MyBot.checkBattery(ANALOG_DIVIDER_PIN, CUTOFF, SMALL_RES_KOHMS, LARGE_RES_KOHMS); +#endif + + if (irrecv.decode(&results)) { // this line triggers the beginning of the fun. if no signal is received, nothing will start. + + switch (results.value){ + + case STOP: // stop ir command + //if (go == -1) MyBot.setMinMaxSpeeds(-100, 100); // reset if initial approach was set slower + go = 0; + break; + + case ALL_STOP: // All stop ir command. Different from stop because all lanes should stop. + go = 0; + break; + + case GO_RACE: // go race ir command + go = 1; + break; + + case BACKUP: // stop ir command + go = 2; + break; + + case BUMP_IN: // stop ir command + go = 3; + break; + } + + irrecv.resume(); // Receive the next value + } + + switch (go) { + + case -1: // initial approach to start line after calibration. Change this area if manual start will be used. + // MyBot.setMinMaxSpeeds(-70, 70); + MyBot.followLine(FOLLOWLINE_MODE); + moving=1; + break; + + case 0: // stop with quick motor reversal for reduced overrun + if(moving){ + MyBot.brakeMotors(200,'B'); + moving=0; + } + break; + + case 1: // race begins + MyBot.followLine(FOLLOWLINE_MODE); + moving=1; + break; + + case 2: // backup until told to stop + MyBot.move(-42,0); + break; + + case 3: // bump in with delay until told to stop + MyBot.brakeMotors(35,'F'); // much less than 100% of brake time needed. 35% seems good for higher voltage setups. + delay(350); // give the scoreboard time to react and send a stop message + break; + } + +} diff --git a/examples/RaceTrack/RaceTrackScoreBoard/RaceTrackScoreBoard.ino b/examples/RaceTrack/RaceTrackScoreBoard/RaceTrackScoreBoard.ino new file mode 100644 index 0000000..a70a080 --- /dev/null +++ b/examples/RaceTrack/RaceTrackScoreBoard/RaceTrackScoreBoard.ino @@ -0,0 +1,702 @@ +/* 10/25/2018 + + This program is for a laser line start/finish line for a Gobbit robot race course with serial monitoring. + + "IRremote" library is also needed. + + ** Further documentation is not currently available. ** + + NOTE: There is an occasional 0.2 second delay for the loser of a near tie race. + Better timing, shorter IR codes/format, and delay may improve issue. + + IR codes are 32 bit values, noted in HEX format as 0x00000000 through 0xFFFFFFFF (decimal value 0 to 4294967294) + We have made up codes here to broadcast and receive in NEC format. They are not necessarily in compliance with + the NEC code standard, but they should work just fine. + + It may be better to revise and use a shorter IR code format for improved speed and timing. + +*/ + +// Left lane IR commands +#define LEFT_STOP 0x2BB27000 +#define LEFT_BACKUP 0x2BB27002 +#define LEFT_BUMP_IN 0x2BB27003 + +// Right lane IR commands +#define RIGHT_STOP 0x2BB28000 +#define RIGHT_BACKUP 0x2BB28002 +#define RIGHT_BUMP_IN 0x2BB28003 + +// Common IR commands +#define GO_RACE 0x2BB26001 +#define ALL_STOP 0x2BB29999 + +#define MAX_LAPS_IN_RACE 9 // this should not be changed since more cannot be selected with current serial interface. +#define START_WAIT 3000 // time limit for a second lane to cross the line, otherwise will run as single lane race +#define LAP_COUNT_TIME_THRESHOLD 2000 // time to make a full lap should take longer than this +#define END_OF_RACE_DELAY 10000 // delay after the race has finished before it auto restarts + +#define LDR_L A0 +#define LDR_R A1 +#define BEEP_PIN 2 +#define LEFT_SERVO_PIN 9 +#define GREEN_SERVO_PIN 10 +#define RIGHT_SERVO_PIN 11 + +#define FLAG_UP_ANGLE 8 +#define FLAG_SWING_ANGLE 40 +#define FLAG_DOWN_ANGLE 120 + +#define LDR_TRIGGER_LEVEL 500 // 0-1023, probably near 400, when it goes below this level it will trigger + +#include + +Servo leftCheckFlagServo; +Servo greenFlagServo; +Servo rightCheckFlagServo; + +byte start =0; // flag for start/stop mode +byte winner = 0; // flag that race was won by lane (1)left or (2)right +int lapCountL=-1; // current count of completed laps left lane +int lapCountR=-1; // current count of completed laps right lane +int totalLaps = MAX_LAPS_IN_RACE; +byte leftStillCrossingLine=0; // flag to test if robot is still breaking the laser since the last read and has not yet moved past. +byte rightStillCrossingLine=0; // flag to test if robot is still breaking the laser since the last read and has not yet moved past. + +byte resetOnLine=1; // flag to direct the reset on the starting line sequence + +int raceMode=1; // initial race mode as default + +long mlastTime = 0; // ref last time used for timing cycles +long mLastLeftTrigger = 0; // ref last time used for last laser beam break on left +long mLastRightTrigger = 0; // ref last time used for last laser beam break on right +long mLeftRaceTime = 0; // total time in race for left lane +long mRightRaceTime = 0; // total time in race for right lane +long mLeftLapTime[MAX_LAPS_IN_RACE + 1]; // time for last lap in left lane +long mRightLapTime[MAX_LAPS_IN_RACE + 1]; // time for last lap in right lane +long mStartTime = 0; + +// incoming Serial data vars +int incomingByte = 0; // for incoming serial data input + +#include + +IRsend irsend; + + +void setup() +{ + + leftCheckFlagServo.attach(LEFT_SERVO_PIN); + greenFlagServo.attach(GREEN_SERVO_PIN); + rightCheckFlagServo.attach(RIGHT_SERVO_PIN); + + // test up position of flags + leftCheckFlagServo.write(FLAG_UP_ANGLE); + greenFlagServo.write(FLAG_UP_ANGLE); + rightCheckFlagServo.write(FLAG_UP_ANGLE); + + delay(2000); + + // put the flags away + leftCheckFlagServo.write(FLAG_DOWN_ANGLE); + greenFlagServo.write(FLAG_DOWN_ANGLE); + rightCheckFlagServo.write(FLAG_DOWN_ANGLE); + + pinMode(BEEP_PIN, OUTPUT); + + Serial.begin(115200); //start the serial monitor + + delay(1000); // time for laser light level to stabilize after servos draw power so a false trigger is not made + + Serial.println(); + Serial.println(F("Typing c and hitting SEND or enter from the terminal prompt at any time will CANCEL the current race")); + Serial.println(); + Serial.println(); + delay(1500); + selectRaceMode(1); +} + +void loop() +{ + while(raceMode){ + if(checkIfCancel()) return; + + // ---- initial approach and reset of robots to start line + if(resetOnLine && raceMode){ + + mlastTime = 0; // clear timer reference + + Serial.println(); + Serial.println(F("**** **** **** **** **** ****")); + Serial.println(F(" NEW RACE NOW STAGING AT THE START LINE")); + Serial.println(F("**** **** **** **** **** ****")); + Serial.println(); + + while((lapCountR + lapCountL < 0) && (lapCountR + lapCountL > -3)){ + + if(checkIfCancel()) return; + + // check if left lane has crossed line and stop it + if(analogRead(LDR_L) > LDR_TRIGGER_LEVEL && lapCountL < 0){ + // beep buzzer + digitalWrite(BEEP_PIN, HIGH); + + // send stop signal to left lane + irsend.sendNEC(LEFT_STOP, 32); + delay(20); + irsend.sendNEC(LEFT_STOP, 32); + delay(20); + + digitalWrite(BEEP_PIN, LOW); + + lapCountL=0; + + // start timer if not already going + if(mlastTime == 0) + mlastTime = millis(); + + // Delay here is to keep a short off for beeper if both lanes are sitting at the line already. + // Otherwise, only a long beep would be heard. + delay(20); + } + + // check if right lane has crossed line and stop it + if(analogRead(LDR_R) > LDR_TRIGGER_LEVEL && lapCountR < 0){ + // beep buzzer + digitalWrite(BEEP_PIN, HIGH); + + // send stop signal to right lane + irsend.sendNEC(RIGHT_STOP, 32); + delay(20); + irsend.sendNEC(RIGHT_STOP, 32); + delay(20); + + digitalWrite(BEEP_PIN, LOW); + + lapCountR=0; + + // start timer if not already going + if(mlastTime == 0) + mlastTime = millis(); + + // Delay here is to keep a short off for beeper if both lanes are sitting at the line already. + // Otherwise, only a long beep would be heard. + delay(20); + } + + // check time limit for other lane to arrive before disqualifying or making it a single lane race + if(mlastTime){ + if(millis()-mlastTime > START_WAIT){ + if(lapCountL <0) + // -3 is indicator that this lane will not be used in race + lapCountL=-3; + else + lapCountR=-3; + } + } + + } + + if(lapCountL != -3) + Serial.println(F(" LEFT lane... will RACE!")); + else Serial.println(F(" LEFT lane... is a NO SHOW!")); + Serial.println(); + if(lapCountR != -3) + Serial.println(F(" RIGHT lane... will RACE!")); + else Serial.println(F(" RIGHT lane... is a NO SHOW!")); + Serial.println(); + // move the robots back and have them stop close to the line + if(lapCountL != -3){ + leftResetAtLine(); + if(!raceMode) return; + Serial.println(F(" LEFT lane... is READY!")); + Serial.println(); + } + if(lapCountR != -3){ + rightResetAtLine(); + if(!raceMode) return; + Serial.println(F(" RIGHT lane... is READY!")); + Serial.println(); + } + resetOnLine=0; + + start=1; + // wait a little while robot sits on start line before race starts + delay(2000); + } // end of reset + + + // ---- start race + if(start){ + // get the flag moving first + greenFlagServo.write(FLAG_UP_ANGLE); + delay(200); + + Serial.println(F("GO, GO, GO!!!!!!")); + Serial.println(); + + mStartTime = millis(); + + // send start signal to robot + irsend.sendNEC(GO_RACE, 32); + delay(20); + irsend.sendNEC(GO_RACE, 32); + + // wave the start flag + for(int i=0;i<8;i++){ + greenFlagServo.write(FLAG_SWING_ANGLE); + delay(200); + greenFlagServo.write(FLAG_UP_ANGLE); + delay(200); + } + greenFlagServo.write(FLAG_DOWN_ANGLE); + start=0; + + delay(3000); // delay to allow power fluctuations from servos to stabilize and not cause false trigger + } // end of start + + + + + // ----- detect laps here + // Left lane lap count + if(raceMode){ + if(analogRead(LDR_L) > LDR_TRIGGER_LEVEL){ + if(lapCountL < totalLaps && lapCountL != -3){ + if((millis()-mLastLeftTrigger > LAP_COUNT_TIME_THRESHOLD) && !leftStillCrossingLine){ + lapCountL++; + leftStillCrossingLine=1; + + // beep buzzer + digitalWrite(BEEP_PIN, HIGH); + delay(40); + digitalWrite(BEEP_PIN, LOW); + + if(raceMode == 3){ + mLeftLapTime[1] = (millis()- mStartTime); + } + else{ + mLeftLapTime[lapCountL] = (millis()- mStartTime); + } + + Serial.print(F(" LEFT lane... LAP ")); + Serial.print(lapCountL); + Serial.print(F(" TIME: ")); + if(raceMode == 3){ + Serial.print(float((mLeftLapTime[1] - mLeftLapTime[0])/1000.0)); + Serial.print(F(" TOTAL RUN TIME: ")); + Serial.println(float((millis()- mStartTime)/1000.0)); + mLeftLapTime[0] = mLeftLapTime[1]; + } + else Serial.println(float((mLeftLapTime[lapCountL] - mLeftLapTime[lapCountL-1])/1000.0)); + Serial.println(); + } + mLastLeftTrigger=millis(); + } + } + // else it is no longer breaking the laser beam + else leftStillCrossingLine=0; + + // Right lane lap count + if(analogRead(LDR_R) > LDR_TRIGGER_LEVEL){ + if(lapCountR < totalLaps && lapCountR != -3){ + if((millis()-mLastRightTrigger > LAP_COUNT_TIME_THRESHOLD) && !rightStillCrossingLine){ + lapCountR++; + rightStillCrossingLine=1; + + // beep buzzer + digitalWrite(BEEP_PIN, HIGH); + delay(40); + digitalWrite(BEEP_PIN, LOW); + + if(raceMode == 3){ + mRightLapTime[1] = (millis()- mStartTime); + } + else{ + mRightLapTime[lapCountR] = (millis()- mStartTime); + } + + Serial.print(F(" RIGHT lane... LAP ")); + Serial.print(lapCountR); + Serial.print(F(" TIME: ")); + if(raceMode == 3){ + Serial.print(float((mRightLapTime[1] - mRightLapTime[0])/1000.0)); + Serial.print(F(" TOTAL RUN TIME: ")); + Serial.println(float((millis()- mStartTime)/1000.0)); + mRightLapTime[0] = mRightLapTime[1]; + } + else Serial.println(float((mRightLapTime[lapCountR] - mRightLapTime[lapCountR-1])/1000.0)); + Serial.println(); + } + mLastRightTrigger=millis(); + } + } + // else it is no longer breaking the laser beam + else rightStillCrossingLine=0; + + }// end of lap detection/counting + + + // -------- + // detect win and stop both robots when they have completed the laps + if(raceMode != 3){ + + if(lapCountL == totalLaps){ + if(!winner){ + // Left lane won !! + mLeftRaceTime = millis()- mStartTime; + leftCheckFlagServo.write(FLAG_UP_ANGLE); + irsend.sendNEC(LEFT_STOP, 32); + delay(20); + irsend.sendNEC(LEFT_STOP, 32); + delay(20); + winner = 1; + Serial.println(F(" LEFT lane... WINNER!!!")); + Serial.println(); + } + else if(winner == 2){ // this lane lost but still will need to stop + mLeftRaceTime = millis()- mStartTime; + irsend.sendNEC(LEFT_STOP, 32); + delay(20); + irsend.sendNEC(LEFT_STOP, 32); + delay(20); + Serial.println(F(" LEFT lane... finished FASHIONABLY LATE")); + Serial.println(); + } + } + if(lapCountR == totalLaps){ + if(!winner){ + // Right lane won !! + mRightRaceTime = millis()- mStartTime; + rightCheckFlagServo.write(FLAG_UP_ANGLE); + irsend.sendNEC(RIGHT_STOP, 32); + delay(20); + irsend.sendNEC(RIGHT_STOP, 32); + delay(20); + winner = 2; + Serial.println(F(" RIGHT lane... WINNER!!!")); + Serial.println(); + } + else if(winner == 1){ // this lane lost but still will need to stop + mRightRaceTime = millis()- mStartTime; + irsend.sendNEC(RIGHT_STOP, 32); + delay(20); + irsend.sendNEC(RIGHT_STOP, 32); + delay(20); + Serial.println(F(" RIGHT lane... finished FASHIONABLY LATE")); + Serial.println(); + } + } + } // end of win and stop + + // ----- check if the race is completed and initialize restart if it is + if (winner && (lapCountL == -3 || lapCountL == totalLaps) && (lapCountR == -3 || lapCountR == totalLaps)){ + + Serial.println(F("RACE RESULTS")); + Serial.println(F("____________________________")); + Serial.println(); + + if(lapCountL != -3){ + Serial.print(F(" LEFT lane")); + if(winner == 1) Serial.println(F(" WINNER")); + else Serial.println(); + + for(int lap=1; lap <= totalLaps; lap++){ + Serial.print(F(" LAP ")); + Serial.print(lap); + Serial.print(F("________")); + Serial.print(float((mLeftLapTime[lap] - mLeftLapTime[lap-1])/1000.0)); + Serial.println(F("s")); + } + + Serial.print(F(" Total Time___")); + Serial.print(float(mLeftRaceTime/1000.0)); + Serial.println(F("s")); + Serial.println(); + } + + + if(lapCountR != -3){ + Serial.print(F(" RIGHT lane")); + if(winner == 2) Serial.println(F(" WINNER")); + else Serial.println(); + + for(int lap=1; lap <= totalLaps; lap++){ + Serial.print(F(" LAP ")); + Serial.print(lap); + Serial.print(F("________")); + Serial.print(float((mRightLapTime[lap] - mRightLapTime[lap-1])/1000.0)); + Serial.println(F("s")); + } + + Serial.print(F(" Total Time___")); + Serial.print(float(mRightRaceTime/1000.0)); + Serial.println(F("s")); + Serial.println(); + } + Serial.println(); + + resetRace(); + if(raceMode == 1) + selectRaceMode(0); + } + } + selectRaceMode(0); +} + + +void leftResetAtLine(void){ + + // signal the robot to backup + irsend.sendNEC(LEFT_BACKUP, 32); + delay(20); + irsend.sendNEC(LEFT_BACKUP, 32); + + // keep going until the laser is clear + while(analogRead(LDR_L) > LDR_TRIGGER_LEVEL){ + if(checkIfCancel()) return; + } + + // signal the robot to bump forward + irsend.sendNEC(LEFT_BUMP_IN, 32); + delay(20); + irsend.sendNEC(LEFT_BUMP_IN, 32); + + // the robot should continue to bump until the laser is again tripped + while(analogRead(LDR_L) < LDR_TRIGGER_LEVEL){ + if(checkIfCancel()) return; + } + + // signal the robot to stop + irsend.sendNEC(LEFT_STOP, 32); + delay(20); + irsend.sendNEC(LEFT_STOP, 32); + delay(20); +} + + +void rightResetAtLine(void){ + + // signal the robot to backup + irsend.sendNEC(RIGHT_BACKUP, 32); + delay(20); + irsend.sendNEC(RIGHT_BACKUP, 32); + + // keep going until the laser is clear + while(analogRead(LDR_R) > LDR_TRIGGER_LEVEL){ + if(checkIfCancel()) return; + } + + // signal the robot to bump forward + irsend.sendNEC(RIGHT_BUMP_IN, 32); + delay(20); + irsend.sendNEC(RIGHT_BUMP_IN, 32); + + // the robot should continue to bump until the laser is again tripped + while(analogRead(LDR_R) < LDR_TRIGGER_LEVEL){ + if(checkIfCancel()) return; + } + + // signal the robot to stop + irsend.sendNEC(RIGHT_STOP, 32); + delay(20); + irsend.sendNEC(RIGHT_STOP, 32); + delay(20); +} + + +void cancelRace(void){ + Serial.println(); + Serial.println(); + Serial.println(F(" ...canceling active races............")); + Serial.println(); + Serial.println(); + Serial.println(F("////////////-------------------\\\\\\\\\\\\")); + Serial.println(F("REMOVE ALL ROBOTS FROM THE START LINE")); + Serial.println(F("\\\\\\\\\\\\-------------------////////////")); + Serial.println(); + raceMode=0; + resetRace(); +} + + +void selectRaceMode(byte timeout){ + Serial.println(F("Please select and enter your race mode:")); + Serial.println(F(" (1) Single race and stop")); + Serial.println(F(" (2) Repeat until canceled")); + Serial.println(F(" (3) Lap times and counting only, will not stop until canceled.")); + Serial.println(); + incomingByte = '*'; + + long mToAutoStart = millis(); + + int notDone = 1; + + while(notDone){ + while (Serial.available() == 0){ // wait for serial input or timeout and start if set to timeout mode + if((millis() - mToAutoStart > 10000) && timeout){ + raceMode = 2; + totalLaps = 2; + return; + } + } + + // read the incoming serial byte: + incomingByte = Serial.read(); + + switch (incomingByte) + { + case '1': case '2': + notDone = 1; + + if(incomingByte == '1') raceMode=1; + else raceMode=2; + + while(notDone){ + Serial.println(F("How many laps?")); + Serial.println(F(" Enter between 1 and 9")); + //Serial.println(MAX_LAPS_IN_RACE); + Serial.println(); + incomingByte = '*'; + + while (Serial.available() == 0); // wait for serial input + + // read the incoming serial byte: + incomingByte = Serial.read(); + + switch (incomingByte) + { + case '1': + totalLaps = 1; + notDone = 0; + break; + + case '2': + totalLaps = 2; + notDone = 0; + break; + + case '3': + totalLaps = 3; + notDone = 0; + break; + + case '4': + totalLaps = 4; + notDone = 0; + break; + + case '5': + totalLaps = 5; + notDone = 0; + break; + + case '6': + totalLaps = 6; + notDone = 0; + break; + + case '7': + totalLaps = 7; + notDone = 0; + break; + + case '8': + totalLaps = 8; + notDone = 0; + break; + + case '9': + totalLaps = 9; + notDone = 0; + break; + + case 'c': + cancelRace(); + return; + break; + + default: + Serial.println(F("INVALID entry. Please try again.")); + Serial.println(); + break; + } + if(!notDone){ + if(raceMode == 1) + Serial.println(F("Single race will begin and run for: ")); + else Serial.println(F("Repeating race will begin and run for: ")); + Serial.print(totalLaps); + Serial.println(F(" Lap(s)")); + delay(500); + Serial.println(); + } + } + break; + + case '3': + totalLaps = 9999; + raceMode = 3; + notDone=0; + Serial.print(F("Lap time and count mode will begin... ")); + delay(1000); + Serial.println(); + break; + + case 'c': + cancelRace(); + return; + break; + + default: + Serial.println(F("INVALID entry. Please try again.")); + Serial.println(); + break; + } + } +} + + +void resetRace(void){ + // signal all robots to stop + irsend.sendNEC(ALL_STOP, 32); + delay(20); + irsend.sendNEC(ALL_STOP, 32); + delay(20); + irsend.sendNEC(ALL_STOP, 32); + delay(20); + + delay(END_OF_RACE_DELAY); // let the winner sit a moment + resetOnLine=1; + start=0; + lapCountL =-1; + lapCountR =-1; + winner = 0; + mLeftRaceTime = 0; + mRightRaceTime = 0; + mLeftLapTime[0]=0; + mRightLapTime[0]=0; + + + // reset the flags + leftCheckFlagServo.write(FLAG_DOWN_ANGLE); + rightCheckFlagServo.write(FLAG_DOWN_ANGLE); + greenFlagServo.write(FLAG_DOWN_ANGLE); + delay(500); +} + + +byte checkIfCancel(void){ + + if(Serial.available() > 0){ + incomingByte = Serial.read(); + if(incomingByte == 'c'){ + cancelRace(); + return 1; + } + } + + return 0; + +} + diff --git a/examples/ScoutBotics/Level_1/BSA_Course/BSA_Course.ino b/examples/ScoutBotics/Level_1/BSA_Course/BSA_Course.ino new file mode 100644 index 0000000..7d252dd --- /dev/null +++ b/examples/ScoutBotics/Level_1/BSA_Course/BSA_Course.ino @@ -0,0 +1,72 @@ +/* 10/20/2018 + * + * Using the Gobbit robot with line sensor on the BSA shaped course... + * + * The program will... + * + * 1) Use the drive() function to navigate from the start to the end point. + * 2) Stop at the end and do nothing else. + * + * *** Change the use of the drive() steps to improve your course time. *** + * + * How the drive() function works... + * Calling drive('L/R/F/S/U') will start driving/following the line and continue following until it + * is able to complete the requested direction/turn at the next found intersection + * or end. If it cannot make the requested direction/turn, it will spin around fast + * to indicate it had a problem, and stop the robot. + * + * Turn direction values are ('L')eft, ('R')ight, ('F')orward, ('S')top, or ('U')turn. + * + * To see a video using a similar sketch: https://youtu.be/c2BB-Bc95Ik + * +*/ + +// Choose your Motor Driver... +// To load default settings for either an Ardumoto version 14 or 20, or an Adafruit v2.3, +// uncomment only the following motor driver define that matches. +// If none are uncommented, Ardumoto v14 values will be used. +// +// DO NOT UNCOMMENT MORE THAN ONE +//#define ARDUMOTO_14 +//#define ARDUMOTO_20 +//#define ADAFRUIT_MS + +#include + +// Give your robot a name. +// I called it "MyBot" here, but you call it whatever you want. +// If you make a new name, make sure to find/replace all of the "MyBot" in the sketch with your new name. +GobbitLineCommand MyBot; + +void setup() { + + MyBot.setBatteryVolts(9); + + MyBot.beginGobbit(); + + MyBot.calibrateLineSensor(); + +} + +void loop() { + + // Use the drive() function to drive from the start to the end of the course + // Note: You could use a for() loop to easily repeat drive() statements. + MyBot.drive('F'); + MyBot.drive('F'); + MyBot.drive('F'); + MyBot.drive('F'); + MyBot.drive('F'); + MyBot.drive('F'); + MyBot.drive('F'); + MyBot.drive('F'); + MyBot.drive('F'); + MyBot.drive('F'); + + // at the end, stop the robot + MyBot.drive('S'); + + // We need to tell it to do nothing else forever so it doesn't start the loop() over + while(1); + +} diff --git a/examples/ScoutBotics/Level_2/GLC_GetIRcodes/GLC_GetIRcodes.ino b/examples/ScoutBotics/Level_2/GLC_GetIRcodes/GLC_GetIRcodes.ino new file mode 100644 index 0000000..0cbd88d --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_GetIRcodes/GLC_GetIRcodes.ino @@ -0,0 +1,57 @@ +/* 10/22/2018 + This program uses the IRremote library to find IR codes with the + Gobbit robot wired and configured for use by the GLC_IRcontrol examples. + + Upload this program then use the serial monitor to test and capture IR codes. + + Make sure the baud in the serial monitor is set to 9600 + + This is a small mod of the original example: + + IRremote: IRrecvDemo - demonstrates receiving IR codes with IRrecv + An IR detector/demodulator must be connected to the input RECV_PIN. + Version 0.1 July, 2009 + Copyright 2009 Ken Shirriff + http://arcfn.com +*/ + +#include + + +// Here we are using an IO pin to supply the IR receiver with +5v. +// This is a shortcut for low mAmp power where you may have available IO pins and +// where the +5v is already used and you do not have a breadboard or splitter. +// Comment out this next line if you do not want to use an IO pin as supply. +#define IR_POWER_PIN A0 // declare pin number used for IR receiver power + +int RECV_PIN = A1; // declare pin number connected to signal pin of IR receiver + + +IRrecv irrecv(RECV_PIN); + +decode_results results; + +void setup() +{ + + // If IR_POWER_PIN was defined, this will set the pin to power the IR receiver, +#ifdef IR_POWER_PIN + pinMode(IR_POWER_PIN, OUTPUT); + digitalWrite(IR_POWER_PIN, HIGH); +#endif + + Serial.begin(9600); + // In case the interrupt driver crashes on setup, give a clue + // to the user what's going on. + Serial.println("Enabling IRin"); + irrecv.enableIRIn(); // Start the receiver + Serial.println("Enabled IRin"); +} + +void loop() { + if (irrecv.decode(&results)) { + Serial.println(results.value, HEX); + irrecv.resume(); // Receive the next value + } + delay(100); +} diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/Config.h b/examples/ScoutBotics/Level_2/GLC_IRcontrol/Config.h new file mode 100644 index 0000000..678e744 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/Config.h @@ -0,0 +1,53 @@ +// Choose your Motor Driver... +// To load default settings for either an Ardumoto version 14 or 20, or an Adafruit v2.3, +// uncomment only the following motor driver define that matches. +// If none are uncommented, Ardumoto v14 values will be used. +// +// DO NOT UNCOMMENT MORE THAN ONE +//#define ARDUMOTO_14 +//#define ARDUMOTO_20 +//#define ADAFRUIT_MS + +// What voltage is your battery (whole numbers only) +#define BATTERY_VOLTS 9 + +// If you will use an IO pin to power the IR receiver +5v +// then declare the define to the pin number. +// Otherwise, comment it at out. +// This is a shortcut for low mAmp power where you may have available IO pins and +// where the +5v pin is already used and you do not have a breadboard or splitter on hand. +#define IR_POWER_PIN A0 // comment out to disable + +#define RECV_PIN A1 // pin your IR receiver is using + +#define PIEZO_PIN A2 // piezo beeper/sensor pin + +// veering turn values have stronger turning with larger value, max 99 where 100 is spinning +// Used with Mode 1 and 2 only +#define MINOR_VEER_STRENGTH 30 +#define MODERATE_VEER_STRENGTH 60 // used for FORWARD_RIGHT/LEFT turns/veering +#define MAJOR_VEER_STRENGTH 90 + +#define CALIBRATE_SPEED 0 // speed 0-100 for line follow calibration mode. 0 uses default values. + +#define RC_SPINSPEED 45 // speed 0-100 for right/left turn/spin in Mode 1 only. + +#define BUTTON_HOLD_DELAY 110 // (was 110) delay for hold down repeat to continue last command to allow for time between IR transmissions + +#define DEBOUNCE_DELAY 250 // debounce time to allow finger off of key + +#define TRIM_MAX_TIME 4000 // maximum time in milliseconds in Trim mode (4) to continue the last command without a new command + + +// set these values ONLY if using battery monitoring +// to disable, comment out the define ANALOG_DIVIDER_PIN +//#define ANALOG_DIVIDER_PIN A5 // analog pin your voltage divider is connected +#define CUTOFF 9 // cut off voltage... for example, for a 3 cell lithium, set at 3.0v x 3 cells = 9 +#define SMALL_RES_KOHMS 33 // K ohm value of your small resistor in the divider +#define LARGE_RES_KOHMS 100 // K ohm value of your small resistor in the divider + +// set these values ONLY if using the gripper +// to disable and conserve memory, comment out #define GRIPPER_PIN +//#define GRIPPER_PIN A5 +#define GRIPPER_OPEN 180 // degree of gripper servo where gripper is open +#define GRIPPER_CLOSED 75 // degree of gripper servo where gripper is closed diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/GLC_IRcontrol.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/GLC_IRcontrol.ino new file mode 100644 index 0000000..19de3d4 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/GLC_IRcontrol.ino @@ -0,0 +1,186 @@ +/* 10/25/2018 + + This program will setup the Gobbit robot to be controlled and programmable by an infrared (IR) remote. + + See videos and project directions : http://www.primalengineering.com/robots/irprogram.html + + ** See the Config.h tab for hardware and other settings ** + (the tabs are at the top of this text area) + + "IRremote" library is also needed. Make sure it is installed through the Library Manager + + The Gobbit robot needs an arduino Uno, motor driver, line sensor, infrared receiver, and piezo beeper (element). + The IR remote needs a minimum of 17 buttons. Ideally, the buttons include a 10 key number pad with + two extra buttons on either side of the 0 button, and the typical four directional arrows with an additional + button in the middle for a stop button. The robot will then be able to be controlled by the remote + in three primary modes (see tabs m1, m2, m3, m4, & m5): + 1) Simple RC type control where the robot moves while the direction button is held down + 2) A direction/speed/time based programming mode where either individual or a series of + direction, speed, and timing commands are entered and executed via the remote. This is also + known as "Dead Reckoning." + 3) Line following with intersection detection based programming mode where either individual or a series of + direction, speed, and timing commands are entered and executed via the remote. + A lined course is needed for this mode. + 4) This mode is a minor Trimming mode for mode 1 & 2. It is only used for "trimming" the motors (full forward and + backward directions only) if needed for Mode 1 & 2 to function better. + 5) Tiled line solving mode. This receives IR number commands only which call, in the order entered, + the matching functions in the Tiles, tab to navigate the tiled path, regardless of which end of the + tiles the robot enters first. + + + Use the example program to find the IR codes from your remote: + GobbitLineCommand->ScoutBotics->Level_2->GLC_GetIRcodes + + In GLC_IRgetCodes, Make sure to set your RECV_PIN and your IR_POWER_PIN if used. + + Update the #define IR button values here in the tab "IRcodes.h" to match your remote's buttons + (the tabs are at the top of this text area) + + There are examples of some IR remote control codes in the various tabs of the example + GobbitLineCommand->ScoutBotics->Level_2->GLC_SampleIRcodes + + For GobbitLineCommand function definitions, see the GobbitLineCommand "GLC_Functions" example sketch. + + ** See comments within the code and modes (m1-5 tabs) for further details ** +*/ + +#include "Config.h" +#include "IRcodes.h" + +#include +IRrecv irrecv(RECV_PIN); +decode_results results; + +#include +GobbitLineCommand MyBot; + + +int mode = 0; +int moving = 0; +byte sensorCalibrated = 0; +int lastButtonValue; +int currentSpeed = 100; +unsigned long previousMillis = 0; +int steerTrimF = 0; // steering trim forward motion +int steerTrimB = 0; // steering trim backward motion + + +void setup() { + +#ifdef GRIPPER_PIN + MyBot.setGripPinOpenClosed(GRIPPER_PIN, GRIPPER_OPEN, GRIPPER_CLOSED); +#endif + + pinMode(PIEZO_PIN, OUTPUT); + + // Serial.begin(9600); + + // If IR_POWER_PIN was defined, this will set the pin to power the IR receiver, +#ifdef IR_POWER_PIN + pinMode(IR_POWER_PIN, OUTPUT); + digitalWrite(IR_POWER_PIN, HIGH); +#endif + + + // Due to limited resources and motor shield pin usage, you need to shift some pins around to avoid PWM and timer + // conflicts between IRremote and some motor driver shield pins. + // This also requires using jumpers between the standard driver pins and the locations called here. + // NOTES: + // -More pins can be freed up if the motor shield is removed from the arduino and wired instead of stacked. + // -Since both A4 and A5 are being used, I2C cannot be used in this configuration since an ardumoto style shield is stacked due to wasted pins. + // -TO free up pins, use an I2C based driver like the Adafruit v2. + + // ArduMoto v14 +#ifdef ARDUMOTO_14 + MyBot.setQTRpins(2, 4, 5, 6, 7, 8, A3, A4); // ** + MyBot.setLeftMotorPinsDirPWM(12, 9); // on ardumoto v14, these were 12,3; use a jumper from 3 to 9 + MyBot.setRightMotorPinsDirPWM(13, 10); // on ardumoto v14, these were 13,11; use a jumper from 11 to 10 +#endif + + // ArduMoto v20 +#ifdef ARDUMOTO_20 + MyBot.setQTRpins(5, 6, 7, 8, 12, 13, A3, A4); // ** + MyBot.setLeftMotorPinsDirPWM(4, 10); // on ardumoto v20, these were 4,11; use a jumper from 11 to 10 + MyBot.setRightMotorPinsDirPWM(2, 9); // on ardumoto v20, these were 2,3; use a jumper from 3 to 9 +#endif + + // use these for 9v, not above + // PID tunes here vary from the default due to pin changes and larger overhead timing of processor cycles. + // See the "ArdumotoDefaults.h" or the "AdafruitMSDefaults.h" files in the library "src" folder to view or change the default values. +#if (BATTERY_VOLTS < 10) + MyBot.setPID(0.25, 0.001, 1); // default at 9v is 0.15, .002, 411.1v is 0.15, 0.002, 4 + MyBot.setPIDcoarse(0.3, 0.001, 4.4); // default at 9v is 0.2, 0.001, 3.4 + MyBot.setPIDfineRange(0.6); // default at 9v is 0.04 +#endif + + MyBot.setBatteryVolts(BATTERY_VOLTS); + + MyBot.beginGobbit(); // initializes robot with settings + +#ifdef GRIPPER_PIN + MyBot.gripOpen(); +#endif + + irrecv.enableIRIn(); // Start the IR receiver + +#ifdef ANALOG_DIVIDER_PIN + MyBot.checkBattery(ANALOG_DIVIDER_PIN, CUTOFF, SMALL_RES_KOHMS, LARGE_RES_KOHMS); +#endif + +} + +void loop() { + +#ifdef ANALOG_DIVIDER_PIN + MyBot.checkBattery(ANALOG_DIVIDER_PIN, CUTOFF, SMALL_RES_KOHMS, LARGE_RES_KOHMS); +#endif + + MyBot.move(0, 0); + + // wait for the mode selection to be received + if (buttonDecode()) { + mode = modeSelect(); + } + + // run in the selected mode + if (mode) { + switch (mode) { + + case 1: // Start Simple RC Mode + modeStartedBeep(); + modeOne(); + break; + + case 2: // Start Free Programming Mode + modeStartedBeep(); + modeTwo(); + break; + + case 3: // Start Line Following Programming Mode + modeStartedBeep(); + modeThree(); + break; + + case 4: // Start Trim Mode + modeStartedBeep(); + modeFour(); + break; + + case 5: // Start Tile Mode + modeStartedBeep(); + modeFive(); + break; + } + } +} + + + + + + + + + + + diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/IRcodes.h b/examples/ScoutBotics/Level_2/GLC_IRcontrol/IRcodes.h new file mode 100644 index 0000000..7d1cb26 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/IRcodes.h @@ -0,0 +1,76 @@ +// IR commands... change these to match your buttons/keys +// ** If your remote button only outputs one code when it is pushed multiple times, +// set the XX_BUTTON value to that code with "0x" before it since it is a HEX value +// and leave the XX_BUTTN_ALT value as "0x0" (zero). +// If it toggles between two different codes, as you tap the button, +// set XX_BUTTON value to one code and +// set the XX_BUTTN_ALT value to the other code. + +// Basic required 17 remote buttons +#define ZERO_BUTTON 0x0 +#define ZERO_BUTTN_ALT 0x0 +#define ONE_BUTTON 0x0 +#define ONE_BUTTN_ALT 0x0 +#define TWO_BUTTON 0X0 +#define TWO_BUTTN_ALT 0x0 +#define THREE_BUTTON 0x0 +#define THREE_BUTTN_ALT 0x0 +#define FOUR_BUTTON 0x0 +#define FOUR_BUTTN_ALT 0x0 +#define FIVE_BUTTON 0x0 +#define FIVE_BUTTN_ALT 0x0 +#define SIX_BUTTON 0x0 +#define SIX_BUTTN_ALT 0x0 +#define SEVEN_BUTTON 0x0 +#define SEVEN_BUTTN_ALT 0x0 +#define EIGHT_BUTTON 0x0 +#define EIGHT_BUTTN_ALT 0x0 +#define NINE_BUTTON 0x0 +#define NINE_BUTTN_ALT 0x0 +#define ENTER_BUTTON 0x0 // button at the bottom right of the number pad +#define ENTER_BUTTN_ALT 0x0 +#define BACK_BUTTON 0x0 // button at the bottom left of the number pad +#define BACK_BUTTN_ALT 0x0 + +#define FORWARD_BUTTON 0x0 // forward/up direction button/arrow +#define FORWARD_BUTTN_ALT 0x0 // alternate forward/up direction button/arrow +#define BACKWARD_BUTTON 0x0 // backward/down direction button/arrow +#define BACKWARD_BUTTN_ALT 0x0 // alternate backward/down direction button/arrow +#define RIGHT_BUTTON 0x0 // right direction button/arrow +#define RIGHT_BUTTN_ALT 0x0 // alternate right direction button/arrow +#define LEFT_BUTTON 0x0 // left direction button/arrow +#define LEFT_BUTTN_ALT 0x0 // alternate left direction button/arrow +#define STOP_BUTTON 0x0 // stop button in the middle of the direction keys +#define STOP_BUTTN_ALT 0x0 // alternate stop button in the middle of the direction keys + +// Optionally set these buttons only if you have available buttons and want to expand +// beyond the number and arrow keys for clarity. +// ** These MUST be left with "0x0" values if they are not used. ** +#define FORWARD_RIGHT 0x0 +#define FORWARD_RT_ALT 0x0 +#define FORWARD_LEFT 0x0 +#define FORWARD_LFT_ALT 0x0 +#define BACKWARD_RIGHT 0x0 +#define BACKWARD_RT_ALT 0x0 +#define BACKWARD_LEFT 0x0 +#define BACKWARD_LFT_ALT 0x0 +#define RC_MODE 0x0 +#define RC_MOD_ALT 0x0 +#define FREE_PROGRAM_MODE 0x0 +#define FREE_PROG_MOD_ALT 0x0 +#define LINE_PROGRAM_MODE 0x0 +#define LINE_PROG_MOD_ALT 0x0 +#define TRIM_MODE 0x0 +#define TRIM_MOD_ALT 0x0 +#define PLAY_PROGRAM 0x0 +#define PLAY_PROG_ALT 0x0 +#define STOP_PROGRAM 0x0 +#define STOP_PROG_ALT 0x0 + +// Gripper buttons +#define GRIP_OPEN 0x0 +#define GRIP_OPN_ALT 0x0 +#define GRIP_CLOSE 0x0 +#define GRIP_CLOS_ALT 0x0 + +#define REPEAT_CODE 0xFFFFFFFF // this probably does not need any change diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/Tiles.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/Tiles.ino new file mode 100644 index 0000000..01cf2e3 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/Tiles.ino @@ -0,0 +1,48 @@ +// ScoutBotics 10/27/2018 Level 2 competition + +/* + These are line following and turning solutions for the + tiles called from modeFive() + + Fill in the appropriate steps in each function to solve + the tiles in your competition. +*/ + + +void Tile1() +{ + // add your directions here +} + + +void Tile2() +{ + // add your directions here +} + + +void Tile3() +{ + // add your directions here +} + + +void Tile4() +{ + // add your directions here +} + + +void Tile5() +{ + // add your directions here +} + + +void Tile6() +{ + // add your directions here +} + + + diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/bDecode.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/bDecode.ino new file mode 100644 index 0000000..eb365ae --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/bDecode.ino @@ -0,0 +1,323 @@ +// decode the received IR code to the corresponding key code if valid +int buttonDecode(void) { + /* + Key codes + 0 ZERO_BUTTON + 1 ONE_BUTTON + 2 TWO_BUTTON + 3 THREE_BUTTON + 4 FOUR_BUTTON + 5 FIVE_BUTTON + 6 SIX_BUTTON + 7 SEVEN_BUTTON + 8 EIGHT_BUTTON + 9 NINE_BUTTON + 10 ENTER_BUTTON + 11 BACK_BUTTON + + 12 FORWARD_BUTTON + 13 BACKWARD_BUTTON + 14 RIGHT_BUTTON + 15 LEFT_BUTTON + 16 STOP_BUTTON + + 17 FORWARD_RIGHT + 18 FORWARD_LEFT + 19 BACKWARD_RIGHT + 20 BACKWARD_LEFT + 21 RC_MODE + 22 FREE_PROGRAM_MODE + 23 LINE_PROGRAM_MODE + 24 TRIM_MODE + 25 PLAY_PROGRAM + 26 STOP_PROGRAM + + 27 GRIP_OPEN + 28 GRIP_CLOSE + */ + + if (irrecv.decode(&results)) { + switch (results.value) { + + case ZERO_BUTTON: +#if ZERO_BUTTN_ALT + case ZERO_BUTTN_ALT: +#endif + lastButtonValue = 0; + break; + + case ONE_BUTTON: +#if ONE_BUTTN_ALT + case ONE_BUTTN_ALT: +#endif + lastButtonValue = 1; + break; + + case TWO_BUTTON: +#if TWO_BUTTN_ALT + case TWO_BUTTN_ALT: +#endif + lastButtonValue = 2; + break; + + case THREE_BUTTON: +#if THREE_BUTTN_ALT + case THREE_BUTTN_ALT: +#endif + lastButtonValue = 3; + break; + + case FOUR_BUTTON: +#if FOUR_BUTTN_ALT + case FOUR_BUTTN_ALT: +#endif + lastButtonValue = 4; + break; + + case FIVE_BUTTON: +#if FIVE_BUTTN_ALT + case FIVE_BUTTN_ALT: +#endif + lastButtonValue = 5; + break; + + case SIX_BUTTON: +#if SIX_BUTTN_ALT + case SIX_BUTTN_ALT: +#endif + lastButtonValue = 6; + break; + + case SEVEN_BUTTON : +#if SEVEN_BUTTN_ALT + case SEVEN_BUTTN_ALT: +#endif + lastButtonValue = 7; + break; + + case EIGHT_BUTTON : +#if EIGHT_BUTTN_ALT + case EIGHT_BUTTN_ALT: +#endif + lastButtonValue = 8; + break; + + case NINE_BUTTON : +#if NINE_BUTTN_ALT + case NINE_BUTTN_ALT: +#endif + lastButtonValue = 9; + break; + + case ENTER_BUTTON : +#if ENTER_BUTTN_ALT + case ENTER_BUTTN_ALT: +#endif + lastButtonValue = 10; + break; + + case BACK_BUTTON : +#if BACK_BUTTN_ALT + case BACK_BUTTN_ALT: +#endif + lastButtonValue = 11; + break; + + case FORWARD_BUTTON: +#if FORWARD_BUTTN_ALT + case FORWARD_BUTTN_ALT: +#endif + lastButtonValue = 12; + break; + + case BACKWARD_BUTTON : +#if BACKWARD_BUTTN_ALT + case BACKWARD_BUTTN_ALT: +#endif + lastButtonValue = 13; + break; + + case RIGHT_BUTTON : +#if RIGHT_BUTTN_ALT + case RIGHT_BUTTN_ALT: +#endif + lastButtonValue = 14; + break; + + case LEFT_BUTTON: +#if LEFT_BUTTN_ALT + case LEFT_BUTTN_ALT: +#endif + lastButtonValue = 15; + break; + + case STOP_BUTTON: +#if STOP_BUTTN_ALT + case STOP_BUTTN_ALT: +#endif + lastButtonValue = 16; + break; + + case REPEAT_CODE : + // do nothing and last code will remain as it was + break; + +#if FORWARD_RIGHT + case FORWARD_RIGHT: + lastButtonValue = 17; + break; +#endif + +#if FORWARD_RT_ALT + case FORWARD_RT_ALT: + lastButtonValue = 17; + break; +#endif + + +#if FORWARD_LEFT + case FORWARD_LEFT: + lastButtonValue = 18; + break; +#endif + +#if FORWARD_LFT_ALT + case FORWARD_LEFT_ALT: + lastButtonValue = 18; + break; +#endif + +#if BACKWARD_RIGHT + case BACKWARD_RIGHT: + lastButtonValue = 19; + break; +#endif + +#if BACKWARD_RT_ALT + case BACKWARD_RT_ALT: + lastButtonValue = 19; + break; +#endif + +#if BACKWARD_LEFT + case BACKWARD_LEFT: + lastButtonValue = 20; + break; +#endif + +#if BACKWARD_LFT_ALT + case BACKWARD_LFT_ALT: + lastButtonValue = 20; + break; +#endif + +#if RC_MODE + case RC_MODE: + lastButtonValue = 21; + break; +#endif + +#if RC_MOD_ALT + case RC_MOD_ALT: + lastButtonValue = 21; + break; +#endif + +#if FREE_PROGRAM_MODE + case FREE_PROGRAM_MODE: + lastButtonValue = 22; + break; +#endif + +#if FREE_PROG_MOD_ALT + case FREE_PROG_MOD_ALT: + lastButtonValue = 22; + break; +#endif + +#if LINE_PROGRAM_MODE + case LINE_PROGRAM_MODE: + lastButtonValue = 23; + break; +#endif + +#if LINE_PROG_MOD_ALT + case LINE_PROG_MOD_ALT: + lastButtonValue = 23; + break; +#endif + +#if TRIM_MODE + case TRIM_MODE: + lastButtonValue = 24; + break; +#endif + +#if TRIM_MOD_ALT + case TRIM_MOD_ALT: + lastButtonValue = 24; + break; +#endif + +#if PLAY_PROGRAM + case PLAY_PROGRAM: + lastButtonValue = 25; + break; +#endif + +#if PLAY_PROG_ALT + case PLAY_PROG_ALT: + lastButtonValue = 25; + break; +#endif + + +#if STOP_PROGRAM + case STOP_PROGRAM: + lastButtonValue = 26; + break; +#endif + +#if STOP_PROG_ALT + case STOP_PROG_ALT: + lastButtonValue = 26; + break; +#endif + +#if GRIP_OPEN + case GRIP_OPEN: + lastButtonValue = 27; + break; +#endif + +#if GRIP_OPN_ALT + case GRIP_OPN_ALT: + lastButtonValue = 27; + break; +#endif + +#if GRIP_CLOSE + case GRIP_CLOSE: + lastButtonValue = 28; + break; +#endif + +#if GRIP_CLOS_ALT + case GRIP_CLOS_ALT: + lastButtonValue = 28; + break; +#endif + + default : + lastButtonValue = -1; + break; + } + + irrecv.resume(); // Receive the next value + return 1; + } + + return 0; + +} + diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/beeps.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/beeps.ino new file mode 100644 index 0000000..bb463ad --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/beeps.ino @@ -0,0 +1,91 @@ +/* + Beeps here use cheapBeep() to signal robot recognition of varying inputs and/or status + + Signal beeps in the various modes use either cheapBeep() directly or one of the + additional beep function to indicate: + -mode entered + -valid button press recognized + -cancel recording step/command before enter pressed + -center veer position if adjusting/trimming back and forth + -step recorded after enter pressed + -end of recorded program + -canceled mode and back at main mode select + +*/ + + + +// main beeper function that does not use any additional timers +void cheapBeep(int beepTime) { + // with 500 microseconds up and down, = 1000 microseconds = 1 millisecond per pulse = 1000 hertz + for (int i = 0; i < beepTime; i++) { + digitalWrite(PIEZO_PIN, HIGH); + delayMicroseconds(500); + digitalWrite(PIEZO_PIN, LOW); + delayMicroseconds(500); + } +} + + + + +void modeStartedBeep(void){ + cheapBeep(100); + delay(170); + cheapBeep(40); + delay(40); + cheapBeep(40); + delay(170); + cheapBeep(100); +} + + + + +void cancelStepBeep(void){ + for (int i = 0; i < 10; i++) { + cheapBeep(40); + delay(40); + } +} + + + + +void stepRecordedBeep(void){ + cheapBeep(200); + delay(100); + cheapBeep(200); +} + + + + +void playBackDoneBeep(void){ + cheapBeep(40); + delay(170); + cheapBeep(40); + delay(40); + cheapBeep(40); + delay(40); + cheapBeep(40); + delay(170); + cheapBeep(40); +} + + + + +void cancelSubBeep(void){ + for (int i = 0; i < 3; i++) { + cheapBeep(40); + delay(40); + } +} + + + + +void mainMenuBeep(void){ + cheapBeep(400); +} diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/m1.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m1.ino new file mode 100644 index 0000000..6de3ef9 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m1.ino @@ -0,0 +1,170 @@ +// Mode 1, Simple RC control mode + +// See video example: https://www.youtube.com/watch?time_continue=1&v=ifJDFvL9Avc + +/* USING THE MODE + + Start by entering Mode 1 from the main control menu by pressing the number 1 key + (If another mode is active, press the back button several times until no mode is active) + + Drive the robot: + Hold down the remote keys to move. Use the number keys 1,2,3,4,6,7,8 & 9 or the directional + arrows and the optional FORWARD_RIGHT,FORWARD_LEFT,BACKWARD_RIGHT,BACKWARD_RIGHT if set. + + To change the speed: + 1) Press the enter key + 2) Use the number pad buttons for 0-100 + 3) Press enter again + + NOTE: The initial speed is 100 (full speed). + + To exit the mode, press the back button until the main menu beep is heard. + + + NOTES: + Make sure you keep the remote pointed at the robot. + + See the Config.h tab to adjust the speeds of the Turning and Veering + (keys 1,3,7,9 or the FORWARD_RIGHT,FORWARD_LEFT,BACKWARD_RIGHT,BACKWARD_RIGHT) + + Signal beeps used in mode: + -mode entered + -valid button press recognized + -cancel recording step/command before enter pressed + -center veer position if adjusting/trimming back and forth + -step recorded after enter pressed + -end of recorded program + -canceled mode and back at main mode select +*/ + + +void modeOne(void) { + while (mode) { + +#ifdef ANALOG_DIVIDER_PIN + MyBot.checkBattery(ANALOG_DIVIDER_PIN, CUTOFF, SMALL_RES_KOHMS, LARGE_RES_KOHMS); +#endif + unsigned long currentMillis = millis(); + + if (buttonDecode()) { + + switch (lastButtonValue) { + + case 1: // forward left + case 18: + MyBot.move( currentSpeed, steerTrimF - MODERATE_VEER_STRENGTH); + //MyBot.setMotors( currentSpeed * MODERATE_VEER_STRENGTH / 100, currentSpeed); + // reset time + previousMillis = currentMillis; + break; + + case 2: // forward + case 12: + MyBot.move(currentSpeed , steerTrimF); + //MyBot.setMotors(currentSpeed , currentSpeed); + // reset time + previousMillis = currentMillis; + break; + + case 3: // forward right + case 17: + MyBot.move( currentSpeed, steerTrimF + MODERATE_VEER_STRENGTH); + //MyBot.setMotors(currentSpeed, (currentSpeed * MODERATE_VEER_STRENGTH / 100)); + // reset time + previousMillis = currentMillis; + break; + + case 4: // spin/turn left + case 15: + MyBot.move( RC_SPINSPEED, -100); + //MyBot.setMotors(-currentSpeed, currentSpeed); + // reset time + previousMillis = currentMillis; + break; + + case 5: // stop or in this case do nothing + case 26: + // MyBot.brakeMotors(); // this does not really do any good at this point due to operator trigger speed + MyBot.move( 0, 0); // not actually needed for current code + // reset time + previousMillis = currentMillis; + break; + + case 6: // spin/turn right + case 14: + MyBot.move( RC_SPINSPEED, 100); + //MyBot.setMotors(currentSpeed, -currentSpeed); + // reset time + previousMillis = currentMillis; + break; + + case 7: // backward left + case 20: + MyBot.move( -currentSpeed, steerTrimB - MODERATE_VEER_STRENGTH); + //MyBot.setMotors(-currentSpeed, (-currentSpeed * MODERATE_VEER_STRENGTH / 100)); + // reset time + previousMillis = currentMillis; + break; + + case 8: // backward + case 13: + MyBot.move(-currentSpeed , steerTrimB); + //MyBot.setMotors(-currentSpeed , -currentSpeed); + // reset time + previousMillis = currentMillis; + break; + + case 9: // backward right + case 19: + MyBot.move( -currentSpeed, steerTrimB + MODERATE_VEER_STRENGTH); + //MyBot.setMotors( (-currentSpeed * MODERATE_VEER_STRENGTH / 100), -currentSpeed); + // reset time + previousMillis = currentMillis; + break; + + /* breaking does not work well here since it always assumes forward motion + case 16: // stop button will brake the motors + MyBot.brakeMotors(); + // reset time + previousMillis = currentMillis; + break; + */ + +#ifdef GRIPPER_PIN + case 27: // GRIP OPEN + MyBot.gripOpen(); + // reset time + previousMillis = currentMillis; + break; + + case 28: // GRIP CLOSE + MyBot.gripClose(); + // reset time + previousMillis = currentMillis; + break; +#endif + + case 10: // enter button + currentSpeed = getSpeed(); + // reset time + //previousMillis = currentMillis; + break; + + case 11: // back button cancels input + mode = 0; + currentSpeed = 100; + deBounce(0); + mainMenuBeep(); + break; + } + } + + else { + if (currentMillis - previousMillis >= BUTTON_HOLD_DELAY) { + MyBot.move( 0, 0); + //MyBot.setMotors(0, 0); + } + } + + } +} diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/m2.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m2.ino new file mode 100644 index 0000000..687548d --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m2.ino @@ -0,0 +1,449 @@ +// Mode 2, Dead Reckoning programming + +// See video examples: https://www.youtube.com/watch?v=o2p5wF-MNAs +// https://www.youtube.com/watch?v=ZEsQPePo8oQ + +/* USING THE MODE + + Start by entering Mode 2 from the main control menu by pressing the number 2 key + (If another mode is active, press the back button several times until no mode is active) + + Begin programming steps + + If Forward/Backward: + 1) Press Forward or Backward + 2) If a veering path is desired, + Press the left/right arrow + 0 times leaves direction as straight forward/backward + 1 time is minor veer + 2 times is moderate veer (same as Mode1 button 1,3,7, and 9) + 3 times is major veer (may leave one wheel none turning) + NOTE: you can press the other direction as well to go back to center or to the other direction + 3) Enter the speed + If you want it to run at the speed of the last command, simply press enter + Else If you want a new speed, use the number pad buttons for 0-100 then press enter + NOTE: The initial speed at the first step is 100 (full speed) if no speed is entered. + + Else if Left/Right: + 1) Press Left/Right + 2) Enter the speed + If you want it to run at the speed of the last command, simply press enter + Else If you want a new speed, use the number pad buttons for 0-100 then press enter + NOTE: The initial speed at the first step is 100 (full speed) if no speed is entered. + + Else If Delay: + 1) Enter the time in milliseconds with the number pad then press enter + NOTE: Maximum time value should be less than what can be stored in an unsigned int value = 65,535 + + Else if Stop: + 1) Press the stop direction button + 2) Press enter + + When done, press the enter button again and the program will run. + + To exit the mode, press the back button until the main menu beep is heard. + + + NOTES: + The robot will continue your last command at the end of your program. + Make sure to give it a stop command at the end if your program. + + OR press the stop button when you want it to cancel all the current motion. + + The Stop button will also cancel a running program. + + Do NOT enter new commands while the robot is executing commands until the + last command has been executed. + + It is not advisable to make your last command a delay. + + During a command entry, the back button can be used to cancel the current entry. + + The programmed steps can be run again, or added onto while you are still in Mode 2. + Exiting with the back key to the main menu will delete the program. + + This program will allow for 50 steps to be recorded. + Change the array dir[50] declaration size to increase the number of steps, but + memory may be exceeded with unexpected results if you push it too far. + + See the Config.h tab to adjust the speeds of the Turning and Veering + (keys 1,3,7,9 or the FORWARD_RIGHT,FORWARD_LEFT,BACKWARD_RIGHT,BACKWARD_RIGHT) + + Signal beeps used in mode: + -mode entered + -valid button press recognized + -cancel recording step/command before enter pressed + -center veer position if adjusting/trimming back and forth + -step recorded after enter pressed + -end of recorded program + -canceled mode and back at main mode select + +*/ + + +void modeTwo(void) { + // The dir variable will store the direction that the robot should take. It + // is stored as an array of characters, each of which represents one of + // the following: + // 'L' for left spin + // 'R' for right spin + // 'F' for go forward, veering strength is set by steer[] array + // 'B' for backward, veering strength is set by steer[] array + // 'S' for stop + // 'D' delay + char dir[50] = ""; + + // time in milliseconds to run current step + // max time = 65,535 milliseconds = 65.535 seconds + unsigned int duration[50]; + + // speed for current program step + byte speedThisStep[50]; + + // steering veer strength for current step + // value -100 to +100 where + // 0 is straight and + // +/-100 is full spin left or right + char steer[50]; + + // the number of recorded steps + byte recordLength = 0; + + // flag if play of recorded steps to continue or cancel + byte keepPlaying = 0; + + // flag to keep waiting for rest of step entries + byte waitForEntry = 0; + + // flag to note if a step was correctly recorded or aborted + byte receivedStep = 0; + + while (mode) { + + /* + Key codes + 0 ZERO_BUTTON + 1 ONE_BUTTON + 2 TWO_BUTTON + 3 THREE_BUTTON + 4 FOUR_BUTTON + 5 FIVE_BUTTON + 6 SIX_BUTTON + 7 SEVEN_BUTTON + 8 EIGHT_BUTTON + 9 NINE_BUTTON + 10 ENTER_BUTTON + 11 BACK_BUTTON + + 12 FORWARD_BUTTON + 13 BACKWARD_BUTTON + 14 RIGHT_BUTTON + 15 LEFT_BUTTON + 16 STOP_BUTTON + + 17 FORWARD_RIGHT + 18 FORWARD_LEFT + 19 BACKWARD_RIGHT + 20 BACKWARD_LEFT + 21 RC_MODE + 22 FREE_PROGRAM_MODE + 23 LINE_PROGRAM_MODE + 24 TRIM_MODE + 25 PLAY_PROGRAM + 26 STOP_PROGRAM + */ + + + // record steps + if (keepPlaying == 0) { + if (buttonDecode()) { + + long currentDuration = 0; + int speedEntry = 0; + int veerStrength = 0; + byte veerLoop = 0; + + switch (lastButtonValue) { + + case 12: // forward + case 13: // backward + veerStrength = 0; + if (lastButtonValue == 12) dir[recordLength] = 'F'; + else dir[recordLength] = 'B'; + deBounce(); + + // wait for other values for command + waitForEntry = 1; + while (waitForEntry) { + if (buttonDecode()) { + switch (lastButtonValue) { + + case 14: // right veer + if (veerStrength < 3) { + veerStrength++; + if(veerStrength == 0){ + cheapBeep(30); + delay(35); + cheapBeep(30); + deBounce(0); + } + else deBounce(); + } + else deBounce(0); + break; + + case 15: // left veer + if (veerStrength > -3) { + veerStrength--; + if(veerStrength == 0){ + cheapBeep(30); + delay(35); + cheapBeep(30); + deBounce(0); + } + else deBounce(); + } + else deBounce(0); + break; + + case 0 ...9: // speed is being entered + speedEntry = getKeyedValue(lastButtonValue); + if (speedEntry > -1) { // -1 means it was canceled within the key value process + constrain(speedEntry, 0, 100); + currentSpeed = speedEntry; + receivedStep = 1; + } + break; + + case 10 : // enter pressed so same speed as last step + deBounce(); + receivedStep = 1; + break; + + default: // bad entry... ignore + deBounce(0); + break; + + case 11: // back button... cancel/leave entry + waitForEntry = 0; + deBounce(0); + break; + } + + if (receivedStep) { + duration[recordLength] = 0; + speedThisStep[recordLength] = currentSpeed; + switch (veerStrength) { + + case -1: + veerStrength = -MINOR_VEER_STRENGTH; + break; + + case -2: + veerStrength = -MODERATE_VEER_STRENGTH; + break; + + case -3: + veerStrength = -MAJOR_VEER_STRENGTH; + break; + + case 1: + veerStrength = MINOR_VEER_STRENGTH; + break; + + case 2: + veerStrength = MODERATE_VEER_STRENGTH; + break; + + case 3: + veerStrength = MAJOR_VEER_STRENGTH; + break; + } + steer[recordLength] = veerStrength; + recordLength++; + stepRecordedBeep(); + waitForEntry = 0; + receivedStep = 0; + } + else if (waitForEntry == 0) { // indicate step was canceled and/or null + deBounce(0); + cancelStepBeep(); + } + } + } + break; + + case 14: // right + case 15: // left + if (lastButtonValue == 14) dir[recordLength] = 'R'; + else dir[recordLength] = 'L'; + deBounce(); + + // wait for enter to store step or other key to cancel it + waitForEntry = 1; + while (waitForEntry) { + if (buttonDecode()) { + switch (lastButtonValue) { + + case 0 ...9: // speed is being entered + speedEntry = getKeyedValue(lastButtonValue); + if (speedEntry > -1) { // -1 means it was canceled within the key value process + constrain(speedEntry, 0, 100); + currentSpeed = speedEntry; + receivedStep = 1; + } + break; + + case 10 : // enter pressed so same speed as last step + deBounce(); + receivedStep = 1; + break; + + default: // bad entry... ignore + deBounce(0); + break; + + case 11: // back button... cancel/leave entry + waitForEntry = 0; + deBounce(0); + break; + } + + if (receivedStep) { + duration[recordLength] = 0; + speedThisStep[recordLength] = currentSpeed; + steer[recordLength] = 0; + recordLength++; + stepRecordedBeep(); + waitForEntry = 0; + } + else if (waitForEntry == 0) { // indicate step was canceled and/or null + deBounce(0); + cancelStepBeep(); + } + } + } + break; + + case 16: // stop + deBounce(); + waitForEntry = 1; + + while (waitForEntry) { + if (buttonDecode()) { + if (lastButtonValue == 10) { + dir[recordLength] = 'S'; + duration[recordLength] = 0; + speedThisStep[recordLength] = 0; + steer[recordLength] = 0; + recordLength++; + deBounce(); + stepRecordedBeep(); + waitForEntry = 0; + } + else if (lastButtonValue == 11) { // back button... cancel/leave entry + waitForEntry = 0; + deBounce(0); + cancelStepBeep(); + } + else { + deBounce(0); // not a valid button so ignore + } + } + } + break; + + + case 0 ...9: // if numbers then start delay recording + currentDuration = getKeyedValue(lastButtonValue); + if (currentDuration > -1) { // -1 means it was canceled within the key value process + dir[recordLength] = 'D'; + duration[recordLength] = currentDuration; + speedThisStep[recordLength] = currentSpeed; + steer[recordLength] = 0; + recordLength++; + deBounce(0); + stepRecordedBeep(); + } + else { + deBounce(0); + cancelStepBeep(); // indicate step was canceled and/or null + } + break; + + case 10: // enter button or + case 25: // play program button to start recorded program + keepPlaying = 1; + deBounce(); + break; + + case 11: // Back button cancels mode + mode = 0; + currentSpeed = 100; + deBounce(0); + mainMenuBeep(); // indicates mode was exited + break; + + default: // bad entry... ignore + deBounce(0); + break; + } + } + } + + + // play the recorded steps + else if (keepPlaying) { + for (int i = 0; i < recordLength; i++) + { + unsigned long currentMillis = millis(); + unsigned long lastMillis = currentMillis; + + switch (dir[i]) { + + case 'L' : // left spin + MyBot.move(speedThisStep[i], -100); + break; + + case 'R' : // right spin + MyBot.move(speedThisStep[i], 100); + break; + + case 'F' : // forward + MyBot.move(speedThisStep[i], steer[i] + steerTrimF); + break; + + case 'B' : // backward + MyBot.move(-speedThisStep[i], steer[i] + steerTrimB); + break; + + case 'S' : // stop + //MyBot.move(0, 0); + MyBot.brakeMotors(); + break; + + case 'D': // play a delay while checking if a cancel or stop was pressed + while ((currentMillis - lastMillis < duration[i]) && keepPlaying) { + keepPlaying = keepGoing(); + currentMillis = millis(); + } + break; + + } + + if (keepPlaying) keepPlaying = keepGoing(); + + // leave play loop if found a stop IR signal + if (!keepPlaying) i = recordLength; + } + playBackDoneBeep(); + keepPlaying = 0; + } + + } + +} + + + + diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/m3.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m3.ino new file mode 100644 index 0000000..d1625d9 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m3.ino @@ -0,0 +1,357 @@ +// Mode 3, Line following with intersection detection based programming + +// See video example: https://www.youtube.com/watch?v=0xryqQvQNyA + +/* USING THE MODE + + Start by entering Mode 3 from the main control menu by pressing the number 3 key + (If another mode is active, press the back button several times until no mode is active) + + NOTE: MAKE SURE THE ROBOT IS SITTING ON THE LINED COURSE BEFORE ENTERING THE MODE. IT NEEDS + TO CALIBRATE WHEN IT FIRST ENTERS THE MODE. + + Begin programming steps + + If Forward/Left Turn/Right Turn/Turn around(down button): + 1) Press the directional key desired + 2) Enter the speed + If you want it to run at the speed of the last command, simply press enter + Else If you want a new speed, use the number pad buttons for 0-100 then press enter + NOTE: The initial speed at the first step is 100 (full speed) if no speed is entered. + + Else If Delay: + 1) Enter the time in milliseconds with the number pad then press enter + NOTE: Maximum time value should be less than what can be stored in an unsigned int value = 65,535 + + Else if Stop: + 1) Press the stop direction button + 2) Press enter + + When done, press the enter button again and the program will run. + + To exit the mode, press the back button until the main menu beep is heard. + + + LIMITATIONS: This Mode does not provide any off the line navigation. + + Any speed values less than 100 may not work well. The PID values and/or the + formulas inside the GLC library will need work for this functionality to + perform at an acceptable level. + + NOTES: The robot will continue your last command at the end of your program. + Make sure to give it a stop command at the end if your program. + + OR press the stop button when you want it to cancel all the current motion. + + The Stop button will also cancel a running program. + + Do NOT enter new commands while the robot is executing commands until the + last command has been executed. + + It is not advisable to make your last command a delay. + + During a command entry, the back button can be used to cancel the current entry. + + The programmed steps can be run again, or added onto while you are still in Mode 2. + Exiting with the back key to the main menu will delete the program. + + This program will allow for 50 steps to be recorded. + Change the array dir[50] declaration size to increase the number of steps, but + memory may be exceeded with unexpected results if you push it too far. + + See the Config.h tab to adjust the calibration speed. + + If the robot is moving very roughly, it may not have calibrated well. + Try turning it off, turn it on, reset it over the line, then enter the mode again. + + Signal beeps used in mode: + -mode entered + -valid button press recognized + -cancel recording step/command before enter pressed + -center veer position if adjusting/trimming back and forth + -step recorded after enter pressed + -end of recorded program + -canceled mode and back at main mode select + +*/ + + + +void modeThree(void) { + + // the robot will calibrate as soon as the mode is entered if it was not already calibrated + if(!sensorCalibrated){ + MyBot.calibrateLineSensor(CALIBRATE_SPEED); + sensorCalibrated = 1; + } + + // The dir variable will store the direction that the robot should take at the next intersection or end of line. + // It is stored as an array of characters, each of which represents one of + // the following: + // 'L' for turn left + // 'R' for turn right + // 'F' for go forward/findNext (going straight through an intersection) + // 'B' for making a U-turn (not always smart enough to be correct) + // 'S' for stop + // 'D' delay, probably should not do this after a "Forward" + char dir[50] = ""; + + // time in milliseconds to run current step + // max time = 65,535 milliseconds = 65.535 seconds + unsigned int duration[50]; + + // speed for current program step + byte speedThisStep[50]; + + // the number of recorded steps + byte recordLength = 0; + + // flag if play of recorded steps to continue or cancel + byte keepPlaying = 0; + + // flag to keep waiting for rest of step entries + byte waitForEntry = 0; + + // flag to note if a step was correctly recorded or aborted + byte receivedStep = 0; + + while (mode) { + + // record steps + if (keepPlaying == 0) { + if (buttonDecode()) { + + long currentDuration = 0; + int speedEntry = 0; + + switch (lastButtonValue) { + + case 12: // forward/findNext + dir[recordLength] = 'F'; + deBounce(); + + // wait for other values for command + waitForEntry = 1; + while (waitForEntry) { + if (buttonDecode()) { + switch (lastButtonValue) { + + case 0 ...9: // speed is being entered + speedEntry = getKeyedValue(lastButtonValue); + if (speedEntry > -1) { // -1 means it was canceled within the key value process + constrain(speedEntry, 0, 100); + currentSpeed = speedEntry; + receivedStep = 1; + } + break; + + case 10 : // enter pressed so same speed as last step + deBounce(); + receivedStep = 1; + break; + + default: // bad entry... ignore + deBounce(0); + break; + + case 11: // back button... cancel/leave entry + waitForEntry = 0; + deBounce(0); + break; + } + + if (receivedStep) { + duration[recordLength] = 0; + speedThisStep[recordLength] = currentSpeed; + recordLength++; + stepRecordedBeep(); + waitForEntry = 0; + receivedStep = 0; + } + else if (waitForEntry == 0) { // indicate step was canceled and/or null + deBounce(0); + cancelStepBeep(); + } + } + } + break; + + + case 13: // backward + case 14: // right + case 15: // left + case 16: // stop + if (lastButtonValue == 13) dir[recordLength] = 'B'; + else if (lastButtonValue == 14) dir[recordLength] = 'R'; + else if (lastButtonValue == 15) dir[recordLength] = 'L'; + else dir[recordLength] = 'S'; + deBounce(); + + // wait for other values for command (either enter or back/cancel) + waitForEntry = 1; + + while (waitForEntry) { + if (buttonDecode()) { + if (lastButtonValue == 10) { + duration[recordLength] = 0; + speedThisStep[recordLength] = 0; + recordLength++; + deBounce(); + stepRecordedBeep(); + waitForEntry = 0; + } + else if (lastButtonValue == 11) { // back button... cancel/leave entry + waitForEntry = 0; + deBounce(0); + cancelStepBeep(); + } + else { + deBounce(0); // not a valid button so ignore + } + } + } + break; + + + case 0 ...9: // if numbers then start delay recording + currentDuration = getKeyedValue(lastButtonValue); + if (currentDuration > -1) { // -1 means it was canceled within the key value process + dir[recordLength] = 'D'; + duration[recordLength] = currentDuration; + speedThisStep[recordLength] = currentSpeed; + recordLength++; + deBounce(0); + stepRecordedBeep(); + } + else { + deBounce(0); + cancelStepBeep(); // indicate step was canceled and/or null + } + break; + + case 10: // enter button or + case 25: // play program button to start recorded program + keepPlaying = 1; + deBounce(); + break; + + case 11: // Back button cancels mode + mode = 0; + currentSpeed = 100; + deBounce(0); + mainMenuBeep(); // indicates mode was exited + break; + + default: // bad entry... ignore + deBounce(0); + break; + } + } + } + + + // play the recorded steps + else if (keepPlaying) { + // reset the found flags + // this means the robot can re-run the program with the same turn actions + MyBot.foundLeft = 0; + MyBot.foundRight = 0; + MyBot.foundForward = 0; + MyBot.foundEnd = 0; + MyBot.foundMark = 0; + + for (int i = 0; i < recordLength; i++) + { + + MyBot.setMinMaxSpeeds(-speedThisStep[i], speedThisStep[i]); + + unsigned long currentMillis = millis(); + unsigned long lastMillis = currentMillis; + + if (keepPlaying) { + switch (dir[i]) { + + case 'L' : // left + MyBot.turn('L'); + break; + + case 'R' : // right + MyBot.turn('R'); + break; + + case 'F' : // forward/findNext intersection or end of line + if(!MyBot.foundEnd) keepPlaying = findNext(keepPlaying); + else { + notFoundSpin(); + keepPlaying = 0; + } + break; + + case 'B' : // U turn/ go back the way it came + + // if there are no found flags, as with an initial back command before any following, + // assume the robot is on a line, not an intersection, and make a single turn + if(!MyBot.foundRight && !MyBot.foundLeft && !MyBot.foundEnd) MyBot.turn('L'); + + // else at least one flag was found so figure out the best turn + + else if (!MyBot.foundRight) { + MyBot.turn('R'); + } + else if (!MyBot.foundLeft) { + MyBot.turn('L'); + } + else { // must be at a tee or full intersection + MyBot.turn('U'); // this will make two lefts but smoother since it will not slow down at the first + } + break; + + case 'S' : // stop + MyBot.brakeMotors(); + break; + + case 'D': // play a delay while checking if a cancel or stop was pressed + currentMillis = millis(); + lastMillis = currentMillis; + while ((currentMillis - lastMillis < duration[i]) && keepPlaying) { + keepPlaying = keepGoing(); + currentMillis = millis(); + } + break; + + } + } + + if (keepPlaying) keepPlaying = keepGoing(); + + // leave play loop if found a stop IR signal + if (!keepPlaying) i = recordLength; + } + playBackDoneBeep(); + keepPlaying = 0; + } + + } + +} + + +// spin in error if cannot make requested turn at found intersection +void notFoundSpin(void) { + MyBot.move(100, 100); + delay(1000); + MyBot.move(0, 0); +} + +// follow the line until next intersection or end of line is found while a stop remote command has not been seen +byte findNext(byte _keepPlaying) { + MyBot.followLine(2); // clears flags + while (!MyBot.foundLeft && !MyBot.foundForward && !MyBot.foundRight && !MyBot.foundEnd && _keepPlaying) { + MyBot.followLine(2); + _keepPlaying = keepGoing(); + } + return _keepPlaying; +} + + + diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/m4.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m4.ino new file mode 100644 index 0000000..304d545 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m4.ino @@ -0,0 +1,202 @@ +// Mode 4, Trimming mode for mode 1 & 2 + +// See video example: https://www.youtube.com/watch?v=QZYdgpcy4Q0 + +/* USING THE MODE + + Start by entering Mode 4 from the main control menu by pressing the number 4 key + (If another mode is active, press the back button several times until no mode is active) + + NOTE: MAKE SURE YOU ARE IN A LARGE ENOUGH AREA THAT YOUR ROBOT CAN DRIVE LONG ENOUGH TO + ALLOW YOU TO MAKE THE ADJUSTMENTS. + + Trim the robot: + + 1) Set the speed you would primarily like to trim: + a) Press the enter button + b) Use the number pad buttons for 0-100 + c) Press enter again + + 2) Point the robot in an open direction and press the forward/up button + + 3) While the robot is moving forward, press left or right as needed until the robot + is tracking straight. + + 4) Press the stop key to stop the robot if needed. + + 5) Press the backward/down button, press left or right as needed until the robot + is tracking straight. + + 6) Repeat the steps 2-5 as needed until you find the best trim value. You can also + change the speed again if needed. + + To exit the mode, press the back button until the main menu beep is heard. + + + LIMITATIONS: The trim settings are applied to all speeds. + + The trim settings are reset when you power off your robot. + + As your battery runs down, and your motors heat up during operation, + the trim values may be less accurate. The only way to improve this + is with more sensor data, such as encoders or an IMU. + + NOTES: See the Config.h tab to adjust the TRIM_MAX_TIME. + + Signal beeps used in this mode: + -mode entered + -valid button press recognized + -center veer position if adjusting/trimming back and forth + -canceled mode and back at main mode select +*/ + +void modeFour(void) { + while (mode) { +#ifdef ANALOG_DIVIDER_PIN + MyBot.checkBattery(ANALOG_DIVIDER_PIN, CUTOFF, SMALL_RES_KOHMS, LARGE_RES_KOHMS); +#endif + unsigned long currentMillis = millis(); + + if (buttonDecode()) { + + switch (lastButtonValue) { + + case 1: // forward left + case 18: + MyBot.move( currentSpeed, steerTrimF - MODERATE_VEER_STRENGTH); + //MyBot.setMotors( currentSpeed * MODERATE_VEER_STRENGTH / 100, currentSpeed); + // reset time + previousMillis = currentMillis; + moving = 0; + break; + + case 2: // forward + case 12: + MyBot.move(currentSpeed , steerTrimF); + //MyBot.setMotors(currentSpeed , currentSpeed); + // reset time + previousMillis = currentMillis; + moving = 1; + break; + + case 3: // forward right + case 17: + MyBot.move( currentSpeed, steerTrimF + MODERATE_VEER_STRENGTH); + //MyBot.setMotors(currentSpeed, (currentSpeed * MODERATE_VEER_STRENGTH / 100)); + // reset time + previousMillis = currentMillis; + moving = 0; + break; + + case 4: // spin/turn left + case 15: + if (moving == 1) { + deBounce(); + steerTrimF--; + MyBot.move(currentSpeed, steerTrimF); + } + else if (moving == -1) { + deBounce(); + steerTrimB++; // this is actually reverse of what the robot thinks, but easier to understand as an operator + MyBot.move(-currentSpeed, steerTrimB); + } + else { + MyBot.move( currentSpeed, -100); + //MyBot.setMotors(-currentSpeed, currentSpeed); + // reset time + previousMillis = currentMillis; + } + break; + + case 5: // stop or in this case do nothing + case 16: + case 26: + MyBot.move( 0, 0); + // reset time + previousMillis = currentMillis; + moving = 0; + break; + + case 6: // spin/turn right + case 14: + if (moving == 1) { + deBounce(); + steerTrimF++; + MyBot.move(currentSpeed, steerTrimF); + } + else if (moving == -1) { + deBounce(); + steerTrimB--; // this is actually reverse of what the robot thinks, but easier to understand as an operator + MyBot.move(-currentSpeed, steerTrimB); + } + else { + MyBot.move( currentSpeed, 100); + //MyBot.setMotors(currentSpeed, -currentSpeed); + // reset time + previousMillis = currentMillis; + } + break; + + case 7: // backward left + case 20: + MyBot.move( -currentSpeed, steerTrimB - MODERATE_VEER_STRENGTH); + //MyBot.setMotors(-currentSpeed, (-currentSpeed * MODERATE_VEER_STRENGTH / 100)); + // reset time + previousMillis = currentMillis; + moving = 0; + break; + + case 8: // backward + case 13: + MyBot.move(-currentSpeed , steerTrimB); + //MyBot.setMotors(-currentSpeed , -currentSpeed); + // reset time + previousMillis = currentMillis; + moving = -1; + break; + + case 9: // backward right + case 19: + MyBot.move( -currentSpeed, steerTrimB + MODERATE_VEER_STRENGTH); + //MyBot.setMotors( (-currentSpeed * MODERATE_VEER_STRENGTH / 100), -currentSpeed); + // reset time + previousMillis = currentMillis; + moving = 0; + break; + + case 10: // enter button + currentSpeed = getSpeed(); + // reset time + //previousMillis = currentMillis; + break; + + case 11: // back button cancels input + MyBot.move( 0, 0); + mode = 0; + currentSpeed = 100; + moving = 0; + deBounce(0); + mainMenuBeep(); + break; + } + } + + else { + if (moving == 0) { // stop of no signal received in none forward or backward + if (currentMillis - previousMillis >= BUTTON_HOLD_DELAY) { + MyBot.move( 0, 0); + //MyBot.setMotors(0, 0); + } + } + // stop in case it has gone out of range + else if (currentMillis - previousMillis > TRIM_MAX_TIME ) { + MyBot.move( 0, 0); + //MyBot.setMotors(0, 0); + moving = 0; + } + } + } + + +} + diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/m5.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m5.ino new file mode 100644 index 0000000..afcd926 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/m5.ino @@ -0,0 +1,222 @@ +// Mode 5, Tiled line programming and solving mode + +// This mode calls the Tile#()'s in the Tiles tab. You will need to add the turn and follow +// commands there to solve your tiles before this mode will be effective. + +/* USING THE MODE + + Start by entering mode5 from the main control menu + (If another mode is active, press the back button several times until no mode is active) + + NOTE: MAKE SURE THE ROBOT IS SITTING ON THE LINED COURSE BEFORE ENTERING THE MODE. IT NEEDS + TO CALIBRATE WHEN IT FIRST ENTERS THE MODE. + + Begin programming the order of the tiles desired + + 1) Enter the tile number with the keypad + + 2) Press the enter button + + 3) When done entering tile numbers, press the enter button again and the program will run. + + To exit the mode, press the back button until the main menu beep is heard. + + + NOTES: The robot will stop at the next transition found after executing all of the called tiles. + + No speed or delay functionality is in this mode. + + The Stop button will cancel the running program. + + Do NOT enter new commands while the robot is executing commands until the + last command has been executed. + + During a command entry, the back button can be used to cancel the current entry. + + The programmed steps can be run again, or added onto while you are still in Mode 5. + Exiting with the back key to the main menu will delete the program. + + See the Config.h tab to adjust the calibration speed. + + If the robot is moving very roughly, it may not have calibrated well. + Try turning it off, turn it on, reset it over the line, then enter the mode again. + + Signal beeps used in this mode: + -mode entered + -valid button press recognized + -cancel recording step/command before enter pressed + -step recorded after enter pressed + -end of recorded program + -canceled mode and back at main mode select + + */ + + +void modeFive(void) { + + // the robot will calibrate as soon as the mode is entered if it was not already calibrated + if(!sensorCalibrated){ + MyBot.calibrateLineSensor(CALIBRATE_SPEED); + sensorCalibrated = 1; + } + + // The tileNumber variable will store the tile that the robot will need to navigate. + // It is stored as an array of bytes (0-255), each of which represents the number/style of the tile. + // This will allow for 50 steps in the program. + // Change the array declared size to increase, but + // memory may be exceeded. + byte tileNumber[50] = ""; + + // the number of recorded steps + byte recordLength = 0; + + // flag if play of recorded steps to continue or cancel + byte keepPlaying = 0; + + // flag to keep waiting for rest of step entries + byte waitForEntry = 0; + + // flag to note if a step was correctly recorded or aborted + byte receivedStep = 0; + + while (mode) { + + /* + Key codes + 0 ZERO_BUTTON + 1 ONE_BUTTON + 2 TWO_BUTTON + 3 THREE_BUTTON + 4 FOUR_BUTTON + 5 FIVE_BUTTON + 6 SIX_BUTTON + 7 SEVEN_BUTTON + 8 EIGHT_BUTTON + 9 NINE_BUTTON + 10 ENTER_BUTTON + 11 BACK_BUTTON + + 12 FORWARD_BUTTON + 13 BACKWARD_BUTTON + 14 RIGHT_BUTTON + 15 LEFT_BUTTON + 16 STOP_BUTTON + + 17 FORWARD_RIGHT + 18 FORWARD_LEFT + 19 BACKWARD_RIGHT + 20 BACKWARD_LEFT + 21 RC_MODE + 22 FREE_PROGRAM_MODE + 23 LINE_PROGRAM_MODE + 24 TRIM_MODE + 25 PLAY_PROGRAM + 26 STOP_PROGRAM + */ + + // record steps + if (keepPlaying == 0) { + if (buttonDecode()) { + + byte currentTile = 0; + + switch (lastButtonValue) { + + case 0 ...9: // if numbers then record tile number + currentTile = getKeyedValue(lastButtonValue); + if (currentTile > -1) { // -1 means it was canceled within the key value process + tileNumber[recordLength] = currentTile; + recordLength++; + deBounce(0); + stepRecordedBeep(); + } + else { + deBounce(0); + cancelStepBeep(); // indicate step was canceled and/or null + } + break; + + case 10: // enter button or + case 25: // play program button to start recorded program + keepPlaying = 1; + deBounce(); + break; + + case 11: // Back button cancels mode + mode = 0; + currentSpeed = 100; + deBounce(0); + mainMenuBeep(); // indicates mode was exited + break; + + default: // bad entry... ignore + deBounce(0); + break; + } + } + } + + + // play the recorded steps + else if (keepPlaying) { + + // start following the line and find the next transition + MyBot.followLine(3); + + // at the first transition a tile must have been entered, so start + // the tile navigating calls + for (int i = 0; i < recordLength; i++) + { + + switch (tileNumber[i]) { + + case 1 : // tile 1 + Tile1(); + break; + + case 2 : // tile 2 + Tile2(); + break; + + + case 3 : // tile 3 + Tile3(); + break; + + case 4 : // tile 4 + Tile4(); + break; + + case 5 : // tile 5 + Tile5(); + break; + + case 6 : // tile 6 + Tile6(); + break; + + } + + if (keepPlaying) keepPlaying = keepGoing(); + + // leave play loop if found a stop IR signal + if (!keepPlaying){ + i = recordLength; + MyBot.brakeMotors(); + } + } + + // when it is here, the the tile functions have been completed + // and it should stop at the next transition + MyBot.brakeMotors(); + playBackDoneBeep(); + keepPlaying = 0; + } + + } + +} + + + + diff --git a/examples/ScoutBotics/Level_2/GLC_IRcontrol/utilities.ino b/examples/ScoutBotics/Level_2/GLC_IRcontrol/utilities.ino new file mode 100644 index 0000000..103d557 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_IRcontrol/utilities.ino @@ -0,0 +1,188 @@ +// these are some of the functions used to select modes, by the modes to get key strokes from the IR remote, +// and checking status while running programs + + + + +// called by main loop to set flags for mode requested +byte modeSelect(void) { + + byte mode; + + switch (lastButtonValue) { + + case 1: // RC Mode 1 + case 21: + mode = 1; + break; + + case 2: // Free Program Mode 2 + case 22: + mode = 2; + break; + + case 3: // Line Program Mode 3 + case 23: + mode = 3; + break; + + case 4: // Trim Mode 4 + case 24: + mode = 4; + break; + + case 5: // Tile Mode 5 + //case ??: // no alternate button code + mode = 5; + break; + + case 11: // Back button cancels input + mode = 0; + deBounce(0); + mainMenuBeep(); + return mode; + break; + + default: + deBounce(0); + return 0; + break; + } + + deBounce(0); + return mode; +} + + + + + +long getKeyedValue() { + return getKeyedValue(-1); +} + +// initialValue as -1 if no prior received key press. Otherwise send lastButtonValue +long getKeyedValue(int initialValue) { + + deBounce(); + + byte entries[10]; + int numberEntries = 0; + long newValue = 0; + long multiplyBy = 1; + + + if (initialValue > -1) { + entries[numberEntries] = lastButtonValue; + numberEntries++; + } + + while (1) { // loop until it either receives a return or back + if (buttonDecode()) { + //Serial.println(lastButtonValue); + switch (lastButtonValue) { + + case 0 ...9: + entries[numberEntries] = lastButtonValue; + deBounce(); + break; + + case 10 : // ENTER button when finished entering values + //Serial.println("number of entries"); + //Serial.println(numberEntries); + for (int i = numberEntries - 1; i >= 0 ; i--) { + newValue = newValue + (entries[i] * multiplyBy); + multiplyBy = multiplyBy * 10; + } + + //constrain(newValue, 0, 65535); // may not be working per serial print + if (newValue > 65535) newValue = 65535; + else if (newValue < 0) newValue = 0; + //Serial.println("newValue in getKeyedValue"); + //Serial.println(newValue); + deBounce(); + return newValue; + break; + + case 11 : // BACK button, cancel process + deBounce(0); + cancelSubBeep(); + return -1; + break; + + default: + // keep the index from tracking an invalid value + numberEntries--; + deBounce(0); + break; + } + numberEntries++; + } + } +} + + + + + +int getSpeed() { + return getSpeed(-1); +} + +// gets speed from keyed value or returns last speed if keyed is null +// initialValue as -1 if no prior received key press. Otherwise send lastButtonValue +int getSpeed(int initialValue) { + + unsigned int newSpeed = getKeyedValue(initialValue); + + if (newSpeed < 0) { + return currentSpeed; + } + + constrain(newSpeed, 0, 100); + //Serial.println(newSpeed); + return newSpeed; +} + + + + + +void deBounce(void) { + deBounce(1); +} + +// deBounce the button presses and make a beep to indicate it was received +void deBounce(byte beep) { + //tone(PIEZO_PIN, 3000); // tone is in conflict with IR library + if (beep) { + cheapBeep(60); + delay(DEBOUNCE_DELAY); + } + else delay(60 + DEBOUNCE_DELAY); + irrecv.resume(); // clear buffer of any extra reads +} + + + + + +// check if it is still OK to keep playing the recording. +// if there are any stop commands, it will return 0 and stop the motors +int keepGoing(void) { + if (buttonDecode()) { + if (lastButtonValue == 16 || lastButtonValue == 11 || lastButtonValue == 26) { // back button cancels input of step + MyBot.move(0, 0); + deBounce(0); + cancelStepBeep(); + return 0; + } + } + else return 1; +} + + + + + + diff --git a/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/DirectTV_RC23.h b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/DirectTV_RC23.h new file mode 100644 index 0000000..21d0e84 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/DirectTV_RC23.h @@ -0,0 +1,78 @@ +// older DirecTV remote model RC23 + +// IR commands... change these to match your buttons/keys +// ** If your remote button only outputs one code when it is pushed multiple times, +// set the XX_BUTTON value to that code with "0x" before it since it is a HEX value +// and leave the XX_BUTTN_ALT value as "0x0" (zero). +// If it toggles between two different codes, as you tap the button, +// set XX_BUTTON value to one code and +// set the XX_BUTTN_ALT value to the other code. + +// Basic required 17 remote buttons +#define ZERO_BUTTON 0xF32F72D7 +#define ZERO_BUTTN_ALT 0x0 +#define ONE_BUTTON 0xC9767F76 +#define ONE_BUTTN_ALT 0x0 +#define TWO_BUTTON 0XC8155AB1 +#define TWO_BUTTN_ALT 0x0 +#define THREE_BUTTON 0xB6996DAE +#define THREE_BUTTN_ALT 0x0 +#define FOUR_BUTTON 0x969AE844 +#define FOUR_BUTTN_ALT 0x0 +#define FIVE_BUTTON 0x4AAFAC67 +#define FIVE_BUTTN_ALT 0x0 +#define SIX_BUTTON 0x9C2A936C +#define SIX_BUTTN_ALT 0x0 +#define SEVEN_BUTTON 0x833ED333 +#define SEVEN_BUTTN_ALT 0x0 +#define EIGHT_BUTTON 0x55F2B93 +#define EIGHT_BUTTN_ALT 0x0 +#define NINE_BUTTON 0xDE78B0D0 +#define NINE_BUTTN_ALT 0x0 +#define ENTER_BUTTON 0x3F23F43 // button at the bottom right of the number pad +#define ENTER_BUTTN_ALT 0x0 +#define BACK_BUTTON 0xBDE97C12 // button at the bottom left of the number pad +#define BACK_BUTTN_ALT 0x0 + +#define FORWARD_BUTTON 0xF24119FE // forward/up direction button/arrow +#define FORWARD_BUTTN_ALT 0x0 // alternate forward/up direction button/arrow +#define BACKWARD_BUTTON 0xB489062B // backward/down direction button/arrow +#define BACKWARD_BUTTN_ALT 0x0 // alternate backward/down direction button/arrow +#define RIGHT_BUTTON 0xBC9DF06 // right direction button/arrow +#define RIGHT_BUTTN_ALT 0x0 // alternate right direction button/arrow +#define LEFT_BUTTON 0xC53794B4 // left direction button/arrow +#define LEFT_BUTTN_ALT 0x0 // alternate left direction button/arrow +#define STOP_BUTTON 0x75A956A7 // stop button in the middle of the direction keys +#define STOP_BUTTN_ALT 0x0 // alternate stop button in the middle of the direction keys + +// Optionally set these buttons only if you have available buttons and want to expand +// beyond the number and arrow keys for clarity. +// ** These MUST be left with "0x0" values if they are not used. ** +#define FORWARD_RIGHT 0x34498102 +#define FORWARD_RT_ALT 0x0 +#define FORWARD_LEFT 0x427EBE9F +#define FORWARD_LFT_ALT 0x0 +#define BACKWARD_RIGHT 0xF640360 +#define BACKWARD_RT_ALT 0x0 +#define BACKWARD_LEFT 0xBB9BDEE7 +#define BACKWARD_LFT_ALT 0x0 +#define RC_MODE 0x0 +#define RC_MOD_ALT 0x0 +#define FREE_PROGRAM_MODE 0x0 +#define FREE_PROG_MOD_ALT 0x0 +#define LINE_PROGRAM_MODE 0x0 +#define LINE_PROG_MOD_ALT 0x0 +#define TRIM_MODE 0x0 +#define TRIM_MOD_ALT 0x0 +#define PLAY_PROGRAM 0x9A6F0576 +#define PLAY_PROG_ALT 0x0 +#define STOP_PROGRAM 0xFBAD8623 +#define STOP_PROG_ALT 0x0 + +// Gripper buttons Gripper codes commented out to save program space if not used +#define GRIP_OPEN 0x0 //0x5815B090 // on this remote using the channel down/backward +#define GRIP_OPN_ALT 0x0 +#define GRIP_CLOSE 0x0 //0x165412B7 // on this remote using the channel up/forward +#define GRIP_CLOS_ALT 0x0 + +#define REPEAT_CODE 0xFFFFFFFF // this probably does not need any change diff --git a/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/DirectTV_RC73.h b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/DirectTV_RC73.h new file mode 100644 index 0000000..453cc76 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/DirectTV_RC73.h @@ -0,0 +1,78 @@ +// DirecTV remote model RC73 (also known as a Genie remote and has RF) + +// IR commands... change these to match your buttons/keys +// ** If your remote button only outputs one code when it is pushed multiple times, +// set the XX_BUTTON value to that code with "0x" before it since it is a HEX value +// and leave the XX_BUTTN_ALT value as "0x0" (zero). +// If it toggles between two different codes, as you tap the button, +// set XX_BUTTON value to one code and +// set the XX_BUTTN_ALT value to the other code. + +// Basic required 17 remote buttons +#define ZERO_BUTTON 0xF32F72D7 +#define ZERO_BUTTN_ALT 0x0 +#define ONE_BUTTON 0xC9767F76 +#define ONE_BUTTN_ALT 0x0 +#define TWO_BUTTON 0XC8155AB1 +#define TWO_BUTTN_ALT 0x0 +#define THREE_BUTTON 0xB6996DAE +#define THREE_BUTTN_ALT 0x0 +#define FOUR_BUTTON 0x969AE844 +#define FOUR_BUTTN_ALT 0x0 +#define FIVE_BUTTON 0x4AAFAC67 +#define FIVE_BUTTN_ALT 0x0 +#define SIX_BUTTON 0x9C2A936C +#define SIX_BUTTN_ALT 0x0 +#define SEVEN_BUTTON 0x833ED333 +#define SEVEN_BUTTN_ALT 0x0 +#define EIGHT_BUTTON 0x55F2B93 +#define EIGHT_BUTTN_ALT 0x0 +#define NINE_BUTTON 0xDE78B0D0 +#define NINE_BUTTN_ALT 0x0 +#define ENTER_BUTTON 0x3F23F43 // button at the bottom right of the number pad "enter" +#define ENTER_BUTTN_ALT 0x0 +#define BACK_BUTTON 0xBDE97C12 // button at the bottom left of the number pad "-" +#define BACK_BUTTN_ALT 0x0 + +#define FORWARD_BUTTON 0xF24119FE // forward/up direction button/arrow +#define FORWARD_BUTTN_ALT 0x0 // alternate forward/up direction button/arrow +#define BACKWARD_BUTTON 0xB489062B // backward/down direction button/arrow +#define BACKWARD_BUTTN_ALT 0x0 // alternate backward/down direction button/arrow +#define RIGHT_BUTTON 0xBC9DF06 // right direction button/arrow +#define RIGHT_BUTTN_ALT 0x0 // alternate right direction button/arrow +#define LEFT_BUTTON 0xC53794B4 // left direction button/arrow +#define LEFT_BUTTN_ALT 0x0 // alternate left direction button/arrow +#define STOP_BUTTON 0x75A956A7 // stop button in the middle of the direction keys +#define STOP_BUTTN_ALT 0x0 // alternate stop button in the middle of the direction keys + +// Optionally set these buttons only if you have available buttons and want to expand +// beyond the number and arrow keys for clarity. +// ** These MUST be left with "0x0" values if they are not used. ** +#define FORWARD_RIGHT 0x34498102 // on this remote using the "Exit" button near the Select button +#define FORWARD_RT_ALT 0x0 +#define FORWARD_LEFT 0x81A840E6 // on this remote using the "R" button near the Select button +#define FORWARD_LFT_ALT 0x0 +#define BACKWARD_RIGHT 0xF640360 // on this remote using the Red dot button near the Select button +#define BACKWARD_RT_ALT 0x0 +#define BACKWARD_LEFT 0xC332FABB // on this remote using the "Info" button near the Select button +#define BACKWARD_LFT_ALT 0x0 +#define RC_MODE 0x0 +#define RC_MOD_ALT 0x0 +#define FREE_PROGRAM_MODE 0x0 +#define FREE_PROG_MOD_ALT 0x0 +#define LINE_PROGRAM_MODE 0x0 +#define LINE_PROG_MOD_ALT 0x0 +#define TRIM_MODE 0x0 +#define TRIM_MOD_ALT 0x0 +#define PLAY_PROGRAM 0x0 +#define PLAY_PROG_ALT 0x0 +#define STOP_PROGRAM 0x0 +#define STOP_PROG_ALT 0x0 + +// Gripper buttons Gripper codes commented out to save program space if not used +#define GRIP_OPEN 0x0 //0x5815B090 // on this remote using the channel down/backward +#define GRIP_OPN_ALT 0x0 +#define GRIP_CLOSE 0x0 //0x165412B7 // on this remote using the channel up/forward +#define GRIP_CLOS_ALT 0x0 + +#define REPEAT_CODE 0xFFFFFFFF // this probably does not need any change diff --git a/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/GLC_SampleIRcodes.ino b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/GLC_SampleIRcodes.ino new file mode 100644 index 0000000..e5218cd --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/GLC_SampleIRcodes.ino @@ -0,0 +1,80 @@ +// See the tabs for some tested remotes with various setups + +// Below is the default code list without any codes entered. + +// IR commands... change these to match your buttons/keys +// ** If your remote button only outputs one code when it is pushed multiple times, +// set the XX_BUTTON value to that code with "0x" before it since it is a HEX value +// and leave the XX_BUTTN_ALT value as "0x0" (zero). +// If it toggles between two different codes, as you tap the button, +// set XX_BUTTON value to one code and +// set the XX_BUTTN_ALT value to the other code. + +// Basic required 17 remote buttons +#define ZERO_BUTTON 0x0 +#define ZERO_BUTTN_ALT 0x0 +#define ONE_BUTTON 0x0 +#define ONE_BUTTN_ALT 0x0 +#define TWO_BUTTON 0X0 +#define TWO_BUTTN_ALT 0x0 +#define THREE_BUTTON 0x0 +#define THREE_BUTTN_ALT 0x0 +#define FOUR_BUTTON 0x0 +#define FOUR_BUTTN_ALT 0x0 +#define FIVE_BUTTON 0x0 +#define FIVE_BUTTN_ALT 0x0 +#define SIX_BUTTON 0x0 +#define SIX_BUTTN_ALT 0x0 +#define SEVEN_BUTTON 0x0 +#define SEVEN_BUTTN_ALT 0x0 +#define EIGHT_BUTTON 0x0 +#define EIGHT_BUTTN_ALT 0x0 +#define NINE_BUTTON 0x0 +#define NINE_BUTTN_ALT 0x0 +#define ENTER_BUTTON 0x0 // button at the bottom right of the number pad +#define ENTER_BUTTN_ALT 0x0 +#define BACK_BUTTON 0x0 // button at the bottom left of the number pad +#define BACK_BUTTN_ALT 0x0 + +#define FORWARD_BUTTON 0x0 // forward/up direction button/arrow +#define FORWARD_BUTTN_ALT 0x0 // alternate forward/up direction button/arrow +#define BACKWARD_BUTTON 0x0 // backward/down direction button/arrow +#define BACKWARD_BUTTN_ALT 0x0 // alternate backward/down direction button/arrow +#define RIGHT_BUTTON 0x0 // right direction button/arrow +#define RIGHT_BUTTN_ALT 0x0 // alternate right direction button/arrow +#define LEFT_BUTTON 0x0 // left direction button/arrow +#define LEFT_BUTTN_ALT 0x0 // alternate left direction button/arrow +#define STOP_BUTTON 0x0 // stop button in the middle of the direction keys +#define STOP_BUTTN_ALT 0x0 // alternate stop button in the middle of the direction keys + +// Optionally set these buttons only if you have available buttons and want to expand +// beyond the number and arrow keys for clarity. +// ** These MUST be left with "0x0" values if they are not used. ** +#define FORWARD_RIGHT 0x0 +#define FORWARD_RT_ALT 0x0 +#define FORWARD_LEFT 0x0 +#define FORWARD_LFT_ALT 0x0 +#define BACKWARD_RIGHT 0x0 +#define BACKWARD_RT_ALT 0x0 +#define BACKWARD_LEFT 0x0 +#define BACKWARD_LFT_ALT 0x0 +#define RC_MODE 0x0 +#define RC_MOD_ALT 0x0 +#define FREE_PROGRAM_MODE 0x0 +#define FREE_PROG_MOD_ALT 0x0 +#define LINE_PROGRAM_MODE 0x0 +#define LINE_PROG_MOD_ALT 0x0 +#define TRIM_MODE 0x0 +#define TRIM_MOD_ALT 0x0 +#define PLAY_PROGRAM 0x0 +#define PLAY_PROG_ALT 0x0 +#define STOP_PROGRAM 0x0 +#define STOP_PROG_ALT 0x0 + +// Gripper buttons +#define GRIP_OPEN 0x0 +#define GRIP_OPN_ALT 0x0 +#define GRIP_CLOSE 0x0 +#define GRIP_CLOS_ALT 0x0 + +#define REPEAT_CODE 0xFFFFFFFF // this probably does not need any change diff --git a/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/MTA.h b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/MTA.h new file mode 100644 index 0000000..dce25e6 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/MTA.h @@ -0,0 +1,79 @@ +// MTA branded remote from the parts bin, for reference where an ALT code was needed + +// IR commands... change these to match your buttons/keys +// ** If your remote button only outputs one code when it is pushed multiple times, +// set the XX_BUTTON value to that code with "0x" before it since it is a HEX value +// and leave the XX_BUTTN_ALT value as "0x0" (zero). +// If it toggles between two different codes, as you tap the button, +// set XX_BUTTON value to one code and +// set the XX_BUTTN_ALT value to the other code. + +// Basic required 17 remote buttons +#define ZERO_BUTTON 0x11544827 +#define ZERO_BUTTN_ALT 0xF1574245 +#define ONE_BUTTON 0xA871F8E6 +#define ONE_BUTTN_ALT 0x74D29C44 +#define TWO_BUTTON 0X2781070B +#define TWO_BUTTN_ALT 0xC5D55143 +#define THREE_BUTTON 0xA12C0CAF +#define THREE_BUTTN_ALT 0xC501BB0D +#define FOUR_BUTTON 0xB053820C +#define FOUR_BUTTN_ALT 0xABE11C8D +#define FIVE_BUTTON 0x7D37A47A +#define FIVE_BUTTN_ALT 0x4AFA16F2 +#define SIX_BUTTON 0x9564D619 +#define SIX_BUTTN_ALT 0xDF897749 +#define SEVEN_BUTTON 0xD1D48F0F +#define SEVEN_BUTTN_ALT 0x5D302205 +#define EIGHT_BUTTON 0xFD7941F0 +#define EIGHT_BUTTN_ALT 0x52DB1BCB +#define NINE_BUTTON 0x19D49086 +#define NINE_BUTTN_ALT 0xD3F2027C +#define ENTER_BUTTON 0x7E8AEB41 // button at the bottom right of the number pad +#define ENTER_BUTTN_ALT 0x5BACB2CB +#define BACK_BUTTON 0x93735FEE // button at the bottom left of the number pad +#define BACK_BUTTN_ALT 0x18CC29C1 + +#define FORWARD_BUTTON 0x8EAEB9BE // forward/up direction button/arrow +#define FORWARD_BUTTN_ALT 0x23A557EA // alternate forward/up direction button/arrow +#define BACKWARD_BUTTON 0x501D512A // backward/down direction button/arrow +#define BACKWARD_BUTTN_ALT 0x39C5E4A4 // alternate backward/down direction button/arrow +#define RIGHT_BUTTON 0xE666C92B // right direction button/arrow +#define RIGHT_BUTTN_ALT 0x9BE817D8 // alternate right direction button/arrow +#define LEFT_BUTTON 0xCA0A713F // left direction button/arrow +#define LEFT_BUTTN_ALT 0x99B63213 // alternate left direction button/arrow +#define STOP_BUTTON 0x8F6018A1 // stop button in the middle of the direction keys +#define STOP_BUTTN_ALT 0xF3B5079F // alternate stop button in the middle of the direction keys + +// Optionally set these buttons only if you have available buttons and want to expand +// beyond the number and arrow keys for clarity. +// ** These MUST be left with "0x0" values if they are not used. ** +#define FORWARD_RIGHT 0x0 +#define FORWARD_RT_ALT 0x0 +#define FORWARD_LEFT 0x0 +#define FORWARD_LFT_ALT 0x0 +#define BACKWARD_RIGHT 0x0 +#define BACKWARD_RT_ALT 0x0 +#define BACKWARD_LEFT 0x0 +#define BACKWARD_LFT_ALT 0x0 +#define RC_MODE 0x0 +#define RC_MOD_ALT 0x0 +#define FREE_PROGRAM_MODE 0x0 +#define FREE_PROG_MOD_ALT 0x0 +#define LINE_PROGRAM_MODE 0x0 +#define LINE_PROG_MOD_ALT 0x0 +#define TRIM_MODE 0x0 +#define TRIM_MOD_ALT 0x0 +#define PLAY_PROGRAM 0x0 +#define PLAY_PROG_ALT 0x0 +#define STOP_PROGRAM 0x0 +#define STOP_PROG_ALT 0x0 + +// Gripper buttons +#define GRIP_OPEN 0xB7314D70 +#define GRIP_OPN_ALT 0x64F85AA8 +#define GRIP_CLOSE 0xEC74B5A2 +#define GRIP_CLOS_ALT 0x744B96E7 + +#define REPEAT_CODE 0xFFFFFFFF // this probably does not need any change + diff --git a/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/Sanyo_FXML.h b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/Sanyo_FXML.h new file mode 100644 index 0000000..a26a705 --- /dev/null +++ b/examples/ScoutBotics/Level_2/GLC_SampleIRcodes/Sanyo_FXML.h @@ -0,0 +1,79 @@ +// Sanyo Remote Model FXML + +// IR commands... change these to match your buttons/keys +// ** If your remote button only outputs one code when it is pushed multiple times, +// set the XX_BUTTON value to that code with "0x" before it since it is a HEX value +// and leave the XX_BUTTN_ALT value as "0x0" (zero). +// If it toggles between two different codes, as you tap the button, +// set XX_BUTTON value to one code and +// set the XX_BUTTN_ALT value to the other code. + +// Basic required 17 remote buttons +#define ZERO_BUTTON 0x1CE300FF +#define ZERO_BUTTN_ALT 0x0 +#define ONE_BUTTON 0x1CE3807F +#define ONE_BUTTN_ALT 0x0 +#define TWO_BUTTON 0X1CE340BF +#define TWO_BUTTN_ALT 0x0 +#define THREE_BUTTON 0x1CE3C03F +#define THREE_BUTTN_ALT 0x0 +#define FOUR_BUTTON 0x1CE320DF +#define FOUR_BUTTN_ALT 0x0 +#define FIVE_BUTTON 0x1CE3A05F +#define FIVE_BUTTN_ALT 0x0 +#define SIX_BUTTON 0x1CE3609F +#define SIX_BUTTN_ALT 0x0 +#define SEVEN_BUTTON 0x1CE3E01F +#define SEVEN_BUTTN_ALT 0x0 +#define EIGHT_BUTTON 0x1CE310EF +#define EIGHT_BUTTN_ALT 0x0 +#define NINE_BUTTON 0x1CE3906F +#define NINE_BUTTN_ALT 0x0 +#define ENTER_BUTTON 0x1CE3B04F // button at the bottom right of the number pad +#define ENTER_BUTTN_ALT 0x0 +#define BACK_BUTTON 0x1CE3E817 // button at the bottom left of the number pad +#define BACK_BUTTN_ALT 0x0 + +#define FORWARD_BUTTON 0x1CE350AF // forward/up direction button/arrow +#define FORWARD_BUTTN_ALT 0x0 // alternate forward/up direction button/arrow +#define BACKWARD_BUTTON 0x1CE3D02F // backward/down direction button/arrow +#define BACKWARD_BUTTN_ALT 0x0 // alternate backward/down direction button/arrow +#define RIGHT_BUTTON 0x1CE3708F // right direction button/arrow +#define RIGHT_BUTTN_ALT 0x0 // alternate right direction button/arrow +#define LEFT_BUTTON 0x1CE3F00F // left direction button/arrow +#define LEFT_BUTTN_ALT 0x0 // alternate left direction button/arrow +#define STOP_BUTTON 0x1CE348B7 // stop button in the middle of the direction keys +#define STOP_BUTTN_ALT 0x0 // alternate stop button in the middle of the direction keys + +// Optionally set these buttons only if you have available buttons and want to expand +// beyond the number and arrow keys for clarity. +// ** These MUST be left with "0x0" values if they are not used. ** +#define FORWARD_RIGHT 0x0 +#define FORWARD_RT_ALT 0x0 +#define FORWARD_LEFT 0x0 +#define FORWARD_LFT_ALT 0x0 +#define BACKWARD_RIGHT 0x0 +#define BACKWARD_RT_ALT 0x0 +#define BACKWARD_LEFT 0x0 +#define BACKWARD_LFT_ALT 0x0 +#define RC_MODE 0x0 +#define RC_MOD_ALT 0x0 +#define FREE_PROGRAM_MODE 0x0 +#define FREE_PROG_MOD_ALT 0x0 +#define LINE_PROGRAM_MODE 0x0 +#define LINE_PROG_MOD_ALT 0x0 +#define TRIM_MODE 0x0 +#define TRIM_MOD_ALT 0x0 +#define PLAY_PROGRAM 0x0 +#define PLAY_PROG_ALT 0x0 +#define STOP_PROGRAM 0x0 +#define STOP_PROG_ALT 0x0 + +// Gripper buttons +#define GRIP_OPEN 0x1CE332CD +#define GRIP_OPN_ALT 0x0 +#define GRIP_CLOSE 0x1CE318E7 +#define GRIP_CLOS_ALT 0x0 + +#define REPEAT_CODE 0xFFFFFFFF // this probably does not need any change + diff --git a/examples/Xtra/RepeatAfterMe/RepeatAfterMe.ino b/examples/Xtra/RepeatAfterMe/RepeatAfterMe.ino new file mode 100644 index 0000000..ec9812a --- /dev/null +++ b/examples/Xtra/RepeatAfterMe/RepeatAfterMe.ino @@ -0,0 +1,118 @@ +/* 04/19/2018 + + This program will use the Gobit robot as a repeater of knocks it senses. + Whatever beat you knock on the robot's surface that has the piezo, it will play back in beeps. + + The Gobbit robot only needs an arduino Uno, a piezo beeper (passive), a 1 MegaOhm resistor, + and maybe a couple of jumpers for this project. + + Wire the resistor between the leads of the piezo. + Attach the black wire of the piezo to ground. + Attachd the red wire of the piezo to an analog pin. + Screw or double stick tape the piezo to the surface you will be knocking on. + Update the define values to match your setup and tunings. +*/ + + +#define PIEZO_PIN A2 // piezo beeper/sensor pin + +#define MAXKNOCKS 50 // maximum number of knocks that will be recorded at a time + +#define MAXGAP 2000 // maximum number of milliseconds to wait for a following knock before playback starts + +#define BEEP_LENGTH 40// duration in milliseconds of the beep sound + +#define KNOCK_LEVEL 40// analog read value threshold to detect a knock + +unsigned long previousMillis = 0; +unsigned long timeGaps[MAXKNOCKS]; +unsigned long currentMillis = millis(); + +void setup() { + + Serial.begin(9600); + + // set the beeper as an output + pinMode(PIEZO_PIN, OUTPUT); + + cheapBeep(BEEP_LENGTH); + delay(100); + cheapBeep(BEEP_LENGTH); + delay(100); + cheapBeep(BEEP_LENGTH); + + // a little delay after beeping before making the sensor an input + delay(200); + + pinMode(PIEZO_PIN, INPUT); +} + +void loop() { + + if (analogRead(PIEZO_PIN) > KNOCK_LEVEL) { + + byte keepRecording = 1; // flag to indicate if it is time to playback + int i = 0; // counter + currentMillis = millis(); + previousMillis = currentMillis; + + while (keepRecording && i < MAXKNOCKS ) { + + if (currentMillis - previousMillis > MAXGAP) { + // exit and replay the recording + keepRecording = 0; + } + + else if (analogRead(PIEZO_PIN) > KNOCK_LEVEL || i == 0) { + // get the duration since last knock + if (i > 0) { + timeGaps[i - 1] = currentMillis - previousMillis; + previousMillis = currentMillis; + } + + i++; + delay(90); // debounce + } + + // get the current time + currentMillis = millis(); + } + + pinMode(PIEZO_PIN, OUTPUT); + + Serial.print(i); + Serial.println(" knocks heard"); + + for (int k = 0; k < i; k++) { + Serial.print("Repeat beep "); + Serial.println(k + 1); + + cheapBeep(BEEP_LENGTH); + + if (k < i - 1) { + long int gap; + if (timeGaps[k] < BEEP_LENGTH) gap = timeGaps[k]; + else gap = timeGaps[k] - BEEP_LENGTH; + delay(gap); + } + + } + + // a little delay after beeping before back to input + delay(200); + pinMode(PIEZO_PIN, INPUT); + + } +} + +// beeper function that does not use any additional timers +// cheap since it does not cost too much on resources, but it is super limited +void cheapBeep(int beepTime) { + // with 500 microseconds up and down, = 1000 microsends = 1 millisecond per pulse = 1000 hertz + for (int i = 0; i < beepTime; i++) { + digitalWrite(PIEZO_PIN, HIGH); + delayMicroseconds(500); + digitalWrite(PIEZO_PIN, LOW); + delayMicroseconds(500); + } +} diff --git a/extras/Change.log b/extras/Change.log index c6d99f0..425f475 100644 --- a/extras/Change.log +++ b/extras/Change.log @@ -1,6 +1,24 @@ change log -0.0.9 +0.0.95 + + Added ScoutBotics Examples for both Level 1 and Level 2 with IRcontrol. + + Updated RaceTrackRobots example to use same default wiring as IRcontrol. + + Set PROCESS_TIME to 0 to disable until proper timing default and PIDs can be tested. + +0.0.93 + + Added PROCESS_TIME define to config.h and a mSampleTime time check in the PID loop to have a more consistent control loop. + +0.0.92 + + Added local copies of QTRSensor library and Adafruit Motor Shield library to eliminate need to load them separate. + This is a temporary solution. + Future solutions will require significant rewrite and probably use Virtual Functions. + +0.0.90 Added Ardumoto v14 and v20 and AdafruitMS default pin values in their .h files diff --git a/extras/responsa/SBL2_2018.txt b/extras/responsa/SBL2_2018.txt new file mode 100644 index 0000000..4f6aabb --- /dev/null +++ b/extras/responsa/SBL2_2018.txt @@ -0,0 +1,156 @@ +// ScoutBotics 10/27/2018 Level 2 competition + +/* + These are line following and turning solutions for the + tiles called from modeFive() + + Fill in the appropriate steps in each function to solve + the tiles in your competition. +*/ + + +void Tile1() +{ + if (MyBot.foundLeft) { + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + } else { + MyBot.turn('R'); + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + } +} + +void Tile2() +{ + if (MyBot.foundRight) { + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + MyBot.followLine(3); + } else { + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + } +} + +void Tile3() +{ + if (MyBot.foundLeft) { + MyBot.followLine(3); + MyBot.followLine(3); + if (MyBot.foundEnd) { + MyBot.turn('L'); // Turn around + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + } + } else { + MyBot.followLine(3); + MyBot.followLine(3); + if (MyBot.foundEnd) { + MyBot.turn('L'); // Turn around + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + } + } +} + +void Tile4() +{ + if (MyBot.foundLeft) { + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.followLine(3); + } else { + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + } +} + +void Tile5() +{ + if (MyBot.foundLeft) { + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.followLine(3); + } else { + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + MyBot.followLine(3); + } +} + +void Tile6() +{ + if (MyBot.foundLeft) { + if (MyBot.foundRight) { + MyBot.turn('L'); + MyBot.followLine(3); + } else { + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.followLine(3); + } + if (MyBot.foundEnd) { + MyBot.turn('L'); // Turn around + MyBot.followLine(3); + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + } + } else { + if (MyBot.foundForward) { + MyBot.turn('R'); + MyBot.followLine(3); + if (MyBot.foundEnd) { + MyBot.turn('L'); // Turn around + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + MyBot.turn('R'); + MyBot.followLine(3); + } + } else { + MyBot.turn('R'); + MyBot.followLine(3); + MyBot.followLine(3); + if (MyBot.foundEnd) { + MyBot.turn('L'); // Turn around + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + MyBot.turn('L'); + MyBot.followLine(3); + } + } + } +} + + + diff --git a/library.properties b/library.properties index e189a06..3d4ed1a 100644 --- a/library.properties +++ b/library.properties @@ -1,9 +1,9 @@ name=GobbitLineCommand -version=0.0.92 +version=0.0.95 author=Jason Talley maintainer=Jason Talley sentence=Basic to advanced line following, intersection detection, basic motor control, battery monitoring, gripper control, and basic collision detection with the Gobbit robot. -paragraph=The built in functions range from simple out of the box single command line following and gripper actuation to deep fine tuning of PID motor control functions which include battery monitoring for variable manipulation, pin selections for custom L298 or similar style motor drivers, sonar range sensor and collision control, presets for the Sparkfun Ardumoto motor driver, and presets for the Adafruit v 2.3 motor shields. For line following, the Pololu QTR-8RC infrared line sensor is required. The Adafruit Motor Shield V2 and the QTRSensors libraries are required. This version has temp local copies of QTRSensors and Adafruit Motorshield v2.3. +paragraph=The built in functions range from simple out of the box single command line following and gripper actuation to deep fine tuning of PID motor control functions which include battery monitoring for variable manipulation, pin selections for custom L298 or similar style motor drivers, sonar range sensor and collision control, presets for the Sparkfun Ardumoto motor driver, and presets for the Adafruit v 2.3 motor shields. For line following, the Pololu QTR-8RC infrared line sensor is required. The Adafruit Motor Shield V2 and the QTRSensors libraries are required. This version has temp local copies of QTRSensors and Adafruit Motorshield v2.3. ScoutBotics Examples added. category=Device Control url=https://github.com/MadTooler/Gobbit_Line_Commander architectures=* diff --git a/src/GobbitLineCommand.cpp b/src/GobbitLineCommand.cpp index 56983ff..4707387 100644 --- a/src/GobbitLineCommand.cpp +++ b/src/GobbitLineCommand.cpp @@ -2,7 +2,7 @@ * GobbitLineCommand.h * Library for line following, intersection detection, and basic motor control of Gobbit robot. * Created by Jason Talley -* Last edit 10/20/2018 +* Last edit 10/25/2018 * Released under GNU agreement */ @@ -56,7 +56,7 @@ #endif // load Adafruit Motor Shield library and initialize objects -// NOTE cannot access USE_AFMS or status of ADAFRUIT_MS from main sketch for conditinal loading of AFMS library. +// NOTE cannot access USE_AFMS or status of ADAFRUIT_MS from main sketch for conditional loading of AFMS library. // This is a limitation of the arduino IDE compiler, and it does not appear to be adopted in the future. //#include #include "Adafruit_Motor_Shield_V2_Library/Adafruit_MotorShield.h" //**** use a local copy of Adafruit Library for temporary easier install @@ -512,7 +512,7 @@ void GobbitLineCommand::drive(char turnDir) // // If setSonar has been called and a valid pin was set for the sensor, followLine also reads the sonar, // updates the distanceInch variable, and controls/slows down the motor speed per the range of safe distance -// everytime it is called +// every time it is called void GobbitLineCommand::followLine(byte followMode) { @@ -536,13 +536,13 @@ void GobbitLineCommand::followLine(byte followMode) runOnce = 1; break; - // follow for one motor adjustment and check if there was an intersction found then return + // follow for one motor adjustment and check if there was an intersection found then return case 2: findIntersection = 1; runOnce = 1; break; - // follow until an intersction is found then return + // follow until an intersection is found then return case 3: findIntersection = 1; runOnce = 0; @@ -568,7 +568,7 @@ void GobbitLineCommand::followLine(byte followMode) // adjusted that had been leading to an exaggerated "twirk" when crossing the intersections. // "Twirk" is a result of fast reaction of bot to early or uneven sensor reads from seeing // part of an intersection before other parts since it would only not "twirk" if the intersection - // was perfecting entered with all sensors reading simulataneously, which could almost never + // was perfecting entered with all sensors reading simultaneously, which could almost never // be the case since they are read in series. // tried this array before with a larger buffer, but it did not seem to work well. @@ -585,7 +585,7 @@ void GobbitLineCommand::followLine(byte followMode) offLine = 0; - // read calibrated sensor values and obtain a measure of the currnet line position from 0 to 7000. + // read calibrated sensor values and obtain a measure of the current line position from 0 to 7000. linePosition = qtrrc.readLine(sensorValues); // simple line following portion for way off line adjustment @@ -643,10 +643,13 @@ void GobbitLineCommand::followLine(byte followMode) kdCurrent = _kd; } - // **** added time check here + +#if PROCESS_TIME + // added time check here to run only if PROCESS_TIME IS greater than 0 unsigned long mCurrentTime = millis(); unsigned long mCurrentDuration = (mCurrentTime - mLastTime); if(mCurrentDuration>=mSampleTime){ +#endif // update iAccumError accumulated error iAccumError = iAccumError+error; @@ -687,11 +690,16 @@ void GobbitLineCommand::followLine(byte followMode) //set motor speeds setMotors(LmotorSpeed, RmotorSpeed); - + +#if PROCESS_TIME mLastTime = mCurrentTime; + } else beepCycle(); // run beeper cycle +#endif + } + // Exit followLine if it is in a single adjustment mode if(runOnce) @@ -1185,7 +1193,7 @@ void GobbitLineCommand::backup(int speed, int delayTime) //----------------- // Sets the battery voltage which is used for choosing proper pd and motor tunings. // This function does not read from any pins. -// 0 will force default of 9volt setttings +// 0 will force default of 9volt settings void GobbitLineCommand::setBatteryVolts(float unreadVolts) { @@ -1830,7 +1838,7 @@ void GobbitLineCommand::brakeMotors(int bStrength,char direction) // devices other than beepers could be controlled by this function. void GobbitLineCommand::setBeeperPin(int pin) { - // set pin mode for peizo style beeper + // set pin mode for piezo style beeper pinMode(pin, OUTPUT); // only used as flag and feedback for serialPrintCurrentSettings diff --git a/src/GobbitLineCommand.h b/src/GobbitLineCommand.h index 7fb058f..b8151d4 100644 --- a/src/GobbitLineCommand.h +++ b/src/GobbitLineCommand.h @@ -2,7 +2,7 @@ * GobbitLineCommand.h * Library for line following, intersection detection, and basic motor control of Gobbit robot. * Created by Jason Talley -* Last edit 10/20/2018 +* Last edit 10/25/2018 * Released under GNU agreement */ @@ -88,7 +88,7 @@ class GobbitLineCommand void setQTRpins(unsigned char pin1, unsigned char pin2, unsigned char pin3, unsigned char pin4, unsigned char pin5, unsigned char pin6, unsigned char pin7, unsigned char pin8); // use to set qtr sensor pins if default Gobbit wiring will not be used void setRightMotorPinsDirPWM(int dirPin, int pwmPin); // sets the Right Motor driver pins for simple direction and PWM style drivers, such as L298 type. void setLeftMotorPinsDirPWM(int dirPin, int pwmPin); // sets the Left Motor driver pins for simple direction and PWM style drivers, such as L298 type. - void setSonar(int analogPin, float range); // sets the Sonar/obstacle avoidance pin# (-1 disables), safe distance/range to maintain for obstacle avaoidance (8 is a good start with no gripper while 1000 is so large it essentially disables any speed adjustments in follow mode) + void setSonar(int analogPin, float range); // sets the Sonar/obstacle avoidance pin# (-1 disables), safe distance/range to maintain for obstacle avoidance (8 is a good start with no gripper while 1000 is so large it essentially disables any speed adjustments in follow mode) //void setGripPinOpenClosed(int pin, int open, int closed); // sets the gripper servo pin#, degree of open position, degree of closed position. void setPID(float kp, float ki, float kd); // sets the fine/basic/small kp, ki, and kd values void setPIDcoarse(float kpC, float kiC, float kdC); // sets the coarse/fast/aggressive kp, ki, and kd values @@ -112,7 +112,7 @@ class GobbitLineCommand void move(float moveSpeed, float moveTurn); // simple moves without any line following. Typically used with delay statements as sensorless control. void setMotors(float leftVelocity, float rightVelocity); // direct motor control. Must have run beginGobbit first. void brakeMotors(void); // Brake motors without any arguments, Auto choice of strength and direction by a quick reversal of motors to stop motion. - void brakeMotors(int bStrength,char direction); // Brake motors expanded function by a quick reversal of motors to stop motion in the declared direction. Strength is a percentage of the BRAKING_TIME milliseconds. 0% to 200%, directin is 'F'orward, 'B'ackward, 'R'ight, 'L'eft, or 'A'uto and intended as the opposite of the current direction of motion. + void brakeMotors(int bStrength,char direction); // Brake motors expanded function by a quick reversal of motors to stop motion in the declared direction. Strength is a percentage of the BRAKING_TIME milliseconds. 0% to 200%, direction is 'F'orward, 'B'ackward, 'R'ight, 'L'eft, or 'A'uto and intended as the opposite of the current direction of motion. void backup(int speed, int delayTime); // backup with declared speed (100 max) and for a period of milliseconds //void gripClose(void); // closes the gripper to the declared closed angle //void gripOpen(void); // opens the gripper to the declared open angle @@ -170,7 +170,7 @@ class GobbitLineCommand unsigned int linePosition = 0; // value from 0-7000 to indicate position of line between sensor 0 - 7 unsigned int pastLinePosition = 0; // value from 0-7000 to indicate position of line between sensor 0 - 7, used for Operation Flux Capacitor // **** moved pin defines to #ifdef's for motor driver options - //unsigned char sensorPins[8]={2, 4, 5, 6, 7, 8, 9, 10}; // defualt values for Gobbit wiring + //unsigned char sensorPins[8]={2, 4, 5, 6, 7, 8, 9, 10}; // default values for Gobbit wiring #if SERVO_ENABLE // gripper angle limit default values @@ -208,7 +208,7 @@ class GobbitLineCommand int pwm_b = AMV20_PWMB; //PWM control for Ardumoto outputs B3 and B4 is on digital pin 11 (Right motor) #else - unsigned char sensorPins[8]={2, 4, 5, 6, 7, 8, 9, 10}; // defualt values for Gobbit wiring + unsigned char sensorPins[8]={2, 4, 5, 6, 7, 8, 9, 10}; // default values for Gobbit wiring // for default if nothing was declared, use original ArduMoto motor driver vars // dir_a/b sets direction. LOW is Forward, HIGH is Reverse // pwm_a/b sets speed. Value range is 0-255. For example, if you set the speed at 100 then 100/255 = 39% duty cycle = slow diff --git a/src/config.h b/src/config.h index 2d59308..a9e3c41 100644 --- a/src/config.h +++ b/src/config.h @@ -2,7 +2,7 @@ * GobbitLineCommand.h - library variables for the Gobbit robot * Library for line following, intersection detection, and basic motor control of Gobbit robot. * Created by Jason Talley -* Last edit 10/20/2018 +* Last edit 10/25/2018 * Released under GNU agreement */ @@ -11,11 +11,11 @@ // time in milliseconds between processing of PID loop // set to 0 to revert back to initial version where processor cycle time was the delay - #define PROCESS_TIME 10 + #define PROCESS_TIME 0 // line sensor defines/constants #define NUM_SENSORS 8 // number of sensors used - #define TIME_OUT 2500 // waits for 2500 microseconds for sensor outputs to go low (2.5 milleseconds) + #define TIME_OUT 2500 // waits for 2500 microseconds for sensor outputs to go low (2.5 milliseconds) #define EMITTER_PIN QTR_NO_EMITTER_PIN // emitter control pin not used. If added, replace QTR_NO_EMITTER_PIN with pin# // Cutoff time in milliseconds to stop sensing for intersection if the end of the intersection has not been found. @@ -24,7 +24,7 @@ #define FIND_INTERSECTION_MAXTIME 1000 // Time in milliseconds to evaluate if a marker on the course was found. - // The marker needs to be a large black area, if using electrical tape, about 3 strips next to eachother by wider than + // The marker needs to be a large black area, if using electrical tape, about 3 strips next to each other by wider than // the sensor (total about 2" x 4-1/4"). // If the robot is in lineFollow mode for detecting intersections, or in drive mode, and it runs over a mark for // period of time greater than this value, and passes back onto white background before the FIND_INTERSECTION_MAXTIME, then it