Priyata Kaneria
Code translation of the ZumoShield library from Arduino to Mbed. This library is performed by the ZumoShield robot on the Arduino Uno R3 and FRDM-K64F boards respectively. All supporting translated Mbed code can be found in the ZumoShield folder below. See Wiki page "Dissertation" below for undergraduate dissertation regarding this topic. The report includes an investigation on the process of porting software between these two platforms as well as the microcontroller board performance comparison.
- Robot Control Interface: From Arduino to ARM Mbed
See https://github.com/priyatakaneria/Robot-Control-Interface-From-Arduino-to-ARM-Mbed/blob/main/Dissertation.pdf for undergraduate dissertation regarding this topic. The report includes an exploration on the process of porting software between these two platforms as well as the microcontroller board performance comparison investigation. All supporting translated Mbed code can be found in the ZumoShield folder below.
Revision 0:711f2d599e34, committed 2020-10-09
- Comitter:
- pnlk20
- Date:
- Fri Oct 09 14:45:19 2020 +0000
- Commit message:
- Initial commit
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/BorderDetect.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,109 @@
+#include "PololuBuzzer.h"
+#include "Pushbutton.h"
+#include "ZumoMotors.h"
+#include "ZumoReflectanceSensorArray.h"
+#include "millis.h"
+#include "mbed.h"
+#include<string>
+
+I2C i2c(I2C_SDA, I2C_SCL);
+#define LED PTD1
+
+// this might need to be tuned for different lighting conditions, surfaces, etc.
+#define QTR_THRESHOLD 1500 // microseconds 1500
+
+// these might need to be tuned for different motor types
+#define REVERSE_SPEED 0.5 // 0 is stopped, 400 is full speed
+#define TURN_SPEED 0.5
+#define FORWARD_SPEED 0.5
+#define REVERSE_DURATION 200000 // microseconds
+#define TURN_DURATION 300000 // microseconds
+
+PololuBuzzer buzzer(PTA1);
+ZumoMotors motors(PTD0, PTC12, PTC4, PTC3);
+Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12
+// static BufferedSerial pc(USBTX, USBRX);
+
+#define NUM_SENSORS 6
+unsigned int sensorValues[NUM_SENSORS];
+
+ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN);
+DigitalOut ledPin(LED, 1);
+// Timer t;
+
+void waitForButtonAndCountDown()
+{
+ ledPin.write(1);
+ button.waitForButton();
+ ledPin.write(0);
+
+ BuzzerValues buzzing;
+
+ // play audible countdown
+ for (int i = 0; i < 3; i++)
+ {
+ wait_us(1000000);
+ buzzing = buzzer.playNote(NOTE_G(3), 200, 15);
+ buzzer.beep(buzzing.returnFrequency(), buzzing.returnDuration(), buzzing.returnVolume());
+
+ }
+ wait_us(1000000);
+ buzzing = buzzer.playNote(NOTE_G(4), 500, 15);
+ buzzer.beep(buzzing.returnFrequency(), buzzing.returnDuration(), buzzing.returnVolume());
+ wait_us(1000000);
+}
+
+void setup()
+{
+ // uncomment if necessary to correct motor directions
+ //motors.flipLeftMotor(true);
+ //motors.flipRightMotor(true);
+// wait_us(500000); //?? we dont need this it's only for callibration
+
+ ledPin.write(1);
+ waitForButtonAndCountDown();
+}
+
+void loop()
+{
+ if (button.isPressed())
+ {
+ // if button is pressed, stop and wait for another press to go again
+ motors.setSpeeds(0, 0);
+ button.waitForRelease();
+ waitForButtonAndCountDown();
+ }
+
+ sensors.read(sensorValues);
+
+ if (sensorValues[0] < QTR_THRESHOLD)
+ {
+ // if leftmost sensor detects line, reverse and turn to the right
+ motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
+ wait_us(REVERSE_DURATION);
+ motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
+ wait_us(TURN_DURATION);
+ motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
+ }
+ else if (sensorValues[5] < QTR_THRESHOLD)
+ {
+ // if rightmost sensor detects line, reverse and turn to the left
+ motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
+ wait_us(REVERSE_DURATION);
+ motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
+ wait_us(TURN_DURATION);
+ motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
+ }
+ else
+ {
+ // otherwise, go straight
+ motors.setSpeeds(FORWARD_SPEED, FORWARD_SPEED);
+ }
+}
+
+int main(){
+ setup();
+ while(1){
+ loop();
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/PololuBuzzer.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,683 @@
+#include "PololuBuzzer.h"
+#include "mbed.h"
+
+using namespace mbed;
+ // constructor
+ /** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+
+BuzzerValues::BuzzerValues(float frequency, float duration, float volume) : freq(frequency), dur(duration), vol(volume) {}
+BuzzerValues::BuzzerValues() {}
+void BuzzerValues::changeValue(float frequency, float duration, float volume) {
+ freq = frequency;
+ dur = duration;
+ vol = volume;
+}
+float BuzzerValues::returnFrequency() {
+ return freq;
+}
+float BuzzerValues::returnDuration() {
+ return dur;
+}
+float BuzzerValues::returnVolume() {
+ return vol;
+}
+
+#define BUZZER_DDR DDRD
+#define BUZZER (1 << PORTD3)
+
+#define TIMER2_CLK_32 0x3 // 500 kHz
+
+static const unsigned int cs2_divider[] = {0, 1, 8, 32, 64, 128, 256, 1024};
+
+#define ENABLE_TIMER_INTERRUPT() TIMSK2 = (1 << TOIE2)
+#define DISABLE_TIMER_INTERRUPT() TIMSK2 = 0
+
+unsigned char buzzerInitialized = 0;
+volatile unsigned char buzzerFinished = 1; // flag: 0 while playing
+const char * volatile buzzerSequence = 0;
+
+// declaring these globals as static means they won't conflict
+// with globals in other .cpp files that share the same name
+static volatile unsigned int buzzerTimeout = 0; // tracks buzzer time limit
+static volatile char play_mode_setting = PLAY_AUTOMATIC;
+
+extern volatile unsigned char buzzerFinished; // flag: 0 while playing
+extern const char * volatile buzzerSequence;
+
+
+static volatile unsigned char use_program_space; // boolean: true if we should
+ // use program space
+
+// music settings and defaults
+static volatile unsigned char octave = 4; // the current octave
+static volatile unsigned int whole_note_duration = 2000; // the duration of a whole note
+static volatile unsigned int note_type = 4; // 4 for quarter, etc
+static volatile unsigned int duration = 500; // the duration of a note in ms
+static volatile unsigned int volume = 15; // the note volume
+static volatile unsigned char staccato = 0; // true if playing staccato
+
+// staccato handling
+static volatile unsigned char staccato_rest_duration; // duration of a staccato rest,
+ // or zero if it is time to play a note
+
+static void nextNote();
+
+PololuBuzzer::PololuBuzzer(PinName pin) : _pwm(pin), playing(0) {
+ _pwm.write(0.0); // after creating it have to be off
+}
+
+ /** stop the beep instantaneous
+ * usually not used
+ */
+void PololuBuzzer::nobeep() {
+ _pwm.write(0.0);
+ buzzerFinished = 1;
+ buzzerSequence = 0;
+ if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+ nextNote();
+ else
+ isPlaying2(0);
+}
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+
+bool PololuBuzzer::isPlaying2(int request){
+ if (request == 0) {
+ playing = false;
+ }
+ else if (request == 1){
+ playing = true;
+ }
+ return playing;
+}
+
+void PololuBuzzer::beep(float freq, float time, float volume) {
+ _pwm.period(1.0/freq);
+ _pwm.write(0.5); // 50% duty cycle - beep on
+ float newTime = (time/500);
+ toff.attach(callback(this, &PololuBuzzer::nobeep), newTime);
+}
+
+// Timer2 overflow interrupt
+// ISR (TIMER2_OVF_vect)
+// {
+// if (buzzerTimeout-- == 0)
+// {
+// DISABLE_TIMER_INTERRUPT();
+// sei(); // re-enable global interrupts (nextNote() is very slow)
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+// buzzerFinished = 1;
+// if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+// nextNote();
+// }
+// }
+
+
+// this is called by playFrequency()
+inline void PololuBuzzer::init()
+{
+ if (!buzzerInitialized)
+ {
+ buzzerInitialized = 1;
+ //init2();
+ }
+}
+
+// initializes timer4 (32U4) or timer2 (328P) for buzzer control
+// void PololuBuzzer::init2()
+// {
+// DISABLE_TIMER_INTERRUPT();
+
+// TCCR2A = 0x21; // bits 7 and 6 clear: normal port op., OC4A disconnected
+// // bit 5 set, 4 clear: clear OC2B on comp match when upcounting,
+// // set OC2B on comp match when downcounting
+// // bits 3 and 2: not used
+// // bit 1 clear, 0 set: combine with bit 3 of TCCR2B...
+
+// TCCR2B = 0x0B; // bit 7 clear: no force output compare for channel A
+// // bit 6 clear: no force output compare for channel B
+// // bits 5 and 4: not used
+// // bit 3 set: combine with bits 1 and 0 of TCCR2A to
+// // select waveform generation mode 5, phase-correct PWM,
+// // TOP = OCR2A, OCR2B set at TOP, TOV2 flag set at BOTTOM
+// // bit 2 clear, 1-0 set: timer clock = clkT2S/32
+
+ // This sets timer 2 to run in phase-correct PWM mode, where TOP = OCR2A,
+ // OCR2B is updated at TOP, TOV2 Flag is set on BOTTOM. OC2B is cleared
+ // on compare match when upcounting, set on compare match when downcounting;
+ // OC2A is disconnected.
+ // Note: if the PWM frequency and duty cycle are changed, the first
+ // cycle of the new frequency will be at the old duty cycle, since
+ // the duty cycle (OCR2B) is not updated until TOP.
+
+
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// BUZZER_DDR |= BUZZER; // buzzer pin set as an output
+// sei();
+// }
+
+
+// Set up the timer to play the desired frequency (in Hz or .1 Hz) for the
+// the desired duration (in ms). Allowed frequencies are 40 Hz to 10 kHz.
+// volume controls buzzer volume, with 15 being loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playFrequency(unsigned int freq, unsigned int dur,
+ unsigned char volume)
+{
+ init(); // initializes the buzzer if necessary
+ buzzerFinished = 0;
+
+ unsigned int timeout;
+ unsigned char multiplier = 1;
+
+ if (freq & DIV_BY_10) // if frequency's DIV_BY_10 bit is set
+ { // then the true frequency is freq/10
+ multiplier = 10; // (gives higher resolution for small freqs)
+ freq &= ~DIV_BY_10; // clear DIV_BY_10 bit
+ }
+
+ unsigned char min = 40 * multiplier;
+ if (freq < min) // min frequency allowed is 40 Hz
+ freq = min;
+ if (multiplier == 1 && freq > 10000)
+ freq = 10000; // max frequency allowed is 10kHz
+
+// unsigned int top;
+// unsigned char newCS2 = 2; // try prescaler divider of 8 first (minimum necessary for 10 kHz)
+// unsigned int divider = cs2_divider[newCS2];
+
+// // calculate necessary clock source and counter top value to get freq
+// top = (unsigned int)(((F_CPU/16 * multiplier) + (freq >> 1))/ freq);
+
+// while (top > 255)
+// {
+// divider = cs2_divider[++newCS2];
+// top = (unsigned int)(((F_CPU/2/divider * multiplier) + (freq >> 1))/ freq);
+// }
+
+ // set timeout (duration):
+ if (multiplier == 10)
+ freq = (freq + 5) / 10;
+
+ if (freq == 1000)
+ timeout = dur; // duration for silent notes is exact
+ else
+ timeout = (unsigned int)((long)dur * freq / 1000);
+
+ if (volume > 15)
+ volume = 15;
+
+ BuzzerValues buzz(freq,timeout,volume);
+ return buzz;
+
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts while writing to registers
+
+// TCCR2B = (TCCR2B & 0xF8) | newCS2; // select timer 2 clock prescaler
+// OCR2A = top; // set timer 2 pwm frequency
+// OCR2B = top >> (16 - volume); // set duty cycle (volume)
+// buzzerTimeout = timeout; // set buzzer duration
+
+// TIFR2 |= 0xFF; // clear any pending t2 overflow int.
+
+// ENABLE_TIMER_INTERRUPT();
+}
+
+
+
+// Determine the frequency for the specified note, then play that note
+// for the desired duration (in ms). This is done without using floats
+// and without having to loop. volume controls buzzer volume, with 15 being
+// loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playNote(unsigned char note, unsigned int dur,
+ unsigned char volume)
+{
+ // note = key + octave * 12, where 0 <= key < 12
+ // example: A4 = A + 4 * 12, where A = 9 (so A4 = 57)
+ // A note is converted to a frequency by the formula:
+ // Freq(n) = Freq(0) * a^n
+ // where
+ // Freq(0) is chosen as A4, which is 440 Hz
+ // and
+ // a = 2 ^ (1/12)
+ // n is the number of notes you are away from A4.
+ // One can see that the frequency will double every 12 notes.
+ // This function exploits this property by defining the frequencies of the
+ // 12 lowest notes allowed and then doubling the appropriate frequency
+ // the appropriate number of times to get the frequency for the specified
+ // note.
+
+ // if note = 16, freq = 41.2 Hz (E1 - lower limit as freq must be >40 Hz)
+ // if note = 57, freq = 440 Hz (A4 - central value of ET Scale)
+ // if note = 111, freq = 9.96 kHz (D#9 - upper limit, freq must be <10 kHz)
+ // if note = 255, freq = 1 kHz and buzzer is silent (silent note)
+
+ // The most significant bit of freq is the "divide by 10" bit. If set,
+ // the units for frequency are .1 Hz, not Hz, and freq must be divided
+ // by 10 to get the true frequency in Hz. This allows for an extra digit
+ // of resolution for low frequencies without the need for using floats.
+
+ unsigned int freq = 0;
+ unsigned char offset_note = note - 16;
+ BuzzerValues buzz;
+
+ if (note == SILENT_NOTE || volume == 0)
+ {
+ freq = 1000; // silent notes => use 1kHz freq (for cycle counter)
+ buzz.changeValue(freq,dur,volume);
+ // buzz = playFrequency(freq, dur, 0);
+ // return buzz;
+ }
+ else {
+ if (note <= 16)
+ offset_note = 0;
+ else if (offset_note > 95)
+ offset_note = 95;
+
+ unsigned char exponent = offset_note / 12;
+
+ // frequency table for the lowest 12 allowed notes
+ // frequencies are specified in tenths of a Hertz for added resolution
+ switch (offset_note - exponent * 12) // equivalent to (offset_note % 12)
+ {
+ case 0: // note E1 = 41.2 Hz
+ freq = 412;
+ break;
+ case 1: // note F1 = 43.7 Hz
+ freq = 437;
+ break;
+ case 2: // note F#1 = 46.3 Hz
+ freq = 463;
+ break;
+ case 3: // note G1 = 49.0 Hz
+ freq = 490;
+ break;
+ case 4: // note G#1 = 51.9 Hz
+ freq = 519;
+ break;
+ case 5: // note A1 = 55.0 Hz
+ freq = 550;
+ break;
+ case 6: // note A#1 = 58.3 Hz
+ freq = 583;
+ break;
+ case 7: // note B1 = 61.7 Hz
+ freq = 617;
+ break;
+ case 8: // note C2 = 65.4 Hz
+ freq = 654;
+ break;
+ case 9: // note C#2 = 69.3 Hz
+ freq = 693;
+ break;
+ case 10: // note D2 = 73.4 Hz
+ freq = 734;
+ break;
+ case 11: // note D#2 = 77.8 Hz
+ freq = 778;
+ break;
+ }
+ if (exponent < 7)
+ {
+ freq = freq << exponent; // frequency *= 2 ^ exponent
+ if (exponent > 1) // if the frequency is greater than 160 Hz
+ freq = (freq + 5) / 10; // we don't need the extra resolution
+ else
+ freq += DIV_BY_10; // else keep the added digit of resolution
+ }
+ else
+ freq = (freq * 64 + 2) / 5; // == freq * 2^7 / 10 without int overflow
+
+ if (volume > 15)
+ volume = 15;
+ buzz = playFrequency(freq, dur, volume); // set buzzer this freq/duration
+ }
+ return buzz;
+}
+
+
+
+// Returns 1 if the buzzer is currently playing, otherwise it returns 0
+unsigned char PololuBuzzer::isPlaying()
+{
+ return !buzzerFinished || buzzerSequence != 0;
+}
+
+
+// Plays the specified sequence of notes. If the play mode is
+// PLAY_AUTOMATIC, the sequence of notes will play with no further
+// action required by the user. If the play mode is PLAY_CHECK,
+// the user will need to call playCheck() in the main loop to initiate
+// the playing of each new note in the sequence. The play mode can
+// be changed while the sequence is playing.
+// This is modeled after the PLAY commands in GW-BASIC, with just a
+// few differences.
+//
+// The notes are specified by the characters C, D, E, F, G, A, and
+// B, and they are played by default as "quarter notes" with a
+// length of 500 ms. This corresponds to a tempo of 120
+// beats/min. Other durations can be specified by putting a number
+// immediately after the note. For example, C8 specifies C played as an
+// eighth note, with half the duration of a quarter note. The special
+// note R plays a rest (no sound).
+//
+// Various control characters alter the sound:
+// '>' plays the next note one octave higher
+//
+// '<' plays the next note one octave lower
+//
+// '+' or '#' after a note raises any note one half-step
+//
+// '-' after a note lowers any note one half-step
+//
+// '.' after a note "dots" it, increasing the length by
+// 50%. Each additional dot adds half as much as the
+// previous dot, so that "A.." is 1.75 times the length of
+// "A".
+//
+// 'O' followed by a number sets the octave (default: O4).
+//
+// 'T' followed by a number sets the tempo (default: T120).
+//
+// 'L' followed by a number sets the default note duration to
+// the type specified by the number: 4 for quarter notes, 8
+// for eighth notes, 16 for sixteenth notes, etc.
+// (default: L4)
+//
+// 'V' followed by a number from 0-15 sets the music volume.
+// (default: V15)
+//
+// 'MS' sets all subsequent notes to play staccato - each note is played
+// for 1/2 of its allotted time, followed by an equal period
+// of silence.
+//
+// 'ML' sets all subsequent notes to play legato - each note is played
+// for its full length. This is the default setting.
+//
+// '!' resets all persistent settings to their defaults.
+//
+// The following plays a c major scale up and back down:
+// play("L16 V8 cdefgab>cbagfedc");
+//
+// Here is an example from Bach:
+// play("T240 L8 a gafaeada c+adaeafa <aa<bac#ada c#adaeaf4");
+void PololuBuzzer::play(const char *notes)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes;
+ use_program_space = 0;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+void PololuBuzzer::playFromProgramSpace(const char *notes_p)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes_p;
+ use_program_space = 1;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+
+// // stop all sound playback immediately
+// void PololuBuzzer::stopPlaying()
+// {
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts
+
+
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// buzzerFinished = 1;
+// buzzerSequence = 0;
+// }
+
+// Gets the current character, converting to lower-case and skipping
+// spaces. For any spaces, this automatically increments sequence!
+static char currentCharacter()
+{
+ char c = 0;
+ do
+ {
+ // if(use_program_space)
+ // c = pgm_read_byte(buzzerSequence);
+ // else
+ c = *buzzerSequence;
+
+ if(c >= 'A' && c <= 'Z')
+ c += 'a'-'A';
+ } while(c == ' ' && (buzzerSequence ++));
+
+ return c;
+}
+
+// Returns the numerical argument specified at buzzerSequence[0] and
+// increments sequence to point to the character immediately after the
+// argument.
+static unsigned int getNumber()
+{
+ unsigned int arg = 0;
+
+ // read all digits, one at a time
+ char c = currentCharacter();
+ while(c >= '0' && c <= '9')
+ {
+ arg *= 10;
+ arg += c-'0';
+ buzzerSequence ++;
+ c = currentCharacter();
+ }
+
+ return arg;
+}
+
+static void nextNote()
+{
+ unsigned char note = 0;
+ unsigned char rest = 0;
+ unsigned char tmp_octave = octave; // the octave for this note
+ unsigned int tmp_duration; // the duration of this note
+ unsigned int dot_add;
+
+ char c; // temporary variable
+
+ // if we are playing staccato, after every note we play a rest
+ if(staccato && staccato_rest_duration)
+ {
+ PololuBuzzer::playNote(SILENT_NOTE, staccato_rest_duration, 0);
+ staccato_rest_duration = 0;
+ return;
+ }
+
+ parse_character:
+
+ // Get current character
+ c = currentCharacter();
+ buzzerSequence ++;
+
+ // Interpret the character.
+ switch(c)
+ {
+ case '>':
+ // shift the octave temporarily up
+ tmp_octave ++;
+ goto parse_character;
+ case '<':
+ // shift the octave temporarily down
+ tmp_octave --;
+ goto parse_character;
+ case 'a':
+ note = NOTE_A(0);
+ break;
+ case 'b':
+ note = NOTE_B(0);
+ break;
+ case 'c':
+ note = NOTE_C(0);
+ break;
+ case 'd':
+ note = NOTE_D(0);
+ break;
+ case 'e':
+ note = NOTE_E(0);
+ break;
+ case 'f':
+ note = NOTE_F(0);
+ break;
+ case 'g':
+ note = NOTE_G(0);
+ break;
+ case 'l':
+ // set the default note duration
+ note_type = getNumber();
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'm':
+ // set music staccato or legato
+ if(currentCharacter() == 'l')
+ staccato = false;
+ else
+ {
+ staccato = true;
+ staccato_rest_duration = 0;
+ }
+ buzzerSequence ++;
+ goto parse_character;
+ case 'o':
+ // set the octave permanently
+ octave = tmp_octave = getNumber();
+ goto parse_character;
+ case 'r':
+ // Rest - the note value doesn't matter.
+ rest = 1;
+ break;
+ case 't':
+ // set the tempo
+ whole_note_duration = 60*400/getNumber()*10;
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'v':
+ // set the volume
+ volume = getNumber();
+ goto parse_character;
+ case '!':
+ // reset to defaults
+ octave = 4;
+ whole_note_duration = 2000;
+ note_type = 4;
+ duration = 500;
+ volume = 15;
+ staccato = 0;
+ // reset temp variables that depend on the defaults
+ tmp_octave = octave;
+ tmp_duration = duration;
+ goto parse_character;
+ default:
+ buzzerSequence = 0;
+ return;
+ }
+
+ note += tmp_octave*12;
+
+ // handle sharps and flats
+ c = currentCharacter();
+ while(c == '+' || c == '#')
+ {
+ buzzerSequence ++;
+ note ++;
+ c = currentCharacter();
+ }
+ while(c == '-')
+ {
+ buzzerSequence ++;
+ note --;
+ c = currentCharacter();
+ }
+
+ // set the duration of just this note
+ tmp_duration = duration;
+
+ // If the input is 'c16', make it a 16th note, etc.
+ if(c > '0' && c < '9')
+ tmp_duration = whole_note_duration/getNumber();
+
+ // Handle dotted notes - the first dot adds 50%, and each
+ // additional dot adds 50% of the previous dot.
+ dot_add = tmp_duration/2;
+ while(currentCharacter() == '.')
+ {
+ buzzerSequence ++;
+ tmp_duration += dot_add;
+ dot_add /= 2;
+ }
+
+ if(staccato)
+ {
+ staccato_rest_duration = tmp_duration / 2;
+ tmp_duration -= staccato_rest_duration;
+ }
+
+ // this will re-enable the timer overflow interrupt
+ PololuBuzzer::playNote(rest ? SILENT_NOTE : note, tmp_duration, volume);
+// wait_ms(tmp_duration);
+}
+
+
+// This puts play() into a mode where instead of advancing to the
+// next note in the sequence automatically, it waits until the
+// function playCheck() is called. The idea is that you can
+// put playCheck() in your main loop and avoid potential
+// delays due to the note sequence being checked in the middle of
+// a time sensitive calculation. It is recommended that you use
+// this function if you are doing anything that can't tolerate
+// being interrupted for more than a few microseconds.
+// Note that the play mode can be changed while a sequence is being
+// played.
+//
+// Usage: playMode(PLAY_AUTOMATIC) makes it automatic (the
+// default), playMode(PLAY_CHECK) sets it to a mode where you have
+// to call playCheck().
+void PololuBuzzer::playMode(unsigned char mode)
+{
+ play_mode_setting = mode;
+
+ // We want to check to make sure that we didn't miss a note if we
+ // are going out of play-check mode.
+ if(mode == PLAY_AUTOMATIC)
+ playCheck();
+}
+
+
+// Checks whether it is time to start another note, and starts
+// it if so. If it is not yet time to start the next note, this method
+// returns without doing anything. Call this as often as possible
+// in your main loop to avoid delays between notes in the sequence.
+//
+// Returns true if it is still playing.
+unsigned char PololuBuzzer::playCheck()
+{
+ if(buzzerFinished && buzzerSequence != 0)
+ nextNote();
+ return buzzerSequence != 0;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/PololuBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,126 @@
+#pragma once
+#include "mbed.h"
+
+#ifndef MBED_BEEP_H
+#define MBED_BEEP_H
+
+/*! \brief Specifies that the sequence of notes will play with no further action
+ * required by the user. */
+#define PLAY_AUTOMATIC 0
+
+
+/*! \brief Specified that the user will need to call `playCheck()` regularly. */
+#define PLAY_CHECK 1
+
+// n
+// Equal Tempered Scale is given by f = f * a
+// n o
+//
+// where f is chosen as A above middle C (A4) at f = 440 Hz
+// o o
+// and a is given by the twelfth root of 2 (~1.059463094359)
+
+/*! \anchor note_macros
+ *
+ * \name Note Macros
+ * \a x specifies the octave of the note
+ * @{
+ */
+#define NOTE_C(x) ( 0 + (x)*12)
+#define NOTE_C_SHARP(x) ( 1 + (x)*12)
+#define NOTE_D_FLAT(x) ( 1 + (x)*12)
+#define NOTE_D(x) ( 2 + (x)*12)
+#define NOTE_D_SHARP(x) ( 3 + (x)*12)
+#define NOTE_E_FLAT(x) ( 3 + (x)*12)
+#define NOTE_E(x) ( 4 + (x)*12)
+#define NOTE_F(x) ( 5 + (x)*12)
+#define NOTE_F_SHARP(x) ( 6 + (x)*12)
+#define NOTE_G_FLAT(x) ( 6 + (x)*12)
+#define NOTE_G(x) ( 7 + (x)*12)
+#define NOTE_G_SHARP(x) ( 8 + (x)*12)
+#define NOTE_A_FLAT(x) ( 8 + (x)*12)
+#define NOTE_A(x) ( 9 + (x)*12)
+#define NOTE_A_SHARP(x) (10 + (x)*12)
+#define NOTE_B_FLAT(x) (10 + (x)*12)
+#define NOTE_B(x) (11 + (x)*12)
+
+/*! \brief silences buzzer for the note duration */
+#define SILENT_NOTE 0xFF
+
+/*! \brief frequency bit that indicates Hz/10<br>
+ * e.g. \a frequency = `(445 | DIV_BY_10)` gives a frequency of 44.5 Hz
+ */
+#define DIV_BY_10 (1 << 15)
+/*! @} */
+
+namespace mbed {
+
+class BuzzerValues{
+ private:
+ float freq;
+ float dur;
+ float vol;
+ public:
+ BuzzerValues(float frequency, float duration, float volume);
+ BuzzerValues();
+ void changeValue(float frequency, float duration, float volume);
+ float returnFrequency();
+ float returnDuration();
+ float returnVolume();
+};
+
+/* Class: PololuBuzzer
+ * A class which uses pwm to controle a beeper to generate sounds.
+ */
+class PololuBuzzer {
+private:
+ PwmOut _pwm;
+ Timeout toff;
+ bool playing;
+
+public:
+
+/** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+ PololuBuzzer (PinName pin);
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+ void beep (float frequency, float time, float volume);
+
+/** stop the beep instantaneous
+ * usually not used
+ */
+ void nobeep();
+
+ static BuzzerValues playFrequency(unsigned int freq, unsigned int duration,
+ unsigned char volume);
+
+ static BuzzerValues playNote(unsigned char note, unsigned int duration,
+ unsigned char volume);
+
+ static void play(const char *sequence);
+
+ static void playFromProgramSpace(const char *sequence);
+
+ static void playMode(unsigned char mode);
+
+ static unsigned char playCheck();
+
+ static unsigned char isPlaying();
+
+ bool isPlaying2(int request);
+
+ //static void stopPlaying();
+
+ // initializes timer for buzzer control
+// static void init2();
+ static void init();
+ };
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/Pushbutton.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,151 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+#include "Pushbutton.h"
+#include "mbed.h"
+#include "millis.h"
+
+// \cond
+
+PushbuttonStateMachine::PushbuttonStateMachine()
+{
+ state = 0;
+}
+
+// state 0: The value is considered to be true.
+// state 1: The value was considered to be true, but there
+// was a recent false reading so it might be falling.
+// state 2: The value is considered to be false.
+// state 3: The value was considered to be false, but there
+// was a recent true reading so it might be rising.
+//
+// The transition from state 3 to state 0 is the point where we
+// have successfully detected a rising edge an we return true.
+//
+// The prevTimeMillis variable holds the last time that we
+// transitioned to states 1 or 3.
+bool PushbuttonStateMachine::getSingleDebouncedRisingEdge(bool value)
+{
+ uint16_t timeMillis = millis();
+
+ switch (state)
+ {
+ case 0:
+ // If value is false, proceed to the next state.
+ if (!value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 1;
+ }
+ break;
+
+ case 1:
+ if (value)
+ {
+ // The value is true or bouncing, so go back to previous (initial)
+ // state.
+ state = 0;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still false, so
+ // proceed to the next state.
+ state = 2;
+ }
+ break;
+
+ case 2:
+ // If the value is true, proceed to the next state.
+ if (value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 3;
+ }
+ break;
+
+ case 3:
+ if (!value)
+ {
+ // The value is false or bouncing, so go back to previous state.
+ state = 2;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still true, so
+ // go back to the initial state and report this rising edge.
+ state = 0;
+ return true;
+ }
+ break;
+ }
+
+ return false;
+}
+
+// \endcond
+
+void PushbuttonBase::waitForPress()
+{
+ do
+ {
+ while (!isPressed()); // wait for button to be pressed
+ wait_us(10000); // debounce the button press
+ }
+ while (!isPressed()); // if button isn't still pressed, loop
+}
+
+void PushbuttonBase::waitForRelease()
+{
+ do
+ {
+ while (isPressed()); // wait for button to be released
+ wait_us(10000); // debounce the button release
+ }
+ while (isPressed()); // if button isn't still released, loop
+}
+
+void PushbuttonBase::waitForButton()
+{
+ waitForPress();
+ waitForRelease();
+}
+
+bool PushbuttonBase::getSingleDebouncedPress()
+{
+ return pressState.getSingleDebouncedRisingEdge(isPressed());
+}
+
+bool PushbuttonBase::getSingleDebouncedRelease()
+{
+ return releaseState.getSingleDebouncedRisingEdge(!isPressed());
+}
+
+Pushbutton::Pushbutton(PinName pin, uint8_t pullUp, uint8_t defaultState)
+{
+ initialized = false;
+ _pin = pin;
+ _pullUp = pullUp;
+ _defaultState = defaultState;
+}
+
+void Pushbutton::init2()
+{
+ if (_pullUp == PULL_UP_ENABLED)
+ {
+ DigitalIn myPin(_pin, PullUp);
+ }
+ else
+ {
+ DigitalIn myPin(_pin);
+ //pinMode(_pin, INPUT); // high impedance
+ }
+
+ wait_us(5); // give pull-up time to stabilize
+}
+
+bool Pushbutton::isPressed()
+{
+ init(); // initialize if needed
+ DigitalIn myPin(_pin);
+
+ return myPin != _defaultState;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/Pushbutton.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,176 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file Pushbutton.h
+ *
+ * This is the main header file for the %Pushbutton library.
+ *
+ * For an overview of the library's features, see
+ * https://github.com/pololu/pushbutton-arduino. That is the main repository
+ * for the library, though copies of the library may exist in other
+ * repositories. */
+
+#pragma once
+
+#include "mbed.h"
+
+/*! Indicates the that pull-up resistor should be disabled. */
+#define PULL_UP_DISABLED 0
+
+/*! Indicates the that pull-up resistor should be enabled. */
+#define PULL_UP_ENABLED 1
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads low. */
+#define DEFAULT_STATE_LOW 0
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads high. */
+#define DEFAULT_STATE_HIGH 1
+
+/*! The pin used for the button on the
+ * [Zumo Shield for Arduino](http://www.pololu.com/product/2508).
+ *
+ * This does not really belong here in this general pushbutton library and will
+ * probably be removed in the future. */
+#define ZUMO_BUTTON PTD3
+
+// \cond
+/** \brief A state machine that detects when a boolean value changes from false
+ * to true, with debouncing.
+ *
+ * This should be considered a private implementation detail of the library.
+ */
+class PushbuttonStateMachine
+{
+public:
+
+ PushbuttonStateMachine();
+
+ /** This should be called repeatedly in a loop. It will return true once after
+ * each time it detects the given value changing from false to true. */
+ bool getSingleDebouncedRisingEdge(bool value);
+
+private:
+
+ uint8_t state;
+ uint16_t prevTimeMillis;
+};
+// \endcond
+
+/*! \brief General pushbutton class that handles debouncing.
+ *
+ * This is an abstract class used for interfacing with pushbuttons. It knows
+ * about debouncing, but it knows nothing about how to read the current state of
+ * the button. The functions in this class get the current state of the button
+ * by calling isPressed(), a virtual function which must be implemented in a
+ * subclass of PushbuttonBase, such as Pushbutton.
+ *
+ * Most users of this library do not need to directly use PushbuttonBase or even
+ * know that it exists. They can use Pushbutton instead.*/
+class PushbuttonBase
+{
+public:
+
+ /*! \brief Waits until the button is pressed and takes care of debouncing.
+ *
+ * This function waits until the button is in the pressed state and then
+ * returns. Note that if the button is already pressed when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForPress();
+
+ /*! \brief Waits until the button is released and takes care of debouncing.
+ *
+ * This function waits until the button is in the released state and then
+ * returns. Note that if the button is already released when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForRelease();
+
+ /*! \brief Waits until the button is pressed and then waits until the button
+ * is released, taking care of debouncing.
+ *
+ * This is equivalent to calling waitForPress() and then waitForRelease(). */
+ void waitForButton();
+
+ /*! \brief Uses a state machine to return true once after each time it detects
+ * the button moving from the released state to the pressed state.
+ *
+ * This is a non-blocking function that is meant to be called repeatedly in a
+ * loop. Each time it is called, it updates a state machine that monitors the
+ * state of the button. When it detects the button changing from the released
+ * state to the pressed state, with debouncing, it returns true. */
+ bool getSingleDebouncedPress();
+
+ /*! \brief Uses a state machine to return true once after each time it detects the button moving from the pressed state to the released state.
+ *
+ * This is just like getSingleDebouncedPress() except it has a separate state
+ * machine and it watches for when the button goes from the pressed state to
+ * the released state.
+ *
+ * There is no strict guarantee that every debounced button press event
+ * returned by getSingleDebouncedPress() will have a corresponding
+ * button release event returned by getSingleDebouncedRelease(); the two
+ * functions use independent state machines and sample the button at different
+ * times. */
+ bool getSingleDebouncedRelease();
+
+ /*! \brief indicates whether button is currently pressed without any debouncing.
+ *
+ * @return 1 if the button is pressed right now, 0 if it is not.
+ *
+ * This function must be implemented in a subclass of PushbuttonBase, such as
+ * Pushbutton. */
+ virtual bool isPressed() = 0;
+
+private:
+
+ PushbuttonStateMachine pressState;
+ PushbuttonStateMachine releaseState;
+};
+
+/** \brief Main class for interfacing with pushbuttons.
+ *
+ * This class can interface with any pushbutton whose state can be read with
+ * the `digitalRead` function, which is part of the Arduino core.
+ *
+ * See https://github.com/pololu/pushbutton-arduino for an overview
+ * of the different ways to use this class. */
+class Pushbutton : public PushbuttonBase
+{
+public:
+
+ /** Constructs a new instance of Pushbutton.
+ *
+ * @param pin The pin number of the pin. This is used as an argument to
+ * `pinMode` and `digitalRead`.
+ *
+ * @param pullUp Specifies whether the pin's internal pull-up resistor should
+ * be enabled. This should be either #PULL_UP_ENABLED (which is the default if
+ * the argument is omitted) or #PULL_UP_DISABLED.
+ *
+ * @param defaultState Specifies the voltage level that corresponds to the
+ * button's default (released) state. This should be either
+ * #DEFAULT_STATE_HIGH (which is the default if this argument is omitted) or
+ * #DEFAULT_STATE_LOW. */
+ Pushbutton(PinName pin, uint8_t pullUp = PULL_UP_ENABLED,
+ uint8_t defaultState = DEFAULT_STATE_HIGH);
+
+ virtual bool isPressed();
+
+private:
+
+ void init()
+ {
+ if (!initialized)
+ {
+ initialized = true;
+ init2();
+ }
+ }
+
+ void init2();
+
+ bool initialized;
+ PinName _pin;
+ bool _pullUp;
+ bool _defaultState;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/QTRSensors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,783 @@
+/*
+ QTRSensors.cpp - Arduino library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010.
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+
+#include "QTRSensors.h"
+#include <stdlib.h>
+#include "mbed.h"
+#include<string>
+
+Timer timer;
+// static BufferedSerial serial_port(USBTX, USBRX);
+
+// Base class data member initialization (called by derived class init())
+void QTRSensors::init(unsigned char *pins, unsigned char numSensors,
+ unsigned char emitterPin, bool analog = false)
+{
+ calibratedMinimumOn=0;
+ calibratedMaximumOn=0;
+ calibratedMinimumOff=0;
+ calibratedMaximumOff=0;
+
+ _lastValue=0;
+
+ if (numSensors > QTR_MAX_SENSORS)
+ _numSensors = QTR_MAX_SENSORS;
+ else
+ _numSensors = numSensors;
+
+
+ if (_pins == 0) {
+ _pins = (PinName *)malloc(sizeof(PinName)*_numSensors);
+ if (_pins == 0)
+ return;
+ }
+
+ unsigned char i;
+ // Copy parameter values to local storage
+ PinName currentPin;
+ for (i = 0; i < _numSensors; i++) {
+ switch(pins[i]){
+ case 1:
+ currentPin = PTB23;
+ break;
+ case 2:
+ currentPin = PTB11;
+ break;
+ case 3:
+ currentPin = PTD2;
+ break;
+ case 4:
+ currentPin = PTB2;
+ break;
+ case 5:
+ currentPin = PTB10;
+ break;
+ case 6:
+ currentPin = PTA2;
+ break;
+ }
+ _pins[i] = currentPin;
+ }
+
+ // Serial pc(USBTX, USBRX);
+ // unsigned char j;
+ // for (j = 0; j < numSensors; j++)
+ // {
+ // std::string s = std::to_string(_pins[j]);
+ // const char * c = s.c_str();
+ // pc.printf(c);
+ // pc.printf("\n");
+ // }
+
+ // // Allocate the arrays
+ // // Allocate Space for Calibrated Maximum On Values
+ // calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOn == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOn[i] = 0;
+
+ // // Allocate Space for Calibrated Maximum Off Values
+ // calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOff == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOff[i] = 0;
+
+ // // Allocate Space for Calibrated Minimum On Values
+ // calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOn == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOn[i] = _maxValue;
+
+ // // Allocate Space for Calibrated Minimum Off Values
+ // calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOff == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOff[i] = _maxValue;
+
+ // emitter pin is used for DigitalOut
+ // So we create a DigitalOut on that pin
+ _emitterPin = emitterPin;
+ if(_emitterPin != 255){
+ if(emitterPin == 2){
+ _emitter = new DigitalOut(PTB9);
+ }
+ }
+
+ // If we have an Analog Sensor then we wil used AnalogIn on the pins provided
+ // We use a Vector for our collection of pins
+ // Here we reserve space for the pins
+ // _analog = analog;
+ // if (_analog) {
+ // _qtrAIPins.reserve(_numSensors);
+ // } else {
+ // // Not analog - so we use _qtrPins (which is a Vector on DigitalInOut)
+ // _qtrPins.reserve(_numSensors);
+ // }
+ // // Create the pins and push onto the vectors
+ // for (i = 0; i < _numSensors; i++) {
+ // if (analog) {
+ // _qtrAIPins.push_back(new AnalogIn(_pins[i]));
+ // } else {
+ // _qtrPins.push_back(new DigitalInOut(_pins[i]));
+ // }
+ // }
+
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in abstract units,
+// with higher values corresponding to lower reflectance (e.g. a black
+// surface or a void).
+void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode)
+{
+ unsigned int off_values[QTR_MAX_SENSORS];
+ unsigned char i;
+
+
+ if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF) {
+ emittersOn();
+ } else {
+ emittersOff();
+ }
+
+ readPrivate(sensor_values);
+
+ emittersOff();
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF) {
+ readPrivate(off_values);
+
+ for(i=0; i<_numSensors; i++) {
+ sensor_values[i] += _maxValue - off_values[i];
+ }
+ }
+}
+
+
+// Turn the IR LEDs off and on. This is mainly for use by the
+// read method, and calling these functions before or
+// after the reading the sensors will have no effect on the
+// readings, but you may wish to use these for testing purposes.
+void QTRSensors::emittersOff()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(0);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+void QTRSensors::emittersOn()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(1);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+// Resets the calibration.
+void QTRSensors::resetCalibration()
+{
+ unsigned char i;
+ for(i=0; i<_numSensors; i++) {
+ if(calibratedMinimumOn)
+ calibratedMinimumOn[i] = _maxValue;
+ if(calibratedMinimumOff)
+ calibratedMinimumOff[i] = _maxValue;
+ if(calibratedMaximumOn)
+ calibratedMaximumOn[i] = 0;
+ if(calibratedMaximumOff)
+ calibratedMaximumOff[i] = 0;
+ }
+}
+
+// Reads the sensors 10 times and uses the results for
+// calibration. The sensor values are not returned; instead, the
+// maximum and minimum values found over time are stored internally
+// and used for the readCalibrated() method.
+void QTRSensors::calibrate(unsigned char readMode)
+{
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) {
+ calibrateOnOrOff(&calibratedMinimumOn,
+ &calibratedMaximumOn,
+ QTR_EMITTERS_ON);
+ }
+
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) {
+ calibrateOnOrOff(&calibratedMinimumOff,
+ &calibratedMaximumOff,
+ QTR_EMITTERS_OFF);
+ }
+}
+
+void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum,
+ unsigned int **calibratedMaximum,
+ unsigned char readMode)
+{
+ int i;
+ unsigned int sensor_values[16];
+ unsigned int max_sensor_values[16];
+ unsigned int min_sensor_values[16];
+
+ // initialisation of calibrated sensor values moved to init()
+
+ // Allocate the arrays if necessary.
+ if(*calibratedMaximum == 0)
+ {
+ *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMaximum == 0)
+ return;
+
+ // Initialize the max and min calibrated values to values that
+ // will cause the first reading to update them.
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMaximum)[i] = 0;
+ }
+ if(*calibratedMinimum == 0)
+ {
+ *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMinimum == 0)
+ return;
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMinimum)[i] = _maxValue;
+ }
+
+ int j;
+ for(j=0; j<10; j++) {
+ read(sensor_values,readMode);
+ for(i=0; i<_numSensors; i++) {
+ // set the max we found THIS time
+ if(j == 0 || max_sensor_values[i] < sensor_values[i])
+ max_sensor_values[i] = sensor_values[i];
+
+ // set the min we found THIS time
+ if(j == 0 || min_sensor_values[i] > sensor_values[i])
+ min_sensor_values[i] = sensor_values[i];
+ }
+ }
+
+ // record the min and max calibration values
+ for(i=0; i<_numSensors; i++) {
+ if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i]
+ (*calibratedMaximum)[i] = min_sensor_values[i];
+ if(max_sensor_values[i] < (*calibratedMinimum)[i])
+ (*calibratedMinimum)[i] = max_sensor_values[i];
+ }
+
+}
+
+
+// Returns values calibrated to a value between 0 and 1000, where
+// 0 corresponds to the minimum value read by calibrate() and 1000
+// corresponds to the maximum value. Calibration values are
+// stored separately for each sensor, so that differences in the
+// sensors are accounted for automatically.
+void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode)
+{
+ int i;
+
+ // if not calibrated, do nothing
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
+ if(!calibratedMinimumOff || !calibratedMaximumOff)
+ return;
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
+ if(!calibratedMinimumOn || !calibratedMaximumOn)
+ return;
+
+ // read the needed values
+ read(sensor_values,readMode);
+
+ for(i=0; i<_numSensors; i++) {
+ unsigned int calmin,calmax;
+ unsigned int denominator;
+
+ // find the correct calibration
+ if(readMode == QTR_EMITTERS_ON) {
+ calmax = calibratedMaximumOn[i];
+ calmin = calibratedMinimumOn[i];
+ } else if(readMode == QTR_EMITTERS_OFF) {
+ calmax = calibratedMaximumOff[i];
+ calmin = calibratedMinimumOff[i];
+ } else { // QTR_EMITTERS_ON_AND_OFF
+
+ if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
+ calmin = _maxValue;
+ else
+ calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue
+
+ if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal
+ calmax = _maxValue;
+ else
+ calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue
+ }
+
+ denominator = calmax - calmin;
+
+ signed int x = 0;
+ if(denominator != 0)
+ x = (((signed long)sensor_values[i]) - calmin)
+ * 1000 / denominator;
+ if(x < 0)
+ x = 0;
+ else if(x > 1000)
+ x = 1000;
+ sensor_values[i] = x;
+ }
+
+}
+
+
+// Operates the same as read calibrated, but also returns an
+// estimated position of the robot with respect to a line. The
+// estimate is made using a weighted average of the sensor indices
+// multiplied by 1000, so that a return value of 0 indicates that
+// the line is directly below sensor 0, a return value of 1000
+// indicates that the line is directly below sensor 1, 2000
+// indicates that it's below sensor 2000, etc. Intermediate
+// values indicate that the line is between two sensors. The
+// formula is:
+//
+// 0*value0 + 1000*value1 + 2000*value2 + ...
+// --------------------------------------------
+// value0 + value1 + value2 + ...
+//
+// By default, this function assumes a dark line (high values)
+// surrounded by white (low values). If your line is light on
+// black, set the optional second argument white_line to true. In
+// this case, each sensor value will be replaced by (1000-value)
+// before the averaging.
+int QTRSensors::readLine(unsigned int *sensor_values,
+ unsigned char readMode, unsigned char white_line)
+{
+ unsigned char i, on_line = 0;
+ unsigned long avg; // this is for the weighted total, which is long
+ // before division
+ unsigned int sum; // this is for the denominator which is <= 64000
+ // static int last_value=0; // assume initially that the line is left.
+
+ readCalibrated(sensor_values, readMode);
+
+ avg = 0;
+ sum = 0;
+
+ for(i=0; i<_numSensors; i++) {
+ int value = sensor_values[i];
+ if(white_line)
+ value = 1000-value;
+
+ // keep track of whether we see the line at all
+ if(value > 200) {
+ on_line = 1;
+ }
+
+ // only average in values that are above a noise threshold
+ if(value > 50) {
+ avg += (long)(value) * (i * 1000);
+ sum += value;
+ }
+ }
+
+ if(!on_line) {
+ // If it last read to the left of center, return 0.
+ if(_lastValue < (_numSensors-1)*1000/2)
+ return 0;
+
+ // If it last read to the right of center, return the max.
+ else
+ return (_numSensors-1)*1000;
+
+ }
+
+ _lastValue = avg/sum;
+
+ return _lastValue;
+}
+
+
+
+// Derived RC class constructors
+QTRSensorsRC::QTRSensorsRC()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsRC::QTRSensorsRC(unsigned char* pins,
+ unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ init(pins, numSensors, timeout, emitterPin);
+}
+
+
+// The array 'pins' contains the Arduino pin number for each sensor.
+
+// 'numSensors' specifies the length of the 'pins' array (i.e. the
+// number of QTR-RC sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'timeout' specifies the length of time in microseconds beyond
+// which you consider the sensor reading completely black. That is to say,
+// if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+// and the reading for that pin will be considered full black.
+// It is recommended that you set timeout to be between 1000 and
+// 3000 us, depending on things like the height of your sensors and
+// ambient lighting. Using timeout allows you to shorten the
+// duration of a sensor-reading cycle while still maintaining
+// useful analog measurements of reflectance
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsRC::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned int timeout, unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin, false);
+
+ _maxValue = timeout;
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// ...
+// The values returned are in microseconds and range from 0 to
+// timeout (as specified in the constructor).
+void QTRSensorsRC::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i;
+ // Serial pc(USBTX, USBRX);
+
+ if (_pins == 0)
+ return;
+
+ DigitalInOut pinout1(_pins[0]);
+ DigitalInOut pinout2(_pins[1]);
+ DigitalInOut pinout3(_pins[2]);
+ DigitalInOut pinout4(_pins[3]);
+ DigitalInOut pinout5(_pins[4]);
+ DigitalInOut pinout6(_pins[5]);
+
+ for(i = 0; i < _numSensors; i++) {
+ sensor_values[i] = _maxValue;
+ switch(i){
+ case 0:
+ pinout1.output(); // make sensor line an output and drive high
+ pinout1.write(1);
+ break;
+ case 1:
+ pinout2.output(); // make sensor line an output and drive high
+ pinout2.write(1);
+ break;
+ case 2:
+ pinout3.output(); // make sensor line an output and drive high
+ pinout3.write(1);
+ break;
+ case 3:
+ pinout4.output(); // make sensor line an output and drive high
+ pinout4.write(1);
+ break;
+ case 4:
+ pinout5.output(); // make sensor line an output and drive high
+ pinout5.write(1);
+ break;
+ case 5:
+ pinout6.output(); // make sensor line an output and drive high
+ pinout6.write(1);
+ break;
+ }
+ }
+
+ wait_us(10); // charge lines for 10 us
+
+ for(i = 0; i < _numSensors; i++) {
+ // important: disable internal pull-up!
+ // ??? do we need to change the mode: _qtrPins[i]->mode(OpenDrain);
+ // or just change mode to input
+ // mbed documentation is not clear and I do not have a test sensor
+ switch(i){
+ case 0:
+ pinout1.input(); // make sensor line an output and drive high
+ pinout1.mode(PullNone);
+ break;
+ case 1:
+ pinout2.input(); // make sensor line an output and drive high
+ pinout2.mode(PullNone);
+ break;
+ case 2:
+ pinout3.input(); // make sensor line an output and drive high
+ pinout3.mode(PullNone);
+ break;
+ case 3:
+ pinout4.input(); // make sensor line an output and drive high
+ pinout4.mode(PullNone);
+ break;
+ case 4:
+ pinout5.input(); // make sensor line an output and drive high
+ pinout5.mode(PullNone);
+ break;
+ case 5:
+ pinout6.input(); // make sensor line an output and drive high
+ pinout6.mode(PullNone);
+ break;
+ }
+ }
+
+ // int pausedTime = 0;
+ // bool addTime = false;
+ // bool detectedLine = false;
+ // bool pauseWhile = false;
+ timer.start();
+ unsigned long startTime = timer.read_us();
+ while ((timer.read_us() - startTime) < _maxValue) {
+ unsigned int time = timer.read_us() - startTime;
+ for (i = 0; i < _numSensors; i++) {
+ switch(i){
+ case 0:
+ if (pinout1.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 1:
+ if (pinout2.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 2:
+ if (pinout3.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 3:
+ if (pinout4.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 4:
+ if (pinout5.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 5:
+ if (pinout6.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ }
+ }
+ // wait_us(1);
+ // wait_us(300);
+ // timer.stop();
+ // pausedTime = pausedTime + 300;
+ // addTime = true;
+ // timer.start();
+ }
+ // printf("Timer2 %d\n", timer.read_us());
+ // pausedTime = 0;
+ timer.stop();
+ timer.reset();
+}
+
+
+// Derived Analog class constructors
+QTRSensorsAnalog::QTRSensorsAnalog()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsAnalog::QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ // this is analog - so use analog = true as a parameter
+
+ init(pins, numSensors, numSamplesPerSensor, emitterPin);
+}
+
+
+// the array 'pins' contains the Arduino analog pin assignment for each
+// sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+// Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+// and sensor 3 is on Arduino analog input 7.
+
+// 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+// number of QTR-A sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+// to average per channel (i.e. per sensor) for each reading. The total
+// number of analog-to-digital conversions performed will be equal to
+// numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+// perform a single analog-to-digital conversion, so:
+// if numSamplesPerSensor is 4 and numSensors is 6, it will take
+// 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+// Increasing this parameter increases noise suppression at the cost of
+// sample rate. The recommended value is 4.
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsAnalog::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin);
+
+ _numSamplesPerSensor = numSamplesPerSensor;
+ _maxValue = 1023; // this is the maximum returned by the A/D conversion
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in terms of a
+// 10-bit ADC average with higher values corresponding to lower
+// reflectance (e.g. a black surface or a void).
+void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i, j;
+
+ if (_pins == 0)
+ return;
+
+ // reset the values
+ for(i = 0; i < _numSensors; i++)
+ sensor_values[i] = 0;
+
+ for (j = 0; j < _numSamplesPerSensor; j++) {
+ for (i = 0; i < _numSensors; i++) {
+ sensor_values[i] += static_cast<unsigned int>(AnalogIn(_pins[i])); // add the conversion result
+ }
+ }
+
+ // get the rounded average of the readings for each sensor
+ for (i = 0; i < _numSensors; i++)
+ sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
+ _numSamplesPerSensor;
+}
+
+// the destructor frees up allocated memory
+QTRSensors::~QTRSensors()
+{
+ if(calibratedMinimumOff)
+ free(calibratedMinimumOff);
+ if(calibratedMinimumOn)
+ free(calibratedMinimumOn);
+ if(calibratedMaximumOff)
+ free(calibratedMaximumOff);
+ if(calibratedMaximumOn)
+ free(calibratedMaximumOn);
+ if (_pins)
+ free(_pins);
+ unsigned int i;
+ // for (i = 0; i < _numSensors; i++) {
+ // if (_analog) {
+ // delete _qtrAIPins[i];
+ // } else {
+ // delete _qtrPins[i];
+ // }
+ // }
+ // if (_analog) {
+ // _qtrAIPins.clear();
+ // vector<AnalogIn *>().swap(_qtrAIPins);
+ // } else {
+ // _qtrPins.clear();
+ // vector<DigitalInOut *>().swap(_qtrPins);
+ // }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/QTRSensors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,283 @@
+/*
+ QTRSensors.h - Library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+#include "mbed.h"
+#include <vector>
+
+#ifndef QTRSensors_h
+#define QTRSensors_h
+
+#define QTR_EMITTERS_OFF 0
+#define QTR_EMITTERS_ON 1
+#define QTR_EMITTERS_ON_AND_OFF 2
+
+#define QTR_NO_EMITTER_PIN 255
+
+#define QTR_MAX_SENSORS 16
+#define HIGH 1
+#define LOW 0
+
+// This class cannot be instantiated directly (it has no constructor).
+// Instead, you should instantiate one of its two derived classes (either the
+// QTR-A or QTR-RC version, depending on the type of your sensor).
+class QTRSensors
+{
+ public:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in abstract units,
+ // with higher values corresponding to lower reflectance (e.g. a black
+ // surface or a void).
+ // If measureOffAndOn is true, measures the values with the
+ // emitters on AND off and returns on - (timeout - off). If this
+ // value is less than zero, it returns zero.
+ // This method will call the appropriate derived class's readPrivate(),
+ // which is defined as a virtual function in the base class and
+ // overridden by each derived class's own implementation.
+ void read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Turn the IR LEDs off and on. This is mainly for use by the
+ // read method, and calling these functions before or
+ // after the reading the sensors will have no effect on the
+ // readings, but you may wish to use these for testing purposes.
+ void emittersOff();
+ void emittersOn();
+
+ // Reads the sensors for calibration. The sensor values are
+ // not returned; instead, the maximum and minimum values found
+ // over time are stored internally and used for the
+ // readCalibrated() method.
+ void calibrate(unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Resets all calibration that has been done.
+ void resetCalibration();
+
+ // Returns values calibrated to a value between 0 and 1000, where
+ // 0 corresponds to the minimum value read by calibrate() and 1000
+ // corresponds to the maximum value. Calibration values are
+ // stored separately for each sensor, so that differences in the
+ // sensors are accounted for automatically.
+ void readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Operates the same as read calibrated, but also returns an
+ // estimated position of the robot with respect to a line. The
+ // estimate is made using a weighted average of the sensor indices
+ // multiplied by 1000, so that a return value of 0 indicates that
+ // the line is directly below sensor 0, a return value of 1000
+ // indicates that the line is directly below sensor 1, 2000
+ // indicates that it's below sensor 2000, etc. Intermediate
+ // values indicate that the line is between two sensors. The
+ // formula is:
+ //
+ // 0*value0 + 1000*value1 + 2000*value2 + ...
+ // --------------------------------------------
+ // value0 + value1 + value2 + ...
+ //
+ // By default, this function assumes a dark line (high values)
+ // surrounded by white (low values). If your line is light on
+ // black, set the optional second argument white_line to true. In
+ // this case, each sensor value will be replaced by (1000-value)
+ // before the averaging.
+ int readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char white_line = 0);
+
+ unsigned int *calibratedMinimumOn;
+ unsigned int *calibratedMaximumOn;
+ unsigned int *calibratedMinimumOff;
+ unsigned int *calibratedMaximumOff;
+
+ // Calibrated minumum and maximum values. These start at 1000 and
+ // 0, respectively, so that the very first sensor reading will
+ // update both of them.
+ //
+ // The pointers are unallocated until calibrate() is called, and
+ // then allocated to exactly the size required. Depending on the
+ // readMode argument to calibrate, only the On or Off values may
+ // be allocated, as required.
+ //
+ // These variables are made public so that you can use them for
+ // your own calculations and do things like saving the values to
+ // EEPROM, performing sanity checking, etc.
+
+ ~QTRSensors();
+
+ protected:
+
+ QTRSensors()
+ {
+
+ };
+
+ void init(unsigned char *pins, unsigned char numSensors, unsigned char emitterPin, bool);
+
+ bool _analog;
+ PinName *_pins;
+ unsigned char _numSensors;
+ unsigned char _emitterPin;
+ unsigned int _maxValue; // the maximum value returned by this function#
+ int _lastValue;
+ DigitalOut *_emitter;
+ // std::vector<DigitalInOut *> _qtrPins;
+ // std::vector<AnalogIn *> _qtrAIPins;
+
+ private:
+
+ virtual void readPrivate(unsigned int *sensor_values) = 0;
+
+ // Handles the actual calibration. calibratedMinimum and
+ // calibratedMaximum are pointers to the requested calibration
+ // arrays, which will be allocated if necessary.
+ void calibrateOnOrOff(unsigned int **calibratedMaximum,
+ unsigned int **calibratedMinimum,
+ unsigned char readMode);
+};
+
+
+
+// Object to be used for QTR-1RC and QTR-8RC sensors
+class QTRSensorsRC : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsRC();
+
+ // this constructor just calls init()
+ QTRSensorsRC(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 4000, unsigned char emitterPin = 255);
+
+ // The array 'pins' contains the Arduino pin number for each sensor.
+
+ // 'numSensors' specifies the length of the 'pins' array (i.e. the
+ // number of QTR-RC sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'timeout' specifies the length of time in microseconds beyond
+ // which you consider the sensor reading completely black. That is to say,
+ // if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+ // and the reading for that pin will be considered full black.
+ // It is recommended that you set timeout to be between 1000 and
+ // 3000 us, depending on things like the height of your sensors and
+ // ambient lighting. Using timeout allows you to shorten the
+ // duration of a sensor-reading cycle while still maintaining
+ // useful analog measurements of reflectance
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 2000, unsigned char emitterPin = QTR_NO_EMITTER_PIN); // NC = Not Connected
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in microseconds.
+ void readPrivate(unsigned int *sensor_values);
+};
+
+
+
+// Object to be used for QTR-1A and QTR-8A sensors
+class QTRSensorsAnalog : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsAnalog();
+
+ // this constructor just calls init()
+ QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4,
+ unsigned char emitterPin = 255);
+
+ // the array 'pins' contains the Arduino analog pin assignment for each
+ // sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+ // Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+ // and sensor 3 is on Arduino analog input 7.
+
+ // 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+ // number of QTR-A sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+ // to average per channel (i.e. per sensor) for each reading. The total
+ // number of analog-to-digital conversions performed will be equal to
+ // numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+ // perform a single analog-to-digital conversion, so:
+ // if numSamplesPerSensor is 4 and numSensors is 6, it will take
+ // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+ // Increasing this parameter increases noise suppression at the cost of
+ // sample rate. The recommended value is 4.
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* analogPins, unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4, unsigned char emitterPin = QTR_NO_EMITTER_PIN);
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in terms of a
+ // 10-bit ADC average with higher values corresponding to lower
+ // reflectance (e.g. a black surface or a void).
+ void readPrivate(unsigned int *sensor_values);
+
+ unsigned char _numSamplesPerSensor;
+};
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/ZumoBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,29 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file ZumoBuzzer.h */
+
+#pragma once
+
+#include "PololuBuzzer.h"
+
+/*! \brief Plays beeps and music on the buzzer on the Zumo 32U4.
+ *
+ * This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on
+ * the Zumo 32U4 buzzer.
+ *
+ * Note durations are timed using a timer overflow interrupt
+ * (`TIMER4_OVF`), which will briefly interrupt execution of your
+ * main program at the frequency of the sound being played. In most cases, the
+ * interrupt-handling routine is very short (several microseconds). However,
+ * when playing a sequence of notes in `PLAY_AUTOMATIC` mode (the default mode)
+ * with the `play()` command, this interrupt takes much longer than normal
+ * (perhaps several hundred microseconds) every time it starts a new note. It is
+ * important to take this into account when writing timing-critical code.
+ */
+class ZumoBuzzer : public PololuBuzzer
+{
+ // This is a trivial subclass of PololuBuzzer; it exists because
+ // we wanted the ZumoShield class names to be consistent and we
+ // didn't just use a typedef to define it because that would make
+ // the Doxygen documentation harder to understand.
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/ZumoMotors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,84 @@
+/* File: DRV8835.cpp
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+
+#include "mbed.h"
+#include "ZumoMotors.h"
+
+
+ZumoMotors::ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin) :
+pwmL(pinPwmL),
+Lin(pinLin),
+pwmR(pinPwmR),
+Rin(pinRin)
+{
+ Lin = 0;
+ Rin = 0;
+ pwmL.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmL = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ pwmR.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmR = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::stop()
+{
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::motorL_stop(void)
+{
+ pwmL = 0;
+}
+
+void ZumoMotors::motorR_stop(void)
+{
+ pwmR = 0;
+}
+
+void ZumoMotors::setSpeeds(float Left,float Right)
+{
+ //Set Right Speed and Direction
+ if(Right<0)
+ {
+ motorR_rev(Right*-1);
+ } else {
+ motorR_fwd(Right);
+ }
+
+ //Set Left Speed and Direction
+ if(Left<0)
+ {
+ motorL_rev(Left*-1);
+ } else {
+ motorL_fwd(Left);
+ }
+}
+
+void ZumoMotors::motorL_fwd(float fPulsewidth)
+{
+ Lin = 0;
+ pwmL = fPulsewidth;
+}
+void ZumoMotors::motorL_rev(float fPulsewidth)
+{
+ Lin = 1;
+ pwmL = fPulsewidth;
+}
+
+void ZumoMotors::motorR_fwd(float fPulsewidth)
+{
+ Rin = 0;
+ pwmR = fPulsewidth;
+}
+void ZumoMotors::motorR_rev(float fPulsewidth)
+{
+ Rin = 1;
+ pwmR = fPulsewidth;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/ZumoMotors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,38 @@
+/* File: DRV8835.h
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+#ifndef __DRV8835_H__
+#define __DRV8835_H__
+
+#include "mbed.h"
+
+#define DRV8835_PWM_PERIOD_DEFAULT (0.00005) // 2 ms period // 50KHz (0.00002)
+#define DRV8835_PWM_PULSEWIDTH_DEFAULT (0.50) // 50% duty cycle
+
+class ZumoMotors
+{
+public:
+ ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin);
+
+ void motorL_stop(void);
+ void motorL_fwd(float fPulsewidth);
+ void motorL_rev(float fPulsewidth);
+ void motorR_stop(void);
+ void motorR_fwd(float fPulsewidth);
+ void motorR_rev(float fPulsewidth);
+ void setSpeeds(float Left,float Right);
+ void stop(void);
+
+private:
+ PwmOut pwmL;
+ DigitalOut Lin;
+ PwmOut pwmR;
+ DigitalOut Rin;
+};
+
+#endif /* __DRV8835_H__ */
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/ZumoReflectanceSensorArray.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,406 @@
+/*! \file ZumoReflectanceSensorArray.h
+ *
+ * See the ZumoReflectanceSensorArray class reference for more information about
+ * this library.
+ *
+ * \class ZumoReflectanceSensorArray ZumoReflectanceSensorArray.h
+ * \brief Read from reflectance sensor array
+ *
+ * ZumoReflectanceSensorArray provides an interface for using a
+ * [Zumo Reflectance Sensor Array](https://www.pololu.com/product/1419) connected
+ * to a Zumo robot. It provides access to the raw sensor values as well
+ * as to high level functions including calibration and line-tracking.
+ *
+ * For calibration, memory is allocated using the `malloc()` function. This
+ * conserves RAM: if all six sensors are calibrated with the emitters both on
+ * and off, a total of 48 bytes is dedicated to storing calibration values.
+ * However, for an application where only two sensors are used, and the
+ * emitters are always on during reads, only 4 bytes are required.
+ *
+ * Internally, it uses all standard Arduino functions such as
+ * `micros()` for timing and `digitalRead()` for getting the sensor values, so
+ * it should work on all Arduinos without conflicting with other libraries.
+ *
+ * ### Calibration ###
+ *
+ * ZumoReflectanceSensorArray allows you to use the `calibrate()`
+ * method to easily calibrate your sensors for the particular
+ * conditions it will encounter. Calibrating your sensors can lead to
+ * substantially more reliable sensor readings, which in turn can help
+ * simplify your code. As such, we recommend you build a calibration
+ * phase into your Zumo's initialization routine. This can be as
+ * simple as a fixed duration over which you repeatedly call the
+ * `calibrate()` method.
+ *
+ * During this calibration phase, you will need to expose each of your
+ * reflectance sensors to the lightest and darkest readings they will
+ * encounter. For example, if your Zumo is programmed to be a line
+ * follower, you will want to slide it across the line during the
+ * calibration phase so the each sensor can get a reading of how dark
+ * the line is and how light the ground is (or you can program it to
+ * automatically turn back and forth to pass all of the sensors over
+ * the line). The **SensorCalibration** example demonstrates a
+ * calibration routine.
+ *
+ * ### Reading the sensors
+ *
+ *
+ * ZumoReflectanceSensorArray gives you a number of different ways to
+ * read the sensors.
+ *
+ * - You can request raw sensor values using the `read()` method.
+ *
+ * - You can request calibrated sensor values using the `readCalibrated()`
+ * method. Calibrated sensor values will always range from 0 to 1000, with the
+ * extreme values corresponding to the most and least reflective surfaces
+ * encountered during calibration.
+ *
+ * - For line-detection applications, you can request the line location using
+ * the `readLine()` method. This function provides calibrated values
+ * for each sensor and returns an integer that tells you where it thinks the
+ * line is.
+ *
+ * ### Class inheritance ###
+ *
+ * The ZumoReflectanceSensorArray class is derived from the QTRSensorsRC class,
+ * which is in turn derived from the QTRSensors base class. The QTRSensorsRC and
+ * QTRSensors classes are part of the \ref QTRSensors.h "QTRSensors" library,
+ * which provides more general functionality for working with reflectance
+ * sensors and is included in the Zumo Shield Arduino Library as a dependency
+ * for this library.
+ *
+ * We recommend using ZumoReflectanceSensorArray instead of
+ * the \ref QTRSensors.h "QTRSensors" library when programming an Arduino on a
+ * Zumo. For documentation specific to the %QTRSensors library, please see its
+ * [user's guide](https://www.pololu.com/docs/0J19) on Pololu's website.
+ */
+
+#ifndef ZumoReflectanceSensorArray_h
+#define ZumoReflectanceSensorArray_h
+
+#include "QTRSensors.h"
+#include "mbed.h"
+
+#define ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN 2
+
+class ZumoReflectanceSensorArray : public QTRSensorsRC
+{
+ public:
+
+ /*! \brief Minimal constructor.
+ *
+ * This version of the constructor performs no initialization. If it is used,
+ * the user must call init() before using the methods in this class.
+ */
+ ZumoReflectanceSensorArray() {}
+
+ /*! \brief Constructor; initializes with given emitter pin and defaults for
+ * other settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char emitterPin)` with the specified
+ * emitter pin and default values for other settings.
+ */
+ ZumoReflectanceSensorArray(unsigned char emitterPin)
+ {
+ init(emitterPin);
+ }
+
+ /*! \brief Constructor; initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char * pins, unsigned char
+ * numSensors, unsigned int timeout, unsigned char emitterPin)` with all
+ * settings as given.
+ */
+ ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+
+ /*! \brief Initializes with given emitter pin and and defaults for other
+ * settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with the
+ * specified emitter pin. The other settings are set to default values: all
+ * six sensors on the array are active, and a timeout of 2000 microseconds is
+ * used.
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The "LED ON" jumper on the Zumo Reflectance Sensor Array must be
+ * configured correctly for this to work.) If the value `QTR_NO_EMITTER_PIN`
+ * (255) is used, you can leave the emitter pin disconnected and the IR
+ * emitters will always be on.
+ */
+ void init(unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ unsigned char sensorPins[] = { 1, 2, 3, 4, 5, 6 };
+ QTRSensorsRC::init(sensorPins, sizeof(sensorPins), 2000, emitterPin);
+ // pc.printf("hello");
+ }
+
+ /*! \brief Initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with all
+ * settings as given.
+ *
+ * The array \a pins should contain the Arduino digital pin numbers for each
+ * sensor.
+ *
+ * \a numSensors specifies the length of the \a pins array (the number of
+ * reflectance sensors you are using).
+ *
+ * \a timeout specifies the length of time in microseconds beyond which you
+ * consider the sensor reading completely black. That is to say, if the pulse
+ * length for a pin exceeds \a timeout, pulse timing will stop and the reading
+ * for that pin will be considered full black. It is recommended that you set
+ * \a timeout to be between 1000 and 3000 us, depending on factors like the
+ * height of your sensors and ambient lighting. This allows you to shorten the
+ * duration of a sensor-reading cycle while maintaining useful measurements of
+ * reflectance. If \a timeout is not specified, it defaults to 2000 us. (See
+ * the [product page](https://www.pololu.com/product/1419) for the Zumo
+ * Reflectance Sensor Array on Pololu's website for an overview of the
+ * sensors' principle of operation.)
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The corresponding connection should be made with the "LED ON"
+ * jumper on the Zumo Reflectance Sensor Array.) If the value
+ * `QTR_NO_EMITTER_PIN` (255) is used, you can leave the emitter pin
+ * disconnected and the IR emitters will always be on.
+ *
+ * This version of `%init()` can be useful if you only want to use a subset
+ * of the six reflectance sensors on the array. For example, using the
+ * outermost two sensors (on pins 4 and 5 by default) is usually enough for
+ * detecting the ring border in sumo competitions:
+ *
+ * ~~~{.ino}
+ * ZumoReflectanceSensorArray reflectanceSensors;
+ *
+ * ...
+ *
+ * reflectanceSensors.init((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ * Alternatively, you can use \ref ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+ * "a different constructor" to declare and initialize the object at the same
+ * time:
+ *
+ * ~~~{.ino}
+ *
+ * ZumoReflectanceSensorArray reflectanceSensors((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ */
+ void init(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+};
+
+// documentation for inherited functions
+
+/*! \fn void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+\memberof ZumoReflectanceSensorArray
+ * \brief Reads the raw sensor values into an array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * There **must** be space in the \a sensor_values array for as many values as
+ * there were sensors specified in the constructor. The values returned are
+ * measures of the reflectance in units of microseconds. They will be raw
+ * readings between 0 and the \a timeout argument (in units of microseconds)
+ * provided in the constructor (which defaults to 2000).
+ *
+ * The \a readMode argument specifies the kind of read that will be performed.
+ * Several options are defined:
+ *
+ * - `QTR_EMITTERS_OFF` specifies that the reading should be made without
+ * turning on the infrared (IR) emitters, in which case the reading represents
+ * ambient light levels near the sensor.
+ * - `QTR_EMITTERS_ON` specifies that the emitters should be turned on for the
+ * reading, which results in a measure of reflectance.
+ * - `QTR_EMITTERS_ON_AND_OFF` specifies that a reading should be made in both
+ * the on and off states. The values returned when this option is used are
+ * given by the formula **on + max − off**, where **on** is the reading
+ * with the emitters on, **off** is the reading with the emitters off, and
+ * **max** is the maximum sensor reading. This option can reduce the amount of
+ * interference from uneven ambient lighting.
+ *
+ * Note that emitter control will only work if you specify a valid emitter pin
+ * in the constructor and make the corresponding connection (with the "LED ON"
+ * jumper or otherwise).
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOff()
+ * \brief Turns the IR LEDs off.
+ *
+ * This is mainly for use by the `read()` method, and calling this function
+ * before or after reading the sensors will have no effect on the readings, but
+ * you might wish to use it for testing purposes. This method will only do
+ * something if the emitter pin specified in the constructor is valid (i.e. not
+ * `QTR_NO_EMITTER_PIN`) and the corresponding connection is made.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOn()
+ * \brief Turns the IR LEDs on.
+ * \copydetails emittersOff
+ */
+
+/*! \fn void QTRSensors::calibrate(unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Reads the sensors for calibration.
+ *
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * The sensor values read by this function are not returned; instead, the
+ * maximum and minimum values found over time are stored internally and used for
+ * the `readCalibrated()` method. You can access the calibration (i.e raw max
+ * and min sensor readings) through the public member pointers
+ * `calibratedMinimumOn`, `calibratedMaximumOn`, `calibratedMinimumOff`, and
+ * `calibratedMaximumOff`. Note that these pointers will point to arrays of
+ * length \a numSensors, as specified in the constructor, and they will only be
+ * allocated **after** `calibrate()` has been called. If you only calibrate with
+ * the emitters on, the calibration arrays that hold the off values will not be
+ * allocated.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::resetCalibration()
+ * \brief Resets all calibration that has been done.
+ *
+ * This function discards the calibration values that have been previously
+ * recorded, resetting the min and max values.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Returns sensor readings normalized to values between 0 and 1000.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * 0 corresponds to a reading that is less than or equal to the minimum value
+ * read by `calibrate()` and 1000 corresponds to a reading that is greater than
+ * or equal to the maximum value. Calibration values are stored separately for
+ * each sensor, so that differences in the sensors are accounted for
+ * automatically.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn int QTRSensors::readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char whiteLine = 0)
+ * \brief Returns an estimated position of a line under the sensor array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ * \param whiteLine 0 to detect a dark line on a light surface; 1 to detect
+ * a light line on a dark surface.
+ * \return An estimated line position.
+ *
+ * This function operates the same as `readCalibrated()`, but with a feature
+ * designed for line following: it returns an estimated position of the line.
+ * The estimate is made using a weighted average of the sensor indices
+ * multiplied by 1000, so that a return value of 0 indicates that the line is
+ * directly below sensor 0 (or was last seen by sensor 0 before being lost), a
+ * return value of 1000 indicates that the line is directly below sensor 1, 2000
+ * indicates that it's below sensor 2, etc. Intermediate values indicate that
+ * the line is between two sensors. The formula is:
+ *
+ * \f[
+ * \newcommand{sv}[1]{\mathtt{sensor_values[#1]}}
+ * \text{return value} =
+ * \frac{(0 \times \sv{0}) + (1000 \times \sv{1}) + (2000 \times \sv{2}) + \ldots}
+ * {\sv{0} + \sv{1} + \sv{2} + \ldots}
+ * \f]
+ *
+ * As long as your sensors aren't spaced too far apart relative to the line,
+ * this returned value is designed to be monotonic, which makes it great for use
+ * in closed-loop PID control. Additionally, this method remembers where it last
+ * saw the line, so if you ever lose the line to the left or the right, its line
+ * position will continue to indicate the direction you need to go to reacquire
+ * the line. For example, if sensor 5 is your rightmost sensor and you end up
+ * completely off the line to the left, this function will continue to return
+ * 5000.
+ *
+ * By default, this function assumes a dark line (high values) on a light
+ * background (low values). If your line is light on dark, set the optional
+ * second argument \a whiteLine to true. In this case, each sensor value will be
+ * replaced by the maximum possible value minus its actual value before the
+ * averaging.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+
+// documentation for inherited member variables
+
+/*!
+ * \property unsigned int * QTRSensors::calibratedMinimumOn
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * on.
+ *
+ * This pointer is unallocated and set to 0 until `calibrate()` is called, and
+ * then allocated to exactly the size required. Depending on the \a readMode
+ * argument to `calibrate()`, only the On or Off values might be allocated, as
+ * required. This variable is made public so that you can use the calibration
+ * values for your own calculations and do things like saving them to EEPROM,
+ * performing sanity checking, etc.
+ *
+ * The ZumoReflectanceSensorArray class inherits this variable from the
+ * QTRSensors class.
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOn
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * on.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMinimumOff
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOff
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ */
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/BorderDetect/millis.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "millis.h"
+/*
+ millis.cpp
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+volatile unsigned long _millis;
+
+void millisStart(void) {
+ SysTick_Config(SystemCoreClock / 1000);
+}
+
+extern "C" void SysTicks_Handler(void) {
+ _millis++;
+}
+
+unsigned long millis(void) {
+ return _millis;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/BorderDetect/millis.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,25 @@ +#ifndef MILLIS_H +#define MILLIS_H +/* + millis.h + Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com> + All rights reserved. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +void millisStart(void); +unsigned long millis(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/FXOS8700Q.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,231 @@
+/* FXOS8700Q sensor driver
+ * Copyright (c) 2014-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "FXOS8700Q.h"
+
+const uint16_t uint14_max = 0x3FFF;
+void static inline normalize_14bits(int16_t &x)
+{
+ x = ((x) > (uint14_max/2)) ? (x - uint14_max) : (x);
+}
+
+static int16_t dummy_int16_t = 0;
+static float dummy_float = 0.0f;
+
+FXOS8700Q::FXOS8700Q(I2C &i2c, uint8_t addr)
+{
+ _i2c = &i2c;
+ _addr = addr;
+ // activate the peripheral
+ uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+ _i2c->frequency(400000);
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_M_CTRL_REG1;
+ data[1] = 0x1F;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_M_CTRL_REG2;
+ data[1] = 0x20;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_XYZ_DATA_CFG;
+ data[1] = 0x00;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_CTRL_REG1;
+ data[1] = 0x1C;
+ writeRegs(data, 2);
+}
+
+FXOS8700Q::~FXOS8700Q()
+{
+ _i2c = 0;
+ _addr = 0;
+}
+
+void FXOS8700Q::readRegs(uint8_t addr, uint8_t *data, uint32_t len) const
+{
+ uint8_t t[1] = {addr};
+ _i2c->write(_addr, (char *)t, sizeof(t), true);
+ _i2c->read(_addr, (char *)data, len);
+}
+
+uint8_t FXOS8700Q::whoAmI() const
+{
+ uint8_t who_am_i = 0;
+ readRegs(FXOS8700Q_WHOAMI, &who_am_i, sizeof(who_am_i));
+ return who_am_i;
+}
+
+void FXOS8700Q::writeRegs(uint8_t * data, uint32_t len) const
+{
+ _i2c->write(_addr, (char *)data, len);
+}
+
+
+int16_t FXOS8700Q::getSensorAxis(uint8_t addr) const
+{
+ uint8_t res[2];
+ readRegs(addr, res, sizeof(res));
+ return static_cast<int16_t>((res[0] << 8) | res[1]);
+}
+
+void FXOS8700Q::enable(void) const
+{
+ uint8_t data[2];
+ readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
+ data[1] |= 0x01;
+ data[0] = FXOS8700Q_CTRL_REG1;
+ writeRegs(data, sizeof(data));
+}
+
+void FXOS8700Q::disable(void) const
+{
+ uint8_t data[2];
+ readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
+ data[1] &= 0xFE;
+ data[0] = FXOS8700Q_CTRL_REG1;
+ writeRegs(data, sizeof(data));
+}
+
+uint32_t FXOS8700Q::dataReady(void) const
+{
+ uint8_t stat = 0;
+ readRegs(FXOS8700Q_STATUS, &stat, 1);
+ return (uint32_t)stat;
+}
+
+uint32_t FXOS8700Q::sampleRate(uint32_t frequency) const
+{
+ return(50); // for now sample rate is fixed at 50Hz
+}
+
+int16_t FXOS8700QAccelerometer::getX(int16_t &x = dummy_int16_t) const
+{
+ x = getSensorAxis(FXOS8700Q_OUT_X_MSB) >> 2;
+ normalize_14bits(x);
+ return x;
+}
+
+int16_t FXOS8700QAccelerometer::getY(int16_t &y = dummy_int16_t) const
+{
+ y = getSensorAxis(FXOS8700Q_OUT_Y_MSB) >> 2;
+ normalize_14bits(y);
+ return y;
+}
+
+int16_t FXOS8700QAccelerometer::getZ(int16_t &z = dummy_int16_t) const
+{
+ z = getSensorAxis(FXOS8700Q_OUT_Z_MSB) >> 2;
+ normalize_14bits(z);
+ return z;
+}
+
+float FXOS8700QAccelerometer::getX(float &x = dummy_float) const
+{
+ int16_t val = getSensorAxis(FXOS8700Q_OUT_X_MSB) >> 2;
+ normalize_14bits(val);
+ x = val / 4096.0f;
+ return x;
+}
+
+float FXOS8700QAccelerometer::getY(float &y = dummy_float) const
+{
+ int16_t val = getSensorAxis(FXOS8700Q_OUT_Y_MSB) >> 2;
+ normalize_14bits(val);
+ y = val / 4096.0f;
+ return y;
+}
+
+float FXOS8700QAccelerometer::getZ(float &z = dummy_float) const
+{
+ int16_t val = getSensorAxis(FXOS8700Q_OUT_Z_MSB) >> 2;
+ normalize_14bits(val);
+ z = val / 4096.0f;
+ return z;
+}
+
+void FXOS8700QAccelerometer::getAxis(motion_data_counts_t &xyz) const
+{
+ uint8_t res[6];
+ readRegs(FXOS8700Q_OUT_X_MSB, res, sizeof(res));
+ xyz.x = static_cast<int16_t>((res[0] << 8) | res[1]) >> 2;
+ xyz.y = static_cast<int16_t>((res[2] << 8) | res[3]) >> 2;
+ xyz.z = static_cast<int16_t>((res[4] << 8) | res[5]) >> 2;
+ normalize_14bits(xyz.x);
+ normalize_14bits(xyz.y);
+ normalize_14bits(xyz.z);
+}
+
+void FXOS8700QAccelerometer::getAxis(motion_data_units_t &xyz) const
+{
+ motion_data_counts_t _xyz;
+ FXOS8700QAccelerometer::getAxis(_xyz);
+ xyz.x = _xyz.x / 4096.0f;
+ xyz.y = _xyz.y / 4096.0f;
+ xyz.z = _xyz.z / 4096.0f;
+}
+
+int16_t FXOS8700QMagnetometer::getX(int16_t &x = dummy_int16_t) const
+{
+ x = getSensorAxis(FXOS8700Q_M_OUT_X_MSB);
+ return x;
+}
+
+int16_t FXOS8700QMagnetometer::getY(int16_t &y = dummy_int16_t) const
+{
+ y = getSensorAxis(FXOS8700Q_M_OUT_Y_MSB);
+ return y;
+}
+
+int16_t FXOS8700QMagnetometer::getZ(int16_t &z = dummy_int16_t) const
+{
+ z = getSensorAxis(FXOS8700Q_M_OUT_Z_MSB);
+ return z;
+}
+
+float FXOS8700QMagnetometer::getX(float &x = dummy_float) const
+{
+ x = static_cast<float>(getSensorAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f;
+ return x;
+}
+
+float FXOS8700QMagnetometer::getY(float &y = dummy_float) const
+{
+ y = static_cast<float>(getSensorAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f;
+ return y;
+}
+
+float FXOS8700QMagnetometer::getZ(float &z = dummy_float) const
+{
+ z = static_cast<float>(getSensorAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f;
+ return z;
+}
+
+void FXOS8700QMagnetometer::getAxis(motion_data_counts_t &xyz) const
+{
+ uint8_t res[6];
+ readRegs(FXOS8700Q_M_OUT_X_MSB, res, sizeof(res));
+ xyz.x = (res[0] << 8) | res[1];
+ xyz.y = (res[2] << 8) | res[3];
+ xyz.z = (res[4] << 8) | res[5];
+}
+
+void FXOS8700QMagnetometer::getAxis(motion_data_units_t &xyz) const
+{
+ motion_data_counts_t _xyz;
+ FXOS8700QMagnetometer::getAxis(_xyz);
+ xyz.x = static_cast<float>(_xyz.x * 0.1f);
+ xyz.y = static_cast<float>(_xyz.y * 0.1f);
+ xyz.z = static_cast<float>(_xyz.z * 0.1f);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/FXOS8700Q.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,174 @@
+/* FXOS8700Q sensor driver
+ * Copyright (c) 2014-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef FXOS8700Q_H
+#define FXOS8700Q_H
+
+#include "mbed.h"
+#include "MotionSensor.h"
+
+// FXOS8700CQ I2C address
+#define FXOS8700CQ_SLAVE_ADDR0 (0x1E<<1) // with pins SA0=0, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR1 (0x1D<<1) // with pins SA0=1, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR2 (0x1C<<1) // with pins SA0=0, SA1=1
+#define FXOS8700CQ_SLAVE_ADDR3 (0x1F<<1) // with pins SA0=1, SA1=1
+// FXOS8700CQ internal register addresses
+#define FXOS8700Q_STATUS 0x00
+#define FXOS8700Q_OUT_X_MSB 0x01
+#define FXOS8700Q_OUT_Y_MSB 0x03
+#define FXOS8700Q_OUT_Z_MSB 0x05
+#define FXOS8700Q_M_OUT_X_MSB 0x33
+#define FXOS8700Q_M_OUT_Y_MSB 0x35
+#define FXOS8700Q_M_OUT_Z_MSB 0x37
+#define FXOS8700Q_WHOAMI 0x0D
+#define FXOS8700Q_XYZ_DATA_CFG 0x0E
+#define FXOS8700Q_CTRL_REG1 0x2A
+#define FXOS8700Q_M_CTRL_REG1 0x5B
+#define FXOS8700Q_M_CTRL_REG2 0x5C
+#define FXOS8700Q_WHOAMI_VAL 0xC7
+
+
+/** FXOS8700Q accelerometer example
+ @code
+ #include "mbed.h"
+ #include "FXOS8700Q.h"
+ I2C i2c(PTE25, PTE24);
+ FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1); // Configured for the FRDM-K64F with onboard sensors
+ FXOS8700QMagnetometer mag(i2c, FXOS8700CQ_SLAVE_ADDR1);
+ int main(void)
+ {
+ motion_data_units_t acc_data, mag_data;
+ motion_data_counts_t acc_raw, mag_raw;
+ float faX, faY, faZ, fmX, fmY, fmZ, tmp_float;
+ int16_t raX, raY, raZ, rmX, rmY, rmZ, tmp_int;
+ acc.enable();
+ mag.enable();
+ while (true) {
+ // counts based results
+ acc.getAxis(acc_raw);
+ mag.getAxis(mag_raw);
+ acc.getX(raX);
+ acc.getY(raY);
+ acc.getZ(raZ);
+ mag.getX(rmX);
+ mag.getY(rmY);
+ mag.getZ(rmZ);
+ // unit based results
+ acc.getAxis(acc_data);
+ mag.getAxis(mag_data);
+ acc.getX(faX);
+ acc.getY(faY);
+ acc.getZ(faZ);
+ mag.getX(fmX);
+ mag.getY(fmY);
+ mag.getZ(fmZ);
+ wait(0.1f);
+ }
+ }
+ @endcode
+ */
+
+/** FXOS8700Q driver class
+ */
+class FXOS8700Q : public MotionSensor
+{
+public:
+
+ template <typename T> struct vector
+ {
+ T x, y, z;
+ };
+
+ vector<int16_t> a;
+ vector<int16_t> m;
+ vector<int16_t> m_max; // maximum magnetometer values, used for calibration
+ vector<int16_t> m_min; // minimum magnetometer values, used for calibration
+
+ /** Read a device register
+ @param addr The address to read from
+ @param data The data to read from it
+ @param len The amount of data to read from it
+ @return 0 if successful, negative number otherwise
+ */
+ void readRegs(uint8_t addr, uint8_t *data, uint32_t len) const;
+
+ /** Read the ID from a whoAmI register
+ @return The device whoAmI register contents
+ */
+ uint8_t whoAmI(void) const;
+
+ virtual void enable(void) const;
+ virtual void disable(void) const;
+ virtual uint32_t sampleRate(uint32_t frequency) const;
+ virtual uint32_t dataReady(void) const;
+
+protected:
+ I2C *_i2c;
+ uint8_t _addr;
+
+ /** FXOS8700Q constructor
+ @param i2c a configured i2c object
+ @param addr addr of the I2C peripheral as wired
+ */
+ FXOS8700Q(I2C &i2c, uint8_t addr);
+
+ /** FXOS8700Q deconstructor
+ */
+ ~FXOS8700Q();
+
+ void writeRegs(uint8_t *data, uint32_t len) const;
+ int16_t getSensorAxis(uint8_t addr) const;
+};
+
+/** FXOS8700QAccelerometer interface
+ */
+class FXOS8700QAccelerometer : public FXOS8700Q
+{
+public:
+
+ FXOS8700QAccelerometer(I2C &i2c, uint8_t addr) : FXOS8700Q(i2c, addr) {}
+
+ virtual int16_t getX(int16_t &x) const;
+ virtual int16_t getY(int16_t &y) const;
+ virtual int16_t getZ(int16_t &z) const;
+ virtual float getX(float &x) const;
+ virtual float getY(float &y) const;
+ virtual float getZ(float &z) const;
+ virtual void getAxis(motion_data_counts_t &xyz) const;
+ virtual void getAxis(motion_data_units_t &xyz) const;
+
+};
+
+/** FXOS8700QMagnetometer interface
+ */
+class FXOS8700QMagnetometer : public FXOS8700Q
+{
+public:
+
+ FXOS8700QMagnetometer(I2C &i2c, uint8_t addr) : FXOS8700Q(i2c, addr) {}
+
+ virtual int16_t getX(int16_t &x) const;
+ virtual int16_t getY(int16_t &y) const;
+ virtual int16_t getZ(int16_t &z) const;
+ virtual float getX(float &x) const;
+ virtual float getY(float &y) const;
+ virtual float getZ(float &z) const;
+ virtual void getAxis(motion_data_counts_t &xyz) const;
+ virtual void getAxis(motion_data_units_t &xyz) const;
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/MotionSensor.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,112 @@
+/* MotionSensor Base Class
+ * Copyright (c) 2014-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MOTIONSENSOR_H
+#define MOTIONSENSOR_H
+
+#include "stdint.h"
+
+/** motion_data_counts_t struct
+ */
+typedef struct {
+ int16_t x; /*!< x-axis counts */
+ int16_t y; /*!< y-axis counts */
+ int16_t z; /*!< z-axis counts */
+} motion_data_counts_t;
+
+/** motion_data_units_t struct
+ */
+typedef struct {
+ float x; /*!< x-axis counts */
+ float y; /*!< y-axis counts */
+ float z; /*!< z-axis counts */
+} motion_data_units_t;
+
+
+/** Motion Sensor Base Class
+ Useful for accessing data in a common way
+ */
+class MotionSensor
+{
+public:
+
+ /** Enable the sensor for operation
+ */
+ virtual void enable(void) const = 0;
+
+ /** disable the sensors operation
+ */
+ virtual void disable(void) const = 0;
+
+ /** Set the sensor sample rate
+ @param frequency The desires sample frequency
+ @return The amount of error in Hz between desired and actual frequency
+ */
+ virtual uint32_t sampleRate(uint32_t frequency) const = 0;
+
+ /** Tells of new data is ready
+ @return The amount of data samples ready to be read from a device
+ */
+ virtual uint32_t dataReady(void) const = 0;
+
+ /** Get the x data in counts
+ @param x A referene to the variable to put the data in, 0 denotes not used
+ @return The x data in counts
+ */
+ virtual int16_t getX(int16_t &x) const = 0;
+
+ /** Get the y data in counts
+ @param y A referene to the variable to put the data in, 0 denotes not used
+ @return The y data in counts
+ */
+ virtual int16_t getY(int16_t &y) const = 0;
+
+ /** Get the z data in counts
+ @param z A referene to the variable to put the data in, 0 denotes not used
+ @return The z data in counts
+ */
+ virtual int16_t getZ(int16_t &z) const = 0;
+
+ /** Get the x data in units
+ @param x A referene to the variable to put the data in, 0 denotes not used
+ @return The x data in units
+ */
+ virtual float getX(float &x) const = 0;
+
+ /** Get the y data in units
+ @param y A referene to the variable to put the data in, 0 denotes not used
+ @return The y data in units
+ */
+ virtual float getY(float &y) const = 0;
+
+ /** Get the z data in units
+ @param z A referene to the variable to put the data in, 0 denotes not used
+ @return The z data in units
+ */
+ virtual float getZ(float &z) const = 0;
+
+ /** Get the x,y,z data in counts
+ @param xyz A referene to the variable to put the data in, 0 denotes not used
+ */
+ virtual void getAxis(motion_data_counts_t &xyz) const = 0;
+
+ /** Get the x,y,z data in units
+ @param xyz A referene to the variable to put the data in, 0 denotes not used
+ */
+ virtual void getAxis(motion_data_units_t &xyz) const = 0;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/Pushbutton.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,151 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+#include "Pushbutton.h"
+#include "mbed.h"
+#include "millis.h"
+
+// \cond
+
+PushbuttonStateMachine::PushbuttonStateMachine()
+{
+ state = 0;
+}
+
+// state 0: The value is considered to be true.
+// state 1: The value was considered to be true, but there
+// was a recent false reading so it might be falling.
+// state 2: The value is considered to be false.
+// state 3: The value was considered to be false, but there
+// was a recent true reading so it might be rising.
+//
+// The transition from state 3 to state 0 is the point where we
+// have successfully detected a rising edge an we return true.
+//
+// The prevTimeMillis variable holds the last time that we
+// transitioned to states 1 or 3.
+bool PushbuttonStateMachine::getSingleDebouncedRisingEdge(bool value)
+{
+ uint16_t timeMillis = millis();
+
+ switch (state)
+ {
+ case 0:
+ // If value is false, proceed to the next state.
+ if (!value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 1;
+ }
+ break;
+
+ case 1:
+ if (value)
+ {
+ // The value is true or bouncing, so go back to previous (initial)
+ // state.
+ state = 0;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still false, so
+ // proceed to the next state.
+ state = 2;
+ }
+ break;
+
+ case 2:
+ // If the value is true, proceed to the next state.
+ if (value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 3;
+ }
+ break;
+
+ case 3:
+ if (!value)
+ {
+ // The value is false or bouncing, so go back to previous state.
+ state = 2;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still true, so
+ // go back to the initial state and report this rising edge.
+ state = 0;
+ return true;
+ }
+ break;
+ }
+
+ return false;
+}
+
+// \endcond
+
+void PushbuttonBase::waitForPress()
+{
+ do
+ {
+ while (!isPressed()); // wait for button to be pressed
+ wait_us(10000); // debounce the button press
+ }
+ while (!isPressed()); // if button isn't still pressed, loop
+}
+
+void PushbuttonBase::waitForRelease()
+{
+ do
+ {
+ while (isPressed()); // wait for button to be released
+ wait_us(10000); // debounce the button release
+ }
+ while (isPressed()); // if button isn't still released, loop
+}
+
+void PushbuttonBase::waitForButton()
+{
+ waitForPress();
+ waitForRelease();
+}
+
+bool PushbuttonBase::getSingleDebouncedPress()
+{
+ return pressState.getSingleDebouncedRisingEdge(isPressed());
+}
+
+bool PushbuttonBase::getSingleDebouncedRelease()
+{
+ return releaseState.getSingleDebouncedRisingEdge(!isPressed());
+}
+
+Pushbutton::Pushbutton(PinName pin, uint8_t pullUp, uint8_t defaultState)
+{
+ initialized = false;
+ _pin = pin;
+ _pullUp = pullUp;
+ _defaultState = defaultState;
+}
+
+void Pushbutton::init2()
+{
+ if (_pullUp == PULL_UP_ENABLED)
+ {
+ DigitalIn myPin(_pin, PullUp);
+ }
+ else
+ {
+ DigitalIn myPin(_pin);
+ //pinMode(_pin, INPUT); // high impedance
+ }
+
+ wait_us(5); // give pull-up time to stabilize
+}
+
+bool Pushbutton::isPressed()
+{
+ init(); // initialize if needed
+ DigitalIn myPin(_pin);
+
+ return myPin != _defaultState;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/Pushbutton.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,176 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file Pushbutton.h
+ *
+ * This is the main header file for the %Pushbutton library.
+ *
+ * For an overview of the library's features, see
+ * https://github.com/pololu/pushbutton-arduino. That is the main repository
+ * for the library, though copies of the library may exist in other
+ * repositories. */
+
+#pragma once
+
+#include "mbed.h"
+
+/*! Indicates the that pull-up resistor should be disabled. */
+#define PULL_UP_DISABLED 0
+
+/*! Indicates the that pull-up resistor should be enabled. */
+#define PULL_UP_ENABLED 1
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads low. */
+#define DEFAULT_STATE_LOW 0
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads high. */
+#define DEFAULT_STATE_HIGH 1
+
+/*! The pin used for the button on the
+ * [Zumo Shield for Arduino](http://www.pololu.com/product/2508).
+ *
+ * This does not really belong here in this general pushbutton library and will
+ * probably be removed in the future. */
+#define ZUMO_BUTTON PTD3
+
+// \cond
+/** \brief A state machine that detects when a boolean value changes from false
+ * to true, with debouncing.
+ *
+ * This should be considered a private implementation detail of the library.
+ */
+class PushbuttonStateMachine
+{
+public:
+
+ PushbuttonStateMachine();
+
+ /** This should be called repeatedly in a loop. It will return true once after
+ * each time it detects the given value changing from false to true. */
+ bool getSingleDebouncedRisingEdge(bool value);
+
+private:
+
+ uint8_t state;
+ uint16_t prevTimeMillis;
+};
+// \endcond
+
+/*! \brief General pushbutton class that handles debouncing.
+ *
+ * This is an abstract class used for interfacing with pushbuttons. It knows
+ * about debouncing, but it knows nothing about how to read the current state of
+ * the button. The functions in this class get the current state of the button
+ * by calling isPressed(), a virtual function which must be implemented in a
+ * subclass of PushbuttonBase, such as Pushbutton.
+ *
+ * Most users of this library do not need to directly use PushbuttonBase or even
+ * know that it exists. They can use Pushbutton instead.*/
+class PushbuttonBase
+{
+public:
+
+ /*! \brief Waits until the button is pressed and takes care of debouncing.
+ *
+ * This function waits until the button is in the pressed state and then
+ * returns. Note that if the button is already pressed when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForPress();
+
+ /*! \brief Waits until the button is released and takes care of debouncing.
+ *
+ * This function waits until the button is in the released state and then
+ * returns. Note that if the button is already released when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForRelease();
+
+ /*! \brief Waits until the button is pressed and then waits until the button
+ * is released, taking care of debouncing.
+ *
+ * This is equivalent to calling waitForPress() and then waitForRelease(). */
+ void waitForButton();
+
+ /*! \brief Uses a state machine to return true once after each time it detects
+ * the button moving from the released state to the pressed state.
+ *
+ * This is a non-blocking function that is meant to be called repeatedly in a
+ * loop. Each time it is called, it updates a state machine that monitors the
+ * state of the button. When it detects the button changing from the released
+ * state to the pressed state, with debouncing, it returns true. */
+ bool getSingleDebouncedPress();
+
+ /*! \brief Uses a state machine to return true once after each time it detects the button moving from the pressed state to the released state.
+ *
+ * This is just like getSingleDebouncedPress() except it has a separate state
+ * machine and it watches for when the button goes from the pressed state to
+ * the released state.
+ *
+ * There is no strict guarantee that every debounced button press event
+ * returned by getSingleDebouncedPress() will have a corresponding
+ * button release event returned by getSingleDebouncedRelease(); the two
+ * functions use independent state machines and sample the button at different
+ * times. */
+ bool getSingleDebouncedRelease();
+
+ /*! \brief indicates whether button is currently pressed without any debouncing.
+ *
+ * @return 1 if the button is pressed right now, 0 if it is not.
+ *
+ * This function must be implemented in a subclass of PushbuttonBase, such as
+ * Pushbutton. */
+ virtual bool isPressed() = 0;
+
+private:
+
+ PushbuttonStateMachine pressState;
+ PushbuttonStateMachine releaseState;
+};
+
+/** \brief Main class for interfacing with pushbuttons.
+ *
+ * This class can interface with any pushbutton whose state can be read with
+ * the `digitalRead` function, which is part of the Arduino core.
+ *
+ * See https://github.com/pololu/pushbutton-arduino for an overview
+ * of the different ways to use this class. */
+class Pushbutton : public PushbuttonBase
+{
+public:
+
+ /** Constructs a new instance of Pushbutton.
+ *
+ * @param pin The pin number of the pin. This is used as an argument to
+ * `pinMode` and `digitalRead`.
+ *
+ * @param pullUp Specifies whether the pin's internal pull-up resistor should
+ * be enabled. This should be either #PULL_UP_ENABLED (which is the default if
+ * the argument is omitted) or #PULL_UP_DISABLED.
+ *
+ * @param defaultState Specifies the voltage level that corresponds to the
+ * button's default (released) state. This should be either
+ * #DEFAULT_STATE_HIGH (which is the default if this argument is omitted) or
+ * #DEFAULT_STATE_LOW. */
+ Pushbutton(PinName pin, uint8_t pullUp = PULL_UP_ENABLED,
+ uint8_t defaultState = DEFAULT_STATE_HIGH);
+
+ virtual bool isPressed();
+
+private:
+
+ void init()
+ {
+ if (!initialized)
+ {
+ initialized = true;
+ init2();
+ }
+ }
+
+ void init2();
+
+ bool initialized;
+ PinName _pin;
+ bool _pullUp;
+ bool _defaultState;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/ZumoMotors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,84 @@
+/* File: DRV8835.cpp
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+
+#include "mbed.h"
+#include "ZumoMotors.h"
+
+
+ZumoMotors::ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin) :
+pwmL(pinPwmL),
+Lin(pinLin),
+pwmR(pinPwmR),
+Rin(pinRin)
+{
+ Lin = 0;
+ Rin = 0;
+ pwmL.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmL = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ pwmR.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmR = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::stop()
+{
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::motorL_stop(void)
+{
+ pwmL = 0;
+}
+
+void ZumoMotors::motorR_stop(void)
+{
+ pwmR = 0;
+}
+
+void ZumoMotors::setSpeeds(float Left,float Right)
+{
+ //Set Right Speed and Direction
+ if(Right<0)
+ {
+ motorR_rev(Right*-1);
+ } else {
+ motorR_fwd(Right);
+ }
+
+ //Set Left Speed and Direction
+ if(Left<0)
+ {
+ motorL_rev(Left*-1);
+ } else {
+ motorL_fwd(Left);
+ }
+}
+
+void ZumoMotors::motorL_fwd(float fPulsewidth)
+{
+ Lin = 0;
+ pwmL = fPulsewidth;
+}
+void ZumoMotors::motorL_rev(float fPulsewidth)
+{
+ Lin = 1;
+ pwmL = fPulsewidth;
+}
+
+void ZumoMotors::motorR_fwd(float fPulsewidth)
+{
+ Rin = 0;
+ pwmR = fPulsewidth;
+}
+void ZumoMotors::motorR_rev(float fPulsewidth)
+{
+ Rin = 1;
+ pwmR = fPulsewidth;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/ZumoMotors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,38 @@
+/* File: DRV8835.h
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+#ifndef __DRV8835_H__
+#define __DRV8835_H__
+
+#include "mbed.h"
+
+#define DRV8835_PWM_PERIOD_DEFAULT (0.00005) // 2 ms period // 50KHz (0.00002)
+#define DRV8835_PWM_PULSEWIDTH_DEFAULT (0.50) // 50% duty cycle
+
+class ZumoMotors
+{
+public:
+ ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin);
+
+ void motorL_stop(void);
+ void motorL_fwd(float fPulsewidth);
+ void motorL_rev(float fPulsewidth);
+ void motorR_stop(void);
+ void motorR_fwd(float fPulsewidth);
+ void motorR_rev(float fPulsewidth);
+ void setSpeeds(float Left,float Right);
+ void stop(void);
+
+private:
+ PwmOut pwmL;
+ DigitalOut Lin;
+ PwmOut pwmR;
+ DigitalOut Rin;
+};
+
+#endif /* __DRV8835_H__ */
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/compass.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,238 @@
+#include "mbed.h"
+#include "ZumoMotors.h"
+#include "Pushbutton.h"
+#include "FXOS8700Q.h"
+
+/* This example uses the magnetometer in the Zumo Shield's onboard
+ * LSM303DLHC to help the Zumo make precise 90-degree turns and drive
+ * in squares. It uses ZumoMotors, Pushbutton, and the LSM303
+ * (compass) library.
+ *
+ * This program first calibrates the compass to account for offsets in
+ * its output. Calibration is accomplished in setup().
+ *
+ * In loop(), The driving angle then changes its offset by 90 degrees
+ * from the heading every second. Essentially, this navigates the
+ * Zumo to drive in square patterns.
+ *
+ * It is important to note that stray magnetic fields from electric
+ * current (including from the Zumo's own motors) and the environment
+ * (for example, steel rebar in a concrete floor) might adversely
+ * affect readings from the LSM303 compass and make them less
+ * reliable.
+ */
+
+
+#define SPEED 0.5 // Maximum motor speed when going straight; variable speed when turning
+#define TURN_BASE_SPEED 0.25 // Base speed when turning (added to variable speed)
+
+
+#define CALIBRATION_SAMPLES 70 // Number of compass readings to take when calibrating
+#define CRB_REG_M_2_5GAUSS 0x60 // CRB_REG_M value for magnetometer +/-2.5 gauss full scale
+#define CRA_REG_M_220HZ 0x1C // CRA_REG_M value for magnetometer 220 Hz update rate
+
+// Allowed deviation (in degrees) relative to target angle that must be achieved before driving straight
+#define DEVIATION_THRESHOLD 5
+#define PI 3.14159265358979323846
+
+ZumoMotors motors(PTD0, PTC12, PTC4, PTC3);
+Pushbutton button(ZUMO_BUTTON);
+I2C i2c(I2C_SDA, I2C_SCL);
+FXOS8700QMagnetometer compass(i2c, FXOS8700CQ_SLAVE_ADDR1);
+
+// Setup will calibrate our compass by finding maximum/minimum magnetic readings
+void setup()
+{
+ // The highest possible magnetic value to read in any direction is 2047
+ // The lowest possible magnetic value to read in any direction is -2047
+ FXOS8700Q::vector<int16_t> running_min = {32767, 32767, 32767}, running_max = {-32767, -32767, -32767};
+ unsigned char index;
+
+ // Initiate the Wire library and join the I2C bus as a master
+
+ // Initiate LSM303
+ compass.enable();
+
+ // Enables accelerometer and magnetometer
+
+// compass.writeReg(LSM303::CRB_REG_M, CRB_REG_M_2_5GAUSS); // +/- 2.5 gauss sensitivity to hopefully avoid overflow problems
+// compass.writeReg(LSM303::CRA_REG_M, CRA_REG_M_220HZ); // 220 Hz compass update rate
+
+ button.waitForButton();
+
+ printf("starting calibration\n");
+
+ // To calibrate the magnetometer, the Zumo spins to find the max/min
+ // magnetic vectors. This information is used to correct for offsets
+ // in the magnetometer data.
+ motors.setSpeeds(SPEED, -SPEED);
+
+ for(index = 0; index < CALIBRATION_SAMPLES; index ++)
+ {
+ // Take a reading of the magnetic vector and store it in compass.m
+ // compass.read();
+
+ int16_t x, y, z;
+
+ compass.m.x = compass.getX(x);
+ compass.m.y = compass.getY(y);
+ compass.m.z = compass.getZ(z);
+
+ running_min.x = min(running_min.x, compass.m.x);
+ running_min.y = min(running_min.y, compass.m.y);
+
+ running_max.x = max(running_max.x, compass.m.x);
+ running_max.y = max(running_max.y, compass.m.y);
+
+ printf("\n");
+ printf("%d", index);
+
+ wait_us(50000);
+ }
+
+ motors.setSpeeds(0,0);
+
+ printf("max.x ");
+ printf("%d", running_max.x);
+ printf("\n");
+ printf("max.y ");
+ printf("%d", running_max.y);
+ printf("\n");
+ printf("min.x ");
+ printf("%d", running_min.x);
+ printf("\n");
+ printf("min.y ");
+ printf("%d", running_min.y);
+ printf("\n");
+
+ // Set calibrated values to compass.m_max and compass.m_min
+ compass.m_max.x = running_max.x;
+ compass.m_max.y = running_max.y;
+ compass.m_min.x = running_min.x;
+ compass.m_min.y = running_min.y;
+
+ button.waitForButton();
+}
+
+// Converts x and y components of a vector to a heading in degrees.
+// This function is used instead of LSM303::heading() because we don't
+// want the acceleration of the Zumo to factor spuriously into the
+// tilt compensation that LSM303::heading() performs. This calculation
+// assumes that the Zumo is always level.
+template <typename T> float heading(FXOS8700Q::vector<T> v)
+{
+ float x_scaled = 2.0*(float)(v.x - compass.m_min.x) / ( compass.m_max.x - compass.m_min.x) - 1.0;
+ float y_scaled = 2.0*(float)(v.y - compass.m_min.y) / (compass.m_max.y - compass.m_min.y) - 1.0;
+
+ float angle = atan2(y_scaled, x_scaled)*180 / PI;
+ if (angle < 0)
+ angle += 360;
+ return angle;
+}
+
+// Yields the angle difference in degrees between two headings
+float relativeHeading(float heading_from, float heading_to)
+{
+ float relative_heading = heading_to - heading_from;
+
+ // constrain to -180 to 180 degree range
+ if (relative_heading > 180)
+ relative_heading -= 360;
+ if (relative_heading < -180)
+ relative_heading += 360;
+
+ return relative_heading;
+}
+
+// Average 10 vectors to get a better measurement and help smooth out
+// the motors' magnetic interference.
+float averageHeading()
+{
+ FXOS8700Q::vector<int32_t> avg = {0, 0, 0};
+
+ for(int i = 0; i < 10; i ++)
+ {
+ int16_t x, y, z;
+
+ compass.m.x = compass.getX(x);
+ compass.m.y = compass.getY(y);
+ compass.m.z = compass.getZ(z);
+ avg.x += compass.m.x;
+ avg.y += compass.m.y;
+ }
+ avg.x /= 10.0;
+ avg.y /= 10.0;
+
+ // avg is the average measure of the magnetic vector.
+ return heading(avg);
+}
+
+void loop()
+{
+ float heading, relative_heading;
+ float speed;
+ static float target_heading = averageHeading();
+
+ // Heading is given in degrees away from the magnetic vector, increasing clockwise
+ heading = averageHeading();
+
+ // This gives us the relative heading with respect to the target angle
+ relative_heading = relativeHeading(heading, target_heading);
+
+ printf("Target heading: ");
+ printf("%f", target_heading);
+ printf(" Actual heading: ");
+ printf("%f", heading);
+ printf(" Difference: ");
+ printf("%f", relative_heading);
+
+ // If the Zumo has turned to the direction it wants to be pointing, go straight and then do another turn
+ if(abs(relative_heading) < DEVIATION_THRESHOLD)
+ {
+ motors.setSpeeds(SPEED, SPEED);
+
+ printf(" Straight");
+
+ wait_us(1000000);
+
+ // Turn off motors and wait a short time to reduce interference from motors
+ motors.setSpeeds(0, 0);
+ wait_us(100000);
+
+ // Turn 90 degrees relative to the direction we are pointing.
+ // This will help account for variable magnetic field, as opposed
+ // to using fixed increments of 90 degrees from the initial
+ // heading (which might have been measured in a different magnetic
+ // field than the one the Zumo is experiencing now).
+ // Note: fmod() is floating point modulo
+ target_heading = fmod(averageHeading() + 90, 360);
+ }
+ else
+ {
+ // To avoid overshooting, the closer the Zumo gets to the target
+ // heading, the slower it should turn. Set the motor speeds to a
+ // minimum base amount plus an additional variable amount based
+ // on the heading difference.
+
+ speed = SPEED*relative_heading/180;
+
+ if (speed < 0)
+ speed -= TURN_BASE_SPEED;
+ else
+ speed += TURN_BASE_SPEED;
+
+ motors.setSpeeds(-speed, speed);
+
+ printf(" Turn");
+ }
+ printf("\n");
+}
+
+int main(){
+ setup();
+ while(1){
+ loop();
+ }
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/mbed_app.json Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,7 @@
+{
+ "target_overrides": {
+ "*": {
+ "target.printf_lib": "std"
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/Compass/millis.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "millis.h"
+/*
+ millis.cpp
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+volatile unsigned long _millis;
+
+void millisStart(void) {
+ SysTick_Config(SystemCoreClock / 1000);
+}
+
+extern "C" void SysTicks_Handler(void) {
+ _millis++;
+}
+
+unsigned long millis(void) {
+ return _millis;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/Compass/millis.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,25 @@ +#ifndef MILLIS_H +#define MILLIS_H +/* + millis.h + Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com> + All rights reserved. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +void millisStart(void); +unsigned long millis(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/LineFollower.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,168 @@
+#include "PololuBuzzer.h"
+#include "Pushbutton.h"
+#include "ZumoMotors.h"
+#include "ZumoReflectanceSensorArray.h"
+#include "millis.h"
+#include "mbed.h"
+#include<string>
+
+I2C i2c(I2C_SDA, I2C_SCL);
+#define LED PTD1
+
+// this might need to be tuned for different lighting conditions, surfaces, etc.
+#define QTR_THRESHOLD 1500 // microseconds 1500
+
+// these might need to be tuned for different motor types
+
+int sum = 0;
+int countPosition = 0;
+
+
+PololuBuzzer buzzer(PTA1);
+BuzzerValues buzzing;
+
+ZumoMotors motors(PTD0, PTC12, PTC4, PTC3);
+Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12
+// static BufferedSerial pc(USBTX, USBRX);
+
+int lastError = 0;
+
+// This is the maximum speed the motors will be allowed to turn.
+// (400 lets the motors go at top speed; decrease to impose a speed limit)
+const int MAX_SPEED = 1;
+
+
+// #define NUM_SENSORS 6
+// unsigned int sensorValues[NUM_SENSORS];
+
+ZumoReflectanceSensorArray reflectanceSensors;
+DigitalOut ledPin(LED, 1);
+
+void setup()
+{
+ // uncomment if necessary to correct motor directions
+ //motors.flipLeftMotor(true);
+ //motors.flipRightMotor(true);
+// wait_us(500000); //??
+
+ reflectanceSensors.init();
+ button.waitForButton();
+
+ ledPin.write(1);
+
+ buzzing = buzzer.playNote(NOTE_G(4), 200, 15);
+ buzzer.beep(buzzing.returnFrequency(), buzzing.returnDuration(), buzzing.returnVolume());
+
+ wait_us(1000000);
+
+ int i;
+ for(i = 0; i < 80; i++)
+ {
+ if ((i > 10 && i <= 30) || (i > 50 && i <= 70))
+ motors.setSpeeds(-0.5, 0.5);
+ else
+ motors.setSpeeds(0.5, -0.5);
+ reflectanceSensors.calibrate();
+
+ // Since our counter runs to 80, the total delay will be
+ // 80*20 = 1600 ms.
+ wait_us(20000);
+ }
+ motors.setSpeeds(0,0);
+
+ ledPin.write(0);
+
+ buzzing = buzzer.playNote(NOTE_G(4), 500, 15);
+ buzzer.beep(buzzing.returnFrequency(), buzzing.returnDuration(), buzzing.returnVolume());
+
+ button.waitForButton();
+ for (int i = 0; i < 3; i++)
+ {
+ wait_us(1000000);
+ buzzing = buzzer.playNote(NOTE_G(3), 200, 15);
+ buzzer.beep(buzzing.returnFrequency(), buzzing.returnDuration(), buzzing.returnVolume());
+
+ }
+}
+
+void loop()
+{
+ unsigned int sensors[6];
+
+ int position = reflectanceSensors.readLine(sensors);
+ // for (int i = 0; i < 6; i++){
+ // printf("Sensor %d: %d\n", i, sensors[i]);
+ // }
+ // printf("Position %d\n", position);
+ // sum = sum + position;
+ // countPosition++;
+
+ // float avg = (sum/countPosition);
+ // printf("Average: %f\n", avg);
+
+ int error = position - 2500;
+ // if (error > 1500){
+ // error = 2500;
+ // }
+ // else if (error < -1500){
+ // error = -2500;
+ // }
+
+ // printf("Error %d\n", error);
+ int speedDifference = error / 4 + 6 * (error - lastError);
+ // int speedDifference = error / 3 + 7 * (error - lastError);
+
+
+
+ // int speedDifference = error / 4 + 6 * (error - lastError);
+
+ // float speedDifference = error / 3.4 + 6.4 * (error - lastError);
+
+ // float speedDifference = error / 3.4 + 6.38 * (error - lastError);
+
+
+
+
+ // float speedDifference = error / 3.4 + 6.38 * (error - lastError);
+
+ lastError = error;
+
+ int m1Speed = MAX_SPEED + speedDifference;
+ int m2Speed = MAX_SPEED - speedDifference;
+
+ if (m1Speed < 0)
+ m1Speed = 0;
+ if (m2Speed < 0)
+ m2Speed = 0;
+ if (m1Speed > MAX_SPEED)
+ m1Speed = MAX_SPEED;
+ if (m2Speed > MAX_SPEED)
+ m2Speed = MAX_SPEED;
+
+ // if (m1Speed == 0 && m2Speed ==400){
+ // for(int i = 0; i<30; i++){
+ // motors.setSpeeds(m1Speed, m2Speed);
+ // }
+ // }
+ // else if (m1Speed == 400 && m2Speed == 0) {
+ // for(int i = 0; i<30; i++){
+ // motors.setSpeeds(m1Speed, m2Speed);
+ // }
+ // }
+
+
+ // printf("%d", m1Speed);
+ // printf(" %d\n", m2Speed);
+ motors.setSpeeds(m1Speed, m2Speed);
+
+ // if (error == 2500 | error == -2500){
+ // wait_us(10000);
+ // }
+}
+
+int main(){
+ setup();
+ while(1){
+ loop();
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/PololuBuzzer.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,683 @@
+#include "PololuBuzzer.h"
+#include "mbed.h"
+
+using namespace mbed;
+ // constructor
+ /** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+
+BuzzerValues::BuzzerValues(float frequency, float duration, float volume) : freq(frequency), dur(duration), vol(volume) {}
+BuzzerValues::BuzzerValues() {}
+void BuzzerValues::changeValue(float frequency, float duration, float volume) {
+ freq = frequency;
+ dur = duration;
+ vol = volume;
+}
+float BuzzerValues::returnFrequency() {
+ return freq;
+}
+float BuzzerValues::returnDuration() {
+ return dur;
+}
+float BuzzerValues::returnVolume() {
+ return vol;
+}
+
+#define BUZZER_DDR DDRD
+#define BUZZER (1 << PORTD3)
+
+#define TIMER2_CLK_32 0x3 // 500 kHz
+
+static const unsigned int cs2_divider[] = {0, 1, 8, 32, 64, 128, 256, 1024};
+
+#define ENABLE_TIMER_INTERRUPT() TIMSK2 = (1 << TOIE2)
+#define DISABLE_TIMER_INTERRUPT() TIMSK2 = 0
+
+unsigned char buzzerInitialized = 0;
+volatile unsigned char buzzerFinished = 1; // flag: 0 while playing
+const char * volatile buzzerSequence = 0;
+
+// declaring these globals as static means they won't conflict
+// with globals in other .cpp files that share the same name
+static volatile unsigned int buzzerTimeout = 0; // tracks buzzer time limit
+static volatile char play_mode_setting = PLAY_AUTOMATIC;
+
+extern volatile unsigned char buzzerFinished; // flag: 0 while playing
+extern const char * volatile buzzerSequence;
+
+
+static volatile unsigned char use_program_space; // boolean: true if we should
+ // use program space
+
+// music settings and defaults
+static volatile unsigned char octave = 4; // the current octave
+static volatile unsigned int whole_note_duration = 2000; // the duration of a whole note
+static volatile unsigned int note_type = 4; // 4 for quarter, etc
+static volatile unsigned int duration = 500; // the duration of a note in ms
+static volatile unsigned int volume = 15; // the note volume
+static volatile unsigned char staccato = 0; // true if playing staccato
+
+// staccato handling
+static volatile unsigned char staccato_rest_duration; // duration of a staccato rest,
+ // or zero if it is time to play a note
+
+static void nextNote();
+
+PololuBuzzer::PololuBuzzer(PinName pin) : _pwm(pin), playing(0) {
+ _pwm.write(0.0); // after creating it have to be off
+}
+
+ /** stop the beep instantaneous
+ * usually not used
+ */
+void PololuBuzzer::nobeep() {
+ _pwm.write(0.0);
+ buzzerFinished = 1;
+ buzzerSequence = 0;
+ if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+ nextNote();
+ else
+ isPlaying2(0);
+}
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+
+bool PololuBuzzer::isPlaying2(int request){
+ if (request == 0) {
+ playing = false;
+ }
+ else if (request == 1){
+ playing = true;
+ }
+ return playing;
+}
+
+void PololuBuzzer::beep(float freq, float time, float volume) {
+ _pwm.period(1.0/freq);
+ _pwm.write(0.5); // 50% duty cycle - beep on
+ float newTime = (time/500);
+ toff.attach(callback(this, &PololuBuzzer::nobeep), newTime);
+}
+
+// Timer2 overflow interrupt
+// ISR (TIMER2_OVF_vect)
+// {
+// if (buzzerTimeout-- == 0)
+// {
+// DISABLE_TIMER_INTERRUPT();
+// sei(); // re-enable global interrupts (nextNote() is very slow)
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+// buzzerFinished = 1;
+// if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+// nextNote();
+// }
+// }
+
+
+// this is called by playFrequency()
+inline void PololuBuzzer::init()
+{
+ if (!buzzerInitialized)
+ {
+ buzzerInitialized = 1;
+ //init2();
+ }
+}
+
+// initializes timer4 (32U4) or timer2 (328P) for buzzer control
+// void PololuBuzzer::init2()
+// {
+// DISABLE_TIMER_INTERRUPT();
+
+// TCCR2A = 0x21; // bits 7 and 6 clear: normal port op., OC4A disconnected
+// // bit 5 set, 4 clear: clear OC2B on comp match when upcounting,
+// // set OC2B on comp match when downcounting
+// // bits 3 and 2: not used
+// // bit 1 clear, 0 set: combine with bit 3 of TCCR2B...
+
+// TCCR2B = 0x0B; // bit 7 clear: no force output compare for channel A
+// // bit 6 clear: no force output compare for channel B
+// // bits 5 and 4: not used
+// // bit 3 set: combine with bits 1 and 0 of TCCR2A to
+// // select waveform generation mode 5, phase-correct PWM,
+// // TOP = OCR2A, OCR2B set at TOP, TOV2 flag set at BOTTOM
+// // bit 2 clear, 1-0 set: timer clock = clkT2S/32
+
+ // This sets timer 2 to run in phase-correct PWM mode, where TOP = OCR2A,
+ // OCR2B is updated at TOP, TOV2 Flag is set on BOTTOM. OC2B is cleared
+ // on compare match when upcounting, set on compare match when downcounting;
+ // OC2A is disconnected.
+ // Note: if the PWM frequency and duty cycle are changed, the first
+ // cycle of the new frequency will be at the old duty cycle, since
+ // the duty cycle (OCR2B) is not updated until TOP.
+
+
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// BUZZER_DDR |= BUZZER; // buzzer pin set as an output
+// sei();
+// }
+
+
+// Set up the timer to play the desired frequency (in Hz or .1 Hz) for the
+// the desired duration (in ms). Allowed frequencies are 40 Hz to 10 kHz.
+// volume controls buzzer volume, with 15 being loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playFrequency(unsigned int freq, unsigned int dur,
+ unsigned char volume)
+{
+ init(); // initializes the buzzer if necessary
+ buzzerFinished = 0;
+
+ unsigned int timeout;
+ unsigned char multiplier = 1;
+
+ if (freq & DIV_BY_10) // if frequency's DIV_BY_10 bit is set
+ { // then the true frequency is freq/10
+ multiplier = 10; // (gives higher resolution for small freqs)
+ freq &= ~DIV_BY_10; // clear DIV_BY_10 bit
+ }
+
+ unsigned char min = 40 * multiplier;
+ if (freq < min) // min frequency allowed is 40 Hz
+ freq = min;
+ if (multiplier == 1 && freq > 10000)
+ freq = 10000; // max frequency allowed is 10kHz
+
+// unsigned int top;
+// unsigned char newCS2 = 2; // try prescaler divider of 8 first (minimum necessary for 10 kHz)
+// unsigned int divider = cs2_divider[newCS2];
+
+// // calculate necessary clock source and counter top value to get freq
+// top = (unsigned int)(((F_CPU/16 * multiplier) + (freq >> 1))/ freq);
+
+// while (top > 255)
+// {
+// divider = cs2_divider[++newCS2];
+// top = (unsigned int)(((F_CPU/2/divider * multiplier) + (freq >> 1))/ freq);
+// }
+
+ // set timeout (duration):
+ if (multiplier == 10)
+ freq = (freq + 5) / 10;
+
+ if (freq == 1000)
+ timeout = dur; // duration for silent notes is exact
+ else
+ timeout = (unsigned int)((long)dur * freq / 1000);
+
+ if (volume > 15)
+ volume = 15;
+
+ BuzzerValues buzz(freq,timeout,volume);
+ return buzz;
+
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts while writing to registers
+
+// TCCR2B = (TCCR2B & 0xF8) | newCS2; // select timer 2 clock prescaler
+// OCR2A = top; // set timer 2 pwm frequency
+// OCR2B = top >> (16 - volume); // set duty cycle (volume)
+// buzzerTimeout = timeout; // set buzzer duration
+
+// TIFR2 |= 0xFF; // clear any pending t2 overflow int.
+
+// ENABLE_TIMER_INTERRUPT();
+}
+
+
+
+// Determine the frequency for the specified note, then play that note
+// for the desired duration (in ms). This is done without using floats
+// and without having to loop. volume controls buzzer volume, with 15 being
+// loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playNote(unsigned char note, unsigned int dur,
+ unsigned char volume)
+{
+ // note = key + octave * 12, where 0 <= key < 12
+ // example: A4 = A + 4 * 12, where A = 9 (so A4 = 57)
+ // A note is converted to a frequency by the formula:
+ // Freq(n) = Freq(0) * a^n
+ // where
+ // Freq(0) is chosen as A4, which is 440 Hz
+ // and
+ // a = 2 ^ (1/12)
+ // n is the number of notes you are away from A4.
+ // One can see that the frequency will double every 12 notes.
+ // This function exploits this property by defining the frequencies of the
+ // 12 lowest notes allowed and then doubling the appropriate frequency
+ // the appropriate number of times to get the frequency for the specified
+ // note.
+
+ // if note = 16, freq = 41.2 Hz (E1 - lower limit as freq must be >40 Hz)
+ // if note = 57, freq = 440 Hz (A4 - central value of ET Scale)
+ // if note = 111, freq = 9.96 kHz (D#9 - upper limit, freq must be <10 kHz)
+ // if note = 255, freq = 1 kHz and buzzer is silent (silent note)
+
+ // The most significant bit of freq is the "divide by 10" bit. If set,
+ // the units for frequency are .1 Hz, not Hz, and freq must be divided
+ // by 10 to get the true frequency in Hz. This allows for an extra digit
+ // of resolution for low frequencies without the need for using floats.
+
+ unsigned int freq = 0;
+ unsigned char offset_note = note - 16;
+ BuzzerValues buzz;
+
+ if (note == SILENT_NOTE || volume == 0)
+ {
+ freq = 1000; // silent notes => use 1kHz freq (for cycle counter)
+ buzz.changeValue(freq,dur,volume);
+ // buzz = playFrequency(freq, dur, 0);
+ // return buzz;
+ }
+ else {
+ if (note <= 16)
+ offset_note = 0;
+ else if (offset_note > 95)
+ offset_note = 95;
+
+ unsigned char exponent = offset_note / 12;
+
+ // frequency table for the lowest 12 allowed notes
+ // frequencies are specified in tenths of a Hertz for added resolution
+ switch (offset_note - exponent * 12) // equivalent to (offset_note % 12)
+ {
+ case 0: // note E1 = 41.2 Hz
+ freq = 412;
+ break;
+ case 1: // note F1 = 43.7 Hz
+ freq = 437;
+ break;
+ case 2: // note F#1 = 46.3 Hz
+ freq = 463;
+ break;
+ case 3: // note G1 = 49.0 Hz
+ freq = 490;
+ break;
+ case 4: // note G#1 = 51.9 Hz
+ freq = 519;
+ break;
+ case 5: // note A1 = 55.0 Hz
+ freq = 550;
+ break;
+ case 6: // note A#1 = 58.3 Hz
+ freq = 583;
+ break;
+ case 7: // note B1 = 61.7 Hz
+ freq = 617;
+ break;
+ case 8: // note C2 = 65.4 Hz
+ freq = 654;
+ break;
+ case 9: // note C#2 = 69.3 Hz
+ freq = 693;
+ break;
+ case 10: // note D2 = 73.4 Hz
+ freq = 734;
+ break;
+ case 11: // note D#2 = 77.8 Hz
+ freq = 778;
+ break;
+ }
+ if (exponent < 7)
+ {
+ freq = freq << exponent; // frequency *= 2 ^ exponent
+ if (exponent > 1) // if the frequency is greater than 160 Hz
+ freq = (freq + 5) / 10; // we don't need the extra resolution
+ else
+ freq += DIV_BY_10; // else keep the added digit of resolution
+ }
+ else
+ freq = (freq * 64 + 2) / 5; // == freq * 2^7 / 10 without int overflow
+
+ if (volume > 15)
+ volume = 15;
+ buzz = playFrequency(freq, dur, volume); // set buzzer this freq/duration
+ }
+ return buzz;
+}
+
+
+
+// Returns 1 if the buzzer is currently playing, otherwise it returns 0
+unsigned char PololuBuzzer::isPlaying()
+{
+ return !buzzerFinished || buzzerSequence != 0;
+}
+
+
+// Plays the specified sequence of notes. If the play mode is
+// PLAY_AUTOMATIC, the sequence of notes will play with no further
+// action required by the user. If the play mode is PLAY_CHECK,
+// the user will need to call playCheck() in the main loop to initiate
+// the playing of each new note in the sequence. The play mode can
+// be changed while the sequence is playing.
+// This is modeled after the PLAY commands in GW-BASIC, with just a
+// few differences.
+//
+// The notes are specified by the characters C, D, E, F, G, A, and
+// B, and they are played by default as "quarter notes" with a
+// length of 500 ms. This corresponds to a tempo of 120
+// beats/min. Other durations can be specified by putting a number
+// immediately after the note. For example, C8 specifies C played as an
+// eighth note, with half the duration of a quarter note. The special
+// note R plays a rest (no sound).
+//
+// Various control characters alter the sound:
+// '>' plays the next note one octave higher
+//
+// '<' plays the next note one octave lower
+//
+// '+' or '#' after a note raises any note one half-step
+//
+// '-' after a note lowers any note one half-step
+//
+// '.' after a note "dots" it, increasing the length by
+// 50%. Each additional dot adds half as much as the
+// previous dot, so that "A.." is 1.75 times the length of
+// "A".
+//
+// 'O' followed by a number sets the octave (default: O4).
+//
+// 'T' followed by a number sets the tempo (default: T120).
+//
+// 'L' followed by a number sets the default note duration to
+// the type specified by the number: 4 for quarter notes, 8
+// for eighth notes, 16 for sixteenth notes, etc.
+// (default: L4)
+//
+// 'V' followed by a number from 0-15 sets the music volume.
+// (default: V15)
+//
+// 'MS' sets all subsequent notes to play staccato - each note is played
+// for 1/2 of its allotted time, followed by an equal period
+// of silence.
+//
+// 'ML' sets all subsequent notes to play legato - each note is played
+// for its full length. This is the default setting.
+//
+// '!' resets all persistent settings to their defaults.
+//
+// The following plays a c major scale up and back down:
+// play("L16 V8 cdefgab>cbagfedc");
+//
+// Here is an example from Bach:
+// play("T240 L8 a gafaeada c+adaeafa <aa<bac#ada c#adaeaf4");
+void PololuBuzzer::play(const char *notes)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes;
+ use_program_space = 0;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+void PololuBuzzer::playFromProgramSpace(const char *notes_p)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes_p;
+ use_program_space = 1;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+
+// // stop all sound playback immediately
+// void PololuBuzzer::stopPlaying()
+// {
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts
+
+
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// buzzerFinished = 1;
+// buzzerSequence = 0;
+// }
+
+// Gets the current character, converting to lower-case and skipping
+// spaces. For any spaces, this automatically increments sequence!
+static char currentCharacter()
+{
+ char c = 0;
+ do
+ {
+ // if(use_program_space)
+ // c = pgm_read_byte(buzzerSequence);
+ // else
+ c = *buzzerSequence;
+
+ if(c >= 'A' && c <= 'Z')
+ c += 'a'-'A';
+ } while(c == ' ' && (buzzerSequence ++));
+
+ return c;
+}
+
+// Returns the numerical argument specified at buzzerSequence[0] and
+// increments sequence to point to the character immediately after the
+// argument.
+static unsigned int getNumber()
+{
+ unsigned int arg = 0;
+
+ // read all digits, one at a time
+ char c = currentCharacter();
+ while(c >= '0' && c <= '9')
+ {
+ arg *= 10;
+ arg += c-'0';
+ buzzerSequence ++;
+ c = currentCharacter();
+ }
+
+ return arg;
+}
+
+static void nextNote()
+{
+ unsigned char note = 0;
+ unsigned char rest = 0;
+ unsigned char tmp_octave = octave; // the octave for this note
+ unsigned int tmp_duration; // the duration of this note
+ unsigned int dot_add;
+
+ char c; // temporary variable
+
+ // if we are playing staccato, after every note we play a rest
+ if(staccato && staccato_rest_duration)
+ {
+ PololuBuzzer::playNote(SILENT_NOTE, staccato_rest_duration, 0);
+ staccato_rest_duration = 0;
+ return;
+ }
+
+ parse_character:
+
+ // Get current character
+ c = currentCharacter();
+ buzzerSequence ++;
+
+ // Interpret the character.
+ switch(c)
+ {
+ case '>':
+ // shift the octave temporarily up
+ tmp_octave ++;
+ goto parse_character;
+ case '<':
+ // shift the octave temporarily down
+ tmp_octave --;
+ goto parse_character;
+ case 'a':
+ note = NOTE_A(0);
+ break;
+ case 'b':
+ note = NOTE_B(0);
+ break;
+ case 'c':
+ note = NOTE_C(0);
+ break;
+ case 'd':
+ note = NOTE_D(0);
+ break;
+ case 'e':
+ note = NOTE_E(0);
+ break;
+ case 'f':
+ note = NOTE_F(0);
+ break;
+ case 'g':
+ note = NOTE_G(0);
+ break;
+ case 'l':
+ // set the default note duration
+ note_type = getNumber();
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'm':
+ // set music staccato or legato
+ if(currentCharacter() == 'l')
+ staccato = false;
+ else
+ {
+ staccato = true;
+ staccato_rest_duration = 0;
+ }
+ buzzerSequence ++;
+ goto parse_character;
+ case 'o':
+ // set the octave permanently
+ octave = tmp_octave = getNumber();
+ goto parse_character;
+ case 'r':
+ // Rest - the note value doesn't matter.
+ rest = 1;
+ break;
+ case 't':
+ // set the tempo
+ whole_note_duration = 60*400/getNumber()*10;
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'v':
+ // set the volume
+ volume = getNumber();
+ goto parse_character;
+ case '!':
+ // reset to defaults
+ octave = 4;
+ whole_note_duration = 2000;
+ note_type = 4;
+ duration = 500;
+ volume = 15;
+ staccato = 0;
+ // reset temp variables that depend on the defaults
+ tmp_octave = octave;
+ tmp_duration = duration;
+ goto parse_character;
+ default:
+ buzzerSequence = 0;
+ return;
+ }
+
+ note += tmp_octave*12;
+
+ // handle sharps and flats
+ c = currentCharacter();
+ while(c == '+' || c == '#')
+ {
+ buzzerSequence ++;
+ note ++;
+ c = currentCharacter();
+ }
+ while(c == '-')
+ {
+ buzzerSequence ++;
+ note --;
+ c = currentCharacter();
+ }
+
+ // set the duration of just this note
+ tmp_duration = duration;
+
+ // If the input is 'c16', make it a 16th note, etc.
+ if(c > '0' && c < '9')
+ tmp_duration = whole_note_duration/getNumber();
+
+ // Handle dotted notes - the first dot adds 50%, and each
+ // additional dot adds 50% of the previous dot.
+ dot_add = tmp_duration/2;
+ while(currentCharacter() == '.')
+ {
+ buzzerSequence ++;
+ tmp_duration += dot_add;
+ dot_add /= 2;
+ }
+
+ if(staccato)
+ {
+ staccato_rest_duration = tmp_duration / 2;
+ tmp_duration -= staccato_rest_duration;
+ }
+
+ // this will re-enable the timer overflow interrupt
+ PololuBuzzer::playNote(rest ? SILENT_NOTE : note, tmp_duration, volume);
+// wait_ms(tmp_duration);
+}
+
+
+// This puts play() into a mode where instead of advancing to the
+// next note in the sequence automatically, it waits until the
+// function playCheck() is called. The idea is that you can
+// put playCheck() in your main loop and avoid potential
+// delays due to the note sequence being checked in the middle of
+// a time sensitive calculation. It is recommended that you use
+// this function if you are doing anything that can't tolerate
+// being interrupted for more than a few microseconds.
+// Note that the play mode can be changed while a sequence is being
+// played.
+//
+// Usage: playMode(PLAY_AUTOMATIC) makes it automatic (the
+// default), playMode(PLAY_CHECK) sets it to a mode where you have
+// to call playCheck().
+void PololuBuzzer::playMode(unsigned char mode)
+{
+ play_mode_setting = mode;
+
+ // We want to check to make sure that we didn't miss a note if we
+ // are going out of play-check mode.
+ if(mode == PLAY_AUTOMATIC)
+ playCheck();
+}
+
+
+// Checks whether it is time to start another note, and starts
+// it if so. If it is not yet time to start the next note, this method
+// returns without doing anything. Call this as often as possible
+// in your main loop to avoid delays between notes in the sequence.
+//
+// Returns true if it is still playing.
+unsigned char PololuBuzzer::playCheck()
+{
+ if(buzzerFinished && buzzerSequence != 0)
+ nextNote();
+ return buzzerSequence != 0;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/PololuBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,126 @@
+#pragma once
+#include "mbed.h"
+
+#ifndef MBED_BEEP_H
+#define MBED_BEEP_H
+
+/*! \brief Specifies that the sequence of notes will play with no further action
+ * required by the user. */
+#define PLAY_AUTOMATIC 0
+
+
+/*! \brief Specified that the user will need to call `playCheck()` regularly. */
+#define PLAY_CHECK 1
+
+// n
+// Equal Tempered Scale is given by f = f * a
+// n o
+//
+// where f is chosen as A above middle C (A4) at f = 440 Hz
+// o o
+// and a is given by the twelfth root of 2 (~1.059463094359)
+
+/*! \anchor note_macros
+ *
+ * \name Note Macros
+ * \a x specifies the octave of the note
+ * @{
+ */
+#define NOTE_C(x) ( 0 + (x)*12)
+#define NOTE_C_SHARP(x) ( 1 + (x)*12)
+#define NOTE_D_FLAT(x) ( 1 + (x)*12)
+#define NOTE_D(x) ( 2 + (x)*12)
+#define NOTE_D_SHARP(x) ( 3 + (x)*12)
+#define NOTE_E_FLAT(x) ( 3 + (x)*12)
+#define NOTE_E(x) ( 4 + (x)*12)
+#define NOTE_F(x) ( 5 + (x)*12)
+#define NOTE_F_SHARP(x) ( 6 + (x)*12)
+#define NOTE_G_FLAT(x) ( 6 + (x)*12)
+#define NOTE_G(x) ( 7 + (x)*12)
+#define NOTE_G_SHARP(x) ( 8 + (x)*12)
+#define NOTE_A_FLAT(x) ( 8 + (x)*12)
+#define NOTE_A(x) ( 9 + (x)*12)
+#define NOTE_A_SHARP(x) (10 + (x)*12)
+#define NOTE_B_FLAT(x) (10 + (x)*12)
+#define NOTE_B(x) (11 + (x)*12)
+
+/*! \brief silences buzzer for the note duration */
+#define SILENT_NOTE 0xFF
+
+/*! \brief frequency bit that indicates Hz/10<br>
+ * e.g. \a frequency = `(445 | DIV_BY_10)` gives a frequency of 44.5 Hz
+ */
+#define DIV_BY_10 (1 << 15)
+/*! @} */
+
+namespace mbed {
+
+class BuzzerValues{
+ private:
+ float freq;
+ float dur;
+ float vol;
+ public:
+ BuzzerValues(float frequency, float duration, float volume);
+ BuzzerValues();
+ void changeValue(float frequency, float duration, float volume);
+ float returnFrequency();
+ float returnDuration();
+ float returnVolume();
+};
+
+/* Class: PololuBuzzer
+ * A class which uses pwm to controle a beeper to generate sounds.
+ */
+class PololuBuzzer {
+private:
+ PwmOut _pwm;
+ Timeout toff;
+ bool playing;
+
+public:
+
+/** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+ PololuBuzzer (PinName pin);
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+ void beep (float frequency, float time, float volume);
+
+/** stop the beep instantaneous
+ * usually not used
+ */
+ void nobeep();
+
+ static BuzzerValues playFrequency(unsigned int freq, unsigned int duration,
+ unsigned char volume);
+
+ static BuzzerValues playNote(unsigned char note, unsigned int duration,
+ unsigned char volume);
+
+ static void play(const char *sequence);
+
+ static void playFromProgramSpace(const char *sequence);
+
+ static void playMode(unsigned char mode);
+
+ static unsigned char playCheck();
+
+ static unsigned char isPlaying();
+
+ bool isPlaying2(int request);
+
+ //static void stopPlaying();
+
+ // initializes timer for buzzer control
+// static void init2();
+ static void init();
+ };
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/Pushbutton.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,151 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+#include "Pushbutton.h"
+#include "mbed.h"
+#include "millis.h"
+
+// \cond
+
+PushbuttonStateMachine::PushbuttonStateMachine()
+{
+ state = 0;
+}
+
+// state 0: The value is considered to be true.
+// state 1: The value was considered to be true, but there
+// was a recent false reading so it might be falling.
+// state 2: The value is considered to be false.
+// state 3: The value was considered to be false, but there
+// was a recent true reading so it might be rising.
+//
+// The transition from state 3 to state 0 is the point where we
+// have successfully detected a rising edge an we return true.
+//
+// The prevTimeMillis variable holds the last time that we
+// transitioned to states 1 or 3.
+bool PushbuttonStateMachine::getSingleDebouncedRisingEdge(bool value)
+{
+ uint16_t timeMillis = millis();
+
+ switch (state)
+ {
+ case 0:
+ // If value is false, proceed to the next state.
+ if (!value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 1;
+ }
+ break;
+
+ case 1:
+ if (value)
+ {
+ // The value is true or bouncing, so go back to previous (initial)
+ // state.
+ state = 0;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still false, so
+ // proceed to the next state.
+ state = 2;
+ }
+ break;
+
+ case 2:
+ // If the value is true, proceed to the next state.
+ if (value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 3;
+ }
+ break;
+
+ case 3:
+ if (!value)
+ {
+ // The value is false or bouncing, so go back to previous state.
+ state = 2;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still true, so
+ // go back to the initial state and report this rising edge.
+ state = 0;
+ return true;
+ }
+ break;
+ }
+
+ return false;
+}
+
+// \endcond
+
+void PushbuttonBase::waitForPress()
+{
+ do
+ {
+ while (!isPressed()); // wait for button to be pressed
+ wait_us(10000); // debounce the button press
+ }
+ while (!isPressed()); // if button isn't still pressed, loop
+}
+
+void PushbuttonBase::waitForRelease()
+{
+ do
+ {
+ while (isPressed()); // wait for button to be released
+ wait_us(10000); // debounce the button release
+ }
+ while (isPressed()); // if button isn't still released, loop
+}
+
+void PushbuttonBase::waitForButton()
+{
+ waitForPress();
+ waitForRelease();
+}
+
+bool PushbuttonBase::getSingleDebouncedPress()
+{
+ return pressState.getSingleDebouncedRisingEdge(isPressed());
+}
+
+bool PushbuttonBase::getSingleDebouncedRelease()
+{
+ return releaseState.getSingleDebouncedRisingEdge(!isPressed());
+}
+
+Pushbutton::Pushbutton(PinName pin, uint8_t pullUp, uint8_t defaultState)
+{
+ initialized = false;
+ _pin = pin;
+ _pullUp = pullUp;
+ _defaultState = defaultState;
+}
+
+void Pushbutton::init2()
+{
+ if (_pullUp == PULL_UP_ENABLED)
+ {
+ DigitalIn myPin(_pin, PullUp);
+ }
+ else
+ {
+ DigitalIn myPin(_pin);
+ //pinMode(_pin, INPUT); // high impedance
+ }
+
+ wait_us(5); // give pull-up time to stabilize
+}
+
+bool Pushbutton::isPressed()
+{
+ init(); // initialize if needed
+ DigitalIn myPin(_pin);
+
+ return myPin != _defaultState;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/Pushbutton.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,176 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file Pushbutton.h
+ *
+ * This is the main header file for the %Pushbutton library.
+ *
+ * For an overview of the library's features, see
+ * https://github.com/pololu/pushbutton-arduino. That is the main repository
+ * for the library, though copies of the library may exist in other
+ * repositories. */
+
+#pragma once
+
+#include "mbed.h"
+
+/*! Indicates the that pull-up resistor should be disabled. */
+#define PULL_UP_DISABLED 0
+
+/*! Indicates the that pull-up resistor should be enabled. */
+#define PULL_UP_ENABLED 1
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads low. */
+#define DEFAULT_STATE_LOW 0
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads high. */
+#define DEFAULT_STATE_HIGH 1
+
+/*! The pin used for the button on the
+ * [Zumo Shield for Arduino](http://www.pololu.com/product/2508).
+ *
+ * This does not really belong here in this general pushbutton library and will
+ * probably be removed in the future. */
+#define ZUMO_BUTTON PTD3
+
+// \cond
+/** \brief A state machine that detects when a boolean value changes from false
+ * to true, with debouncing.
+ *
+ * This should be considered a private implementation detail of the library.
+ */
+class PushbuttonStateMachine
+{
+public:
+
+ PushbuttonStateMachine();
+
+ /** This should be called repeatedly in a loop. It will return true once after
+ * each time it detects the given value changing from false to true. */
+ bool getSingleDebouncedRisingEdge(bool value);
+
+private:
+
+ uint8_t state;
+ uint16_t prevTimeMillis;
+};
+// \endcond
+
+/*! \brief General pushbutton class that handles debouncing.
+ *
+ * This is an abstract class used for interfacing with pushbuttons. It knows
+ * about debouncing, but it knows nothing about how to read the current state of
+ * the button. The functions in this class get the current state of the button
+ * by calling isPressed(), a virtual function which must be implemented in a
+ * subclass of PushbuttonBase, such as Pushbutton.
+ *
+ * Most users of this library do not need to directly use PushbuttonBase or even
+ * know that it exists. They can use Pushbutton instead.*/
+class PushbuttonBase
+{
+public:
+
+ /*! \brief Waits until the button is pressed and takes care of debouncing.
+ *
+ * This function waits until the button is in the pressed state and then
+ * returns. Note that if the button is already pressed when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForPress();
+
+ /*! \brief Waits until the button is released and takes care of debouncing.
+ *
+ * This function waits until the button is in the released state and then
+ * returns. Note that if the button is already released when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForRelease();
+
+ /*! \brief Waits until the button is pressed and then waits until the button
+ * is released, taking care of debouncing.
+ *
+ * This is equivalent to calling waitForPress() and then waitForRelease(). */
+ void waitForButton();
+
+ /*! \brief Uses a state machine to return true once after each time it detects
+ * the button moving from the released state to the pressed state.
+ *
+ * This is a non-blocking function that is meant to be called repeatedly in a
+ * loop. Each time it is called, it updates a state machine that monitors the
+ * state of the button. When it detects the button changing from the released
+ * state to the pressed state, with debouncing, it returns true. */
+ bool getSingleDebouncedPress();
+
+ /*! \brief Uses a state machine to return true once after each time it detects the button moving from the pressed state to the released state.
+ *
+ * This is just like getSingleDebouncedPress() except it has a separate state
+ * machine and it watches for when the button goes from the pressed state to
+ * the released state.
+ *
+ * There is no strict guarantee that every debounced button press event
+ * returned by getSingleDebouncedPress() will have a corresponding
+ * button release event returned by getSingleDebouncedRelease(); the two
+ * functions use independent state machines and sample the button at different
+ * times. */
+ bool getSingleDebouncedRelease();
+
+ /*! \brief indicates whether button is currently pressed without any debouncing.
+ *
+ * @return 1 if the button is pressed right now, 0 if it is not.
+ *
+ * This function must be implemented in a subclass of PushbuttonBase, such as
+ * Pushbutton. */
+ virtual bool isPressed() = 0;
+
+private:
+
+ PushbuttonStateMachine pressState;
+ PushbuttonStateMachine releaseState;
+};
+
+/** \brief Main class for interfacing with pushbuttons.
+ *
+ * This class can interface with any pushbutton whose state can be read with
+ * the `digitalRead` function, which is part of the Arduino core.
+ *
+ * See https://github.com/pololu/pushbutton-arduino for an overview
+ * of the different ways to use this class. */
+class Pushbutton : public PushbuttonBase
+{
+public:
+
+ /** Constructs a new instance of Pushbutton.
+ *
+ * @param pin The pin number of the pin. This is used as an argument to
+ * `pinMode` and `digitalRead`.
+ *
+ * @param pullUp Specifies whether the pin's internal pull-up resistor should
+ * be enabled. This should be either #PULL_UP_ENABLED (which is the default if
+ * the argument is omitted) or #PULL_UP_DISABLED.
+ *
+ * @param defaultState Specifies the voltage level that corresponds to the
+ * button's default (released) state. This should be either
+ * #DEFAULT_STATE_HIGH (which is the default if this argument is omitted) or
+ * #DEFAULT_STATE_LOW. */
+ Pushbutton(PinName pin, uint8_t pullUp = PULL_UP_ENABLED,
+ uint8_t defaultState = DEFAULT_STATE_HIGH);
+
+ virtual bool isPressed();
+
+private:
+
+ void init()
+ {
+ if (!initialized)
+ {
+ initialized = true;
+ init2();
+ }
+ }
+
+ void init2();
+
+ bool initialized;
+ PinName _pin;
+ bool _pullUp;
+ bool _defaultState;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/QTRSensors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,813 @@
+/*
+ QTRSensors.cpp - Arduino library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010.
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+
+#include "QTRSensors.h"
+#include <stdlib.h>
+#include "mbed.h"
+#include<string>
+
+Timer timer;
+// static BufferedSerial serial_port(USBTX, USBRX);
+
+// Base class data member initialization (called by derived class init())
+void QTRSensors::init(unsigned char *pins, unsigned char numSensors,
+ unsigned char emitterPin, bool analog = false)
+{
+ calibratedMinimumOn=0;
+ calibratedMaximumOn=0;
+ calibratedMinimumOff=0;
+ calibratedMaximumOff=0;
+
+ _lastValue=0;
+
+ if (numSensors > QTR_MAX_SENSORS)
+ _numSensors = QTR_MAX_SENSORS;
+ else
+ _numSensors = numSensors;
+
+
+ if (_pins == 0) {
+ _pins = (PinName *)malloc(sizeof(PinName)*_numSensors);
+ if (_pins == 0)
+ return;
+ }
+
+ unsigned char i;
+ // Copy parameter values to local storage
+ PinName currentPin;
+ for (i = 0; i < _numSensors; i++) {
+ switch(pins[i]){
+ case 1:
+ currentPin = PTB23;
+ break;
+ case 2:
+ currentPin = PTB11;
+ break;
+ case 3:
+ currentPin = PTD2;
+ break;
+ case 4:
+ currentPin = PTB2;
+ break;
+ case 5:
+ currentPin = PTB10;
+ break;
+ case 6:
+ currentPin = PTA2;
+ break;
+ }
+ _pins[i] = currentPin;
+ }
+
+ _qtrPins.reserve(_numSensors);
+
+ for (int i = 0; i < _numSensors; i++) {
+ _qtrPins.push_back(new DigitalInOut(_pins[i]));
+ }
+
+ // Serial pc(USBTX, USBRX);
+ // unsigned char j;
+ // for (j = 0; j < numSensors; j++)
+ // {
+ // std::string s = std::to_string(_pins[j]);
+ // const char * c = s.c_str();
+ // pc.printf(c);
+ // pc.printf("\n");
+ // }
+
+ // // Allocate the arrays
+ // // Allocate Space for Calibrated Maximum On Values
+ // calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOn == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOn[i] = 0;
+
+ // // Allocate Space for Calibrated Maximum Off Values
+ // calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOff == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOff[i] = 0;
+
+ // // Allocate Space for Calibrated Minimum On Values
+ // calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOn == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOn[i] = _maxValue;
+
+ // // Allocate Space for Calibrated Minimum Off Values
+ // calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOff == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOff[i] = _maxValue;
+
+ // emitter pin is used for DigitalOut
+ // So we create a DigitalOut on that pin
+ _emitterPin = emitterPin;
+ if(_emitterPin != 255){
+ if(emitterPin == 2){
+ _emitter = new DigitalOut(PTB9);
+ }
+ }
+
+ // If we have an Analog Sensor then we wil used AnalogIn on the pins provided
+ // We use a Vector for our collection of pins
+ // Here we reserve space for the pins
+ // _analog = analog;
+ // if (_analog) {
+ // _qtrAIPins.reserve(_numSensors);
+ // } else {
+ // // Not analog - so we use _qtrPins (which is a Vector on DigitalInOut)
+ // _qtrPins.reserve(_numSensors);
+ // }
+ // // Create the pins and push onto the vectors
+ // for (i = 0; i < _numSensors; i++) {
+ // if (analog) {
+ // _qtrAIPins.push_back(new AnalogIn(_pins[i]));
+ // } else {
+ // _qtrPins.push_back(new DigitalInOut(_pins[i]));
+ // }
+ // }
+
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in abstract units,
+// with higher values corresponding to lower reflectance (e.g. a black
+// surface or a void).
+void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode)
+{
+ unsigned int off_values[QTR_MAX_SENSORS];
+ unsigned char i;
+
+
+ if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF) {
+ emittersOn();
+ } else {
+ emittersOff();
+ }
+
+ readPrivate(sensor_values);
+
+ emittersOff();
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF) {
+ readPrivate(off_values);
+
+ for(i=0; i<_numSensors; i++) {
+ sensor_values[i] += _maxValue - off_values[i];
+ }
+ }
+}
+
+
+// Turn the IR LEDs off and on. This is mainly for use by the
+// read method, and calling these functions before or
+// after the reading the sensors will have no effect on the
+// readings, but you may wish to use these for testing purposes.
+void QTRSensors::emittersOff()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(0);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+void QTRSensors::emittersOn()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(1);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+// Resets the calibration.
+void QTRSensors::resetCalibration()
+{
+ unsigned char i;
+ for(i=0; i<_numSensors; i++) {
+ if(calibratedMinimumOn)
+ calibratedMinimumOn[i] = _maxValue;
+ if(calibratedMinimumOff)
+ calibratedMinimumOff[i] = _maxValue;
+ if(calibratedMaximumOn)
+ calibratedMaximumOn[i] = 0;
+ if(calibratedMaximumOff)
+ calibratedMaximumOff[i] = 0;
+ }
+}
+
+// Reads the sensors 10 times and uses the results for
+// calibration. The sensor values are not returned; instead, the
+// maximum and minimum values found over time are stored internally
+// and used for the readCalibrated() method.
+void QTRSensors::calibrate(unsigned char readMode)
+{
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) {
+ calibrateOnOrOff(&calibratedMinimumOn,
+ &calibratedMaximumOn,
+ QTR_EMITTERS_ON);
+ }
+
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) {
+ calibrateOnOrOff(&calibratedMinimumOff,
+ &calibratedMaximumOff,
+ QTR_EMITTERS_OFF);
+ }
+}
+
+void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum,
+ unsigned int **calibratedMaximum,
+ unsigned char readMode)
+{
+ int i;
+ unsigned int sensor_values[16];
+ unsigned int max_sensor_values[16];
+ unsigned int min_sensor_values[16];
+
+ // initialisation of calibrated sensor values moved to init()
+
+ // Allocate the arrays if necessary.
+ if(*calibratedMaximum == 0)
+ {
+ *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMaximum == 0)
+ return;
+
+ // Initialize the max and min calibrated values to values that
+ // will cause the first reading to update them.
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMaximum)[i] = 0;
+ }
+ if(*calibratedMinimum == 0)
+ {
+ *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMinimum == 0)
+ return;
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMinimum)[i] = _maxValue;
+ }
+
+ int j;
+ for(j=0; j<10; j++) {
+ read(sensor_values,readMode);
+ for(i=0; i<_numSensors; i++) {
+ // set the max we found THIS time
+ if(j == 0 || max_sensor_values[i] < sensor_values[i])
+ max_sensor_values[i] = sensor_values[i];
+
+ // set the min we found THIS time
+ if(j == 0 || min_sensor_values[i] > sensor_values[i])
+ min_sensor_values[i] = sensor_values[i];
+ }
+ }
+
+ // record the min and max calibration values
+ for(i=0; i<_numSensors; i++) {
+ if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i]
+ (*calibratedMaximum)[i] = min_sensor_values[i];
+ if(max_sensor_values[i] < (*calibratedMinimum)[i])
+ (*calibratedMinimum)[i] = max_sensor_values[i];
+ }
+
+}
+
+
+// Returns values calibrated to a value between 0 and 1000, where
+// 0 corresponds to the minimum value read by calibrate() and 1000
+// corresponds to the maximum value. Calibration values are
+// stored separately for each sensor, so that differences in the
+// sensors are accounted for automatically.
+void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode)
+{
+ int i;
+
+ // if not calibrated, do nothing
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
+ if(!calibratedMinimumOff || !calibratedMaximumOff)
+ return;
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
+ if(!calibratedMinimumOn || !calibratedMaximumOn)
+ return;
+
+ // read the needed values
+ read(sensor_values,readMode);
+
+ for(i=0; i<_numSensors; i++) {
+ unsigned int calmin,calmax;
+ unsigned int denominator;
+
+ // find the correct calibration
+ if(readMode == QTR_EMITTERS_ON) {
+ calmax = calibratedMaximumOn[i];
+ calmin = calibratedMinimumOn[i];
+ } else if(readMode == QTR_EMITTERS_OFF) {
+ calmax = calibratedMaximumOff[i];
+ calmin = calibratedMinimumOff[i];
+ } else { // QTR_EMITTERS_ON_AND_OFF
+
+ if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
+ calmin = _maxValue;
+ else
+ calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue
+
+ if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal
+ calmax = _maxValue;
+ else
+ calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue
+ }
+
+ denominator = calmax - calmin;
+
+ signed int x = 0;
+ if(denominator != 0)
+ x = (((signed long)sensor_values[i]) - calmin)
+ * 1000 / denominator;
+ if(x < 0)
+ x = 0;
+ else if(x > 1000)
+ x = 1000;
+ sensor_values[i] = x;
+ }
+
+}
+
+
+// Operates the same as read calibrated, but also returns an
+// estimated position of the robot with respect to a line. The
+// estimate is made using a weighted average of the sensor indices
+// multiplied by 1000, so that a return value of 0 indicates that
+// the line is directly below sensor 0, a return value of 1000
+// indicates that the line is directly below sensor 1, 2000
+// indicates that it's below sensor 2000, etc. Intermediate
+// values indicate that the line is between two sensors. The
+// formula is:
+//
+// 0*value0 + 1000*value1 + 2000*value2 + ...
+// --------------------------------------------
+// value0 + value1 + value2 + ...
+//
+// By default, this function assumes a dark line (high values)
+// surrounded by white (low values). If your line is light on
+// black, set the optional second argument white_line to true. In
+// this case, each sensor value will be replaced by (1000-value)
+// before the averaging.
+int QTRSensors::readLine(unsigned int *sensor_values,
+ unsigned char readMode, unsigned char white_line)
+{
+ unsigned char i, on_line = 0;
+ unsigned long avg; // this is for the weighted total, which is long
+ // before division
+ unsigned int sum; // this is for the denominator which is <= 64000
+ // static int last_value=0; // assume initially that the line is left.
+
+ readCalibrated(sensor_values, readMode);
+
+ avg = 0;
+ sum = 0;
+
+ for(i=0; i<_numSensors; i++) {
+ int value = sensor_values[i];
+ // printf("Sensor %d: %d\n", i, value);
+ if(white_line)
+ value = 1000-value;
+
+ // keep track of whether we see the line at all
+ if(value > 200) {
+ on_line = 1;
+ // printf("On line: %d\n", value);
+ }
+
+ // only average in values that are above a noise threshold
+ if(value > 50) {
+ avg += (long)(value) * (i * 1000);
+ sum += value;
+ }
+ }
+
+ if(!on_line) {
+ // If it last read to the left of center, return 0.
+ if(_lastValue < (_numSensors-1)*1000/2)
+ return 0;
+
+ // If it last read to the right of center, return the max.
+ else
+ return (_numSensors-1)*1000;
+
+ }
+
+ // printf("Average: %lu\n", avg);
+ // printf("Sum: %d\n", sum);
+
+ _lastValue = avg/sum;
+
+ // printf("Last value: %d\n", _lastValue);
+
+ return _lastValue;
+}
+
+
+
+// Derived RC class constructors
+QTRSensorsRC::QTRSensorsRC()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsRC::QTRSensorsRC(unsigned char* pins,
+ unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ init(pins, numSensors, timeout, emitterPin);
+}
+
+
+// The array 'pins' contains the Arduino pin number for each sensor.
+
+// 'numSensors' specifies the length of the 'pins' array (i.e. the
+// number of QTR-RC sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'timeout' specifies the length of time in microseconds beyond
+// which you consider the sensor reading completely black. That is to say,
+// if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+// and the reading for that pin will be considered full black.
+// It is recommended that you set timeout to be between 1000 and
+// 3000 us, depending on things like the height of your sensors and
+// ambient lighting. Using timeout allows you to shorten the
+// duration of a sensor-reading cycle while still maintaining
+// useful analog measurements of reflectance
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsRC::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned int timeout, unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin, false);
+
+ _maxValue = timeout;
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// ...
+// The values returned are in microseconds and range from 0 to
+// timeout (as specified in the constructor).
+void QTRSensorsRC::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i;
+ // Serial pc(USBTX, USBRX);
+
+ if (_pins == 0)
+ return;
+
+ // DigitalInOut pinout1(_pins[0]);
+ // DigitalInOut pinout2(_pins[1]);
+ // DigitalInOut pinout3(_pins[2]);
+ // DigitalInOut pinout4(_pins[3]);
+ // DigitalInOut pinout5(_pins[4]);
+ // DigitalInOut pinout6(_pins[5]);
+
+ for(i = 0; i < _numSensors; i++) {
+ sensor_values[i] = _maxValue;
+ _qtrPins[i]->output();
+ _qtrPins[i]->write(1); // make sensor line an output and drive high
+ }
+
+ // for(i = 0; i < _numSensors; i++) {
+ // sensor_values[i] = _maxValue;
+ // switch(i){
+ // case 0:
+ // pinout1.output(); // make sensor line an output and drive high
+ // pinout1.write(1);
+ // break;
+ // case 1:
+ // pinout2.output(); // make sensor line an output and drive high
+ // pinout2.write(1);
+ // break;
+ // case 2:
+ // pinout3.output(); // make sensor line an output and drive high
+ // pinout3.write(1);
+ // break;
+ // case 3:
+ // pinout4.output(); // make sensor line an output and drive high
+ // pinout4.write(1);
+ // break;
+ // case 4:
+ // pinout5.output(); // make sensor line an output and drive high
+ // pinout5.write(1);
+ // break;
+ // case 5:
+ // pinout6.output(); // make sensor line an output and drive high
+ // pinout6.write(1);
+ // break;
+ // }
+ // }
+
+ wait_us(10); // charge lines for 10 us
+
+ for(i = 0; i < _numSensors; i++) {
+ _qtrPins[i]->input();
+ _qtrPins[i]->mode(PullNone);
+ }
+
+ // for(i = 0; i < _numSensors; i++) {
+ // // important: disable internal pull-up!
+ // // ??? do we need to change the mode: _qtrPins[i]->mode(OpenDrain);
+ // // or just change mode to input
+ // // mbed documentation is not clear and I do not have a test sensor
+ // switch(i){
+ // case 0:
+ // pinout1.input(); // make sensor line an output and drive high
+ // pinout1.mode(PullNone);
+ // break;
+ // case 1:
+ // pinout2.input(); // make sensor line an output and drive high
+ // pinout2.mode(PullNone);
+ // break;
+ // case 2:
+ // pinout3.input(); // make sensor line an output and drive high
+ // pinout3.mode(PullNone);
+ // break;
+ // case 3:
+ // pinout4.input(); // make sensor line an output and drive high
+ // pinout4.mode(PullNone);
+ // break;
+ // case 4:
+ // pinout5.input(); // make sensor line an output and drive high
+ // pinout5.mode(PullNone);
+ // break;
+ // case 5:
+ // pinout6.input(); // make sensor line an output and drive high
+ // pinout6.mode(PullNone);
+ // break;
+ // }
+ // }
+
+ timer.start();
+ unsigned long startTime = timer.read_us();
+ while ((timer.read_us() - startTime) < _maxValue) {
+ unsigned int time = timer.read_us() - startTime;
+ for (i = 0; i < _numSensors; i++) {
+ if (_qtrPins[i]->read() == 0 && time < sensor_values[i])
+ sensor_values[i] = time;
+ }
+ }
+
+ // timer.start();
+ // unsigned long startTime = timer.read_us();
+ // while ((timer.read_us() - startTime) < _maxValue) {
+ // unsigned int time = timer.read_us() - startTime;
+ // for (i = 0; i < _numSensors; i++) {
+ // switch(i){
+ // case 0:
+ // if (pinout1.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 1:
+ // if (pinout2.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 2:
+ // if (pinout3.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 3:
+ // if (pinout4.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 4:
+ // if (pinout5.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 5:
+ // if (pinout6.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // }
+ // }
+ // wait_us(1);
+ // wait_us(300);
+ // timer.stop();
+ // pausedTime = pausedTime + 300;
+ // addTime = true;
+ // timer.start();
+ // printf("Timer2 %d\n", timer.read_us());
+ // pausedTime = 0;
+ timer.stop();
+ timer.reset();
+}
+
+
+// Derived Analog class constructors
+QTRSensorsAnalog::QTRSensorsAnalog()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsAnalog::QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ // this is analog - so use analog = true as a parameter
+
+ init(pins, numSensors, numSamplesPerSensor, emitterPin);
+}
+
+
+// the array 'pins' contains the Arduino analog pin assignment for each
+// sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+// Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+// and sensor 3 is on Arduino analog input 7.
+
+// 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+// number of QTR-A sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+// to average per channel (i.e. per sensor) for each reading. The total
+// number of analog-to-digital conversions performed will be equal to
+// numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+// perform a single analog-to-digital conversion, so:
+// if numSamplesPerSensor is 4 and numSensors is 6, it will take
+// 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+// Increasing this parameter increases noise suppression at the cost of
+// sample rate. The recommended value is 4.
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsAnalog::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin);
+
+ _numSamplesPerSensor = numSamplesPerSensor;
+ _maxValue = 1023; // this is the maximum returned by the A/D conversion
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in terms of a
+// 10-bit ADC average with higher values corresponding to lower
+// reflectance (e.g. a black surface or a void).
+void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i, j;
+
+ if (_pins == 0)
+ return;
+
+ // reset the values
+ for(i = 0; i < _numSensors; i++)
+ sensor_values[i] = 0;
+
+ for (j = 0; j < _numSamplesPerSensor; j++) {
+ for (i = 0; i < _numSensors; i++) {
+ sensor_values[i] += static_cast<unsigned int>(AnalogIn(_pins[i])); // add the conversion result
+ }
+ }
+
+ // get the rounded average of the readings for each sensor
+ for (i = 0; i < _numSensors; i++)
+ sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
+ _numSamplesPerSensor;
+}
+
+// the destructor frees up allocated memory
+QTRSensors::~QTRSensors()
+{
+ if(calibratedMinimumOff)
+ free(calibratedMinimumOff);
+ if(calibratedMinimumOn)
+ free(calibratedMinimumOn);
+ if(calibratedMaximumOff)
+ free(calibratedMaximumOff);
+ if(calibratedMaximumOn)
+ free(calibratedMaximumOn);
+ if (_pins)
+ free(_pins);
+
+ unsigned int i;
+ for (i = 0; i < _numSensors; i++) {
+ // if (_analog) {
+ // delete _qtrAIPins[i];
+ // } else {
+ delete _qtrPins[i];
+ // }
+ }
+ // if (_analog) {
+ // _qtrAIPins.clear();
+ // vector<AnalogIn *>().swap(_qtrAIPins);
+ // } else {
+ _qtrPins.clear();
+ vector<DigitalInOut *>().swap(_qtrPins);
+ // }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/QTRSensors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,283 @@
+/*
+ QTRSensors.h - Library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+#include "mbed.h"
+#include <vector>
+
+#ifndef QTRSensors_h
+#define QTRSensors_h
+
+#define QTR_EMITTERS_OFF 0
+#define QTR_EMITTERS_ON 1
+#define QTR_EMITTERS_ON_AND_OFF 2
+
+#define QTR_NO_EMITTER_PIN 255
+
+#define QTR_MAX_SENSORS 16
+#define HIGH 1
+#define LOW 0
+
+// This class cannot be instantiated directly (it has no constructor).
+// Instead, you should instantiate one of its two derived classes (either the
+// QTR-A or QTR-RC version, depending on the type of your sensor).
+class QTRSensors
+{
+ public:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in abstract units,
+ // with higher values corresponding to lower reflectance (e.g. a black
+ // surface or a void).
+ // If measureOffAndOn is true, measures the values with the
+ // emitters on AND off and returns on - (timeout - off). If this
+ // value is less than zero, it returns zero.
+ // This method will call the appropriate derived class's readPrivate(),
+ // which is defined as a virtual function in the base class and
+ // overridden by each derived class's own implementation.
+ void read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Turn the IR LEDs off and on. This is mainly for use by the
+ // read method, and calling these functions before or
+ // after the reading the sensors will have no effect on the
+ // readings, but you may wish to use these for testing purposes.
+ void emittersOff();
+ void emittersOn();
+
+ // Reads the sensors for calibration. The sensor values are
+ // not returned; instead, the maximum and minimum values found
+ // over time are stored internally and used for the
+ // readCalibrated() method.
+ void calibrate(unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Resets all calibration that has been done.
+ void resetCalibration();
+
+ // Returns values calibrated to a value between 0 and 1000, where
+ // 0 corresponds to the minimum value read by calibrate() and 1000
+ // corresponds to the maximum value. Calibration values are
+ // stored separately for each sensor, so that differences in the
+ // sensors are accounted for automatically.
+ void readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Operates the same as read calibrated, but also returns an
+ // estimated position of the robot with respect to a line. The
+ // estimate is made using a weighted average of the sensor indices
+ // multiplied by 1000, so that a return value of 0 indicates that
+ // the line is directly below sensor 0, a return value of 1000
+ // indicates that the line is directly below sensor 1, 2000
+ // indicates that it's below sensor 2000, etc. Intermediate
+ // values indicate that the line is between two sensors. The
+ // formula is:
+ //
+ // 0*value0 + 1000*value1 + 2000*value2 + ...
+ // --------------------------------------------
+ // value0 + value1 + value2 + ...
+ //
+ // By default, this function assumes a dark line (high values)
+ // surrounded by white (low values). If your line is light on
+ // black, set the optional second argument white_line to true. In
+ // this case, each sensor value will be replaced by (1000-value)
+ // before the averaging.
+ int readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char white_line = 0);
+
+ unsigned int *calibratedMinimumOn;
+ unsigned int *calibratedMaximumOn;
+ unsigned int *calibratedMinimumOff;
+ unsigned int *calibratedMaximumOff;
+
+ // Calibrated minumum and maximum values. These start at 1000 and
+ // 0, respectively, so that the very first sensor reading will
+ // update both of them.
+ //
+ // The pointers are unallocated until calibrate() is called, and
+ // then allocated to exactly the size required. Depending on the
+ // readMode argument to calibrate, only the On or Off values may
+ // be allocated, as required.
+ //
+ // These variables are made public so that you can use them for
+ // your own calculations and do things like saving the values to
+ // EEPROM, performing sanity checking, etc.
+
+ ~QTRSensors();
+
+ protected:
+
+ QTRSensors()
+ {
+
+ };
+
+ void init(unsigned char *pins, unsigned char numSensors, unsigned char emitterPin, bool);
+
+ bool _analog;
+ PinName *_pins;
+ unsigned char _numSensors;
+ unsigned char _emitterPin;
+ unsigned int _maxValue; // the maximum value returned by this function#
+ int _lastValue;
+ DigitalOut *_emitter;
+ std::vector<DigitalInOut *> _qtrPins;
+ // std::vector<AnalogIn *> _qtrAIPins;
+
+ private:
+
+ virtual void readPrivate(unsigned int *sensor_values) = 0;
+
+ // Handles the actual calibration. calibratedMinimum and
+ // calibratedMaximum are pointers to the requested calibration
+ // arrays, which will be allocated if necessary.
+ void calibrateOnOrOff(unsigned int **calibratedMaximum,
+ unsigned int **calibratedMinimum,
+ unsigned char readMode);
+};
+
+
+
+// Object to be used for QTR-1RC and QTR-8RC sensors
+class QTRSensorsRC : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsRC();
+
+ // this constructor just calls init()
+ QTRSensorsRC(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 4000, unsigned char emitterPin = 255);
+
+ // The array 'pins' contains the Arduino pin number for each sensor.
+
+ // 'numSensors' specifies the length of the 'pins' array (i.e. the
+ // number of QTR-RC sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'timeout' specifies the length of time in microseconds beyond
+ // which you consider the sensor reading completely black. That is to say,
+ // if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+ // and the reading for that pin will be considered full black.
+ // It is recommended that you set timeout to be between 1000 and
+ // 3000 us, depending on things like the height of your sensors and
+ // ambient lighting. Using timeout allows you to shorten the
+ // duration of a sensor-reading cycle while still maintaining
+ // useful analog measurements of reflectance
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 2000, unsigned char emitterPin = QTR_NO_EMITTER_PIN); // NC = Not Connected
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in microseconds.
+ void readPrivate(unsigned int *sensor_values);
+};
+
+
+
+// Object to be used for QTR-1A and QTR-8A sensors
+class QTRSensorsAnalog : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsAnalog();
+
+ // this constructor just calls init()
+ QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4,
+ unsigned char emitterPin = 255);
+
+ // the array 'pins' contains the Arduino analog pin assignment for each
+ // sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+ // Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+ // and sensor 3 is on Arduino analog input 7.
+
+ // 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+ // number of QTR-A sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+ // to average per channel (i.e. per sensor) for each reading. The total
+ // number of analog-to-digital conversions performed will be equal to
+ // numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+ // perform a single analog-to-digital conversion, so:
+ // if numSamplesPerSensor is 4 and numSensors is 6, it will take
+ // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+ // Increasing this parameter increases noise suppression at the cost of
+ // sample rate. The recommended value is 4.
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* analogPins, unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4, unsigned char emitterPin = QTR_NO_EMITTER_PIN);
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in terms of a
+ // 10-bit ADC average with higher values corresponding to lower
+ // reflectance (e.g. a black surface or a void).
+ void readPrivate(unsigned int *sensor_values);
+
+ unsigned char _numSamplesPerSensor;
+};
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/ZumoBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,29 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file ZumoBuzzer.h */
+
+#pragma once
+
+#include "PololuBuzzer.h"
+
+/*! \brief Plays beeps and music on the buzzer on the Zumo 32U4.
+ *
+ * This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on
+ * the Zumo 32U4 buzzer.
+ *
+ * Note durations are timed using a timer overflow interrupt
+ * (`TIMER4_OVF`), which will briefly interrupt execution of your
+ * main program at the frequency of the sound being played. In most cases, the
+ * interrupt-handling routine is very short (several microseconds). However,
+ * when playing a sequence of notes in `PLAY_AUTOMATIC` mode (the default mode)
+ * with the `play()` command, this interrupt takes much longer than normal
+ * (perhaps several hundred microseconds) every time it starts a new note. It is
+ * important to take this into account when writing timing-critical code.
+ */
+class ZumoBuzzer : public PololuBuzzer
+{
+ // This is a trivial subclass of PololuBuzzer; it exists because
+ // we wanted the ZumoShield class names to be consistent and we
+ // didn't just use a typedef to define it because that would make
+ // the Doxygen documentation harder to understand.
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/ZumoMotors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,84 @@
+/* File: DRV8835.cpp
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+
+#include "mbed.h"
+#include "ZumoMotors.h"
+
+
+ZumoMotors::ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin) :
+pwmL(pinPwmL),
+Lin(pinLin),
+pwmR(pinPwmR),
+Rin(pinRin)
+{
+ Lin = 0;
+ Rin = 0;
+ pwmL.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmL = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ pwmR.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmR = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::stop()
+{
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::motorL_stop(void)
+{
+ pwmL = 0;
+}
+
+void ZumoMotors::motorR_stop(void)
+{
+ pwmR = 0;
+}
+
+void ZumoMotors::setSpeeds(float Left,float Right)
+{
+ //Set Right Speed and Direction
+ if(Right<0)
+ {
+ motorR_rev(Right*-1);
+ } else {
+ motorR_fwd(Right);
+ }
+
+ //Set Left Speed and Direction
+ if(Left<0)
+ {
+ motorL_rev(Left*-1);
+ } else {
+ motorL_fwd(Left);
+ }
+}
+
+void ZumoMotors::motorL_fwd(float fPulsewidth)
+{
+ Lin = 0;
+ pwmL = fPulsewidth;
+}
+void ZumoMotors::motorL_rev(float fPulsewidth)
+{
+ Lin = 1;
+ pwmL = fPulsewidth;
+}
+
+void ZumoMotors::motorR_fwd(float fPulsewidth)
+{
+ Rin = 0;
+ pwmR = fPulsewidth;
+}
+void ZumoMotors::motorR_rev(float fPulsewidth)
+{
+ Rin = 1;
+ pwmR = fPulsewidth;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/ZumoMotors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,38 @@
+/* File: DRV8835.h
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+#ifndef __DRV8835_H__
+#define __DRV8835_H__
+
+#include "mbed.h"
+
+#define DRV8835_PWM_PERIOD_DEFAULT (0.00005) // 2 ms period // 50KHz (0.00002)
+#define DRV8835_PWM_PULSEWIDTH_DEFAULT (0.50) // 50% duty cycle
+
+class ZumoMotors
+{
+public:
+ ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin);
+
+ void motorL_stop(void);
+ void motorL_fwd(float fPulsewidth);
+ void motorL_rev(float fPulsewidth);
+ void motorR_stop(void);
+ void motorR_fwd(float fPulsewidth);
+ void motorR_rev(float fPulsewidth);
+ void setSpeeds(float Left,float Right);
+ void stop(void);
+
+private:
+ PwmOut pwmL;
+ DigitalOut Lin;
+ PwmOut pwmR;
+ DigitalOut Rin;
+};
+
+#endif /* __DRV8835_H__ */
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/ZumoReflectanceSensorArray.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,406 @@
+/*! \file ZumoReflectanceSensorArray.h
+ *
+ * See the ZumoReflectanceSensorArray class reference for more information about
+ * this library.
+ *
+ * \class ZumoReflectanceSensorArray ZumoReflectanceSensorArray.h
+ * \brief Read from reflectance sensor array
+ *
+ * ZumoReflectanceSensorArray provides an interface for using a
+ * [Zumo Reflectance Sensor Array](https://www.pololu.com/product/1419) connected
+ * to a Zumo robot. It provides access to the raw sensor values as well
+ * as to high level functions including calibration and line-tracking.
+ *
+ * For calibration, memory is allocated using the `malloc()` function. This
+ * conserves RAM: if all six sensors are calibrated with the emitters both on
+ * and off, a total of 48 bytes is dedicated to storing calibration values.
+ * However, for an application where only two sensors are used, and the
+ * emitters are always on during reads, only 4 bytes are required.
+ *
+ * Internally, it uses all standard Arduino functions such as
+ * `micros()` for timing and `digitalRead()` for getting the sensor values, so
+ * it should work on all Arduinos without conflicting with other libraries.
+ *
+ * ### Calibration ###
+ *
+ * ZumoReflectanceSensorArray allows you to use the `calibrate()`
+ * method to easily calibrate your sensors for the particular
+ * conditions it will encounter. Calibrating your sensors can lead to
+ * substantially more reliable sensor readings, which in turn can help
+ * simplify your code. As such, we recommend you build a calibration
+ * phase into your Zumo's initialization routine. This can be as
+ * simple as a fixed duration over which you repeatedly call the
+ * `calibrate()` method.
+ *
+ * During this calibration phase, you will need to expose each of your
+ * reflectance sensors to the lightest and darkest readings they will
+ * encounter. For example, if your Zumo is programmed to be a line
+ * follower, you will want to slide it across the line during the
+ * calibration phase so the each sensor can get a reading of how dark
+ * the line is and how light the ground is (or you can program it to
+ * automatically turn back and forth to pass all of the sensors over
+ * the line). The **SensorCalibration** example demonstrates a
+ * calibration routine.
+ *
+ * ### Reading the sensors
+ *
+ *
+ * ZumoReflectanceSensorArray gives you a number of different ways to
+ * read the sensors.
+ *
+ * - You can request raw sensor values using the `read()` method.
+ *
+ * - You can request calibrated sensor values using the `readCalibrated()`
+ * method. Calibrated sensor values will always range from 0 to 1000, with the
+ * extreme values corresponding to the most and least reflective surfaces
+ * encountered during calibration.
+ *
+ * - For line-detection applications, you can request the line location using
+ * the `readLine()` method. This function provides calibrated values
+ * for each sensor and returns an integer that tells you where it thinks the
+ * line is.
+ *
+ * ### Class inheritance ###
+ *
+ * The ZumoReflectanceSensorArray class is derived from the QTRSensorsRC class,
+ * which is in turn derived from the QTRSensors base class. The QTRSensorsRC and
+ * QTRSensors classes are part of the \ref QTRSensors.h "QTRSensors" library,
+ * which provides more general functionality for working with reflectance
+ * sensors and is included in the Zumo Shield Arduino Library as a dependency
+ * for this library.
+ *
+ * We recommend using ZumoReflectanceSensorArray instead of
+ * the \ref QTRSensors.h "QTRSensors" library when programming an Arduino on a
+ * Zumo. For documentation specific to the %QTRSensors library, please see its
+ * [user's guide](https://www.pololu.com/docs/0J19) on Pololu's website.
+ */
+
+#ifndef ZumoReflectanceSensorArray_h
+#define ZumoReflectanceSensorArray_h
+
+#include "QTRSensors.h"
+#include "mbed.h"
+
+#define ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN 2
+
+class ZumoReflectanceSensorArray : public QTRSensorsRC
+{
+ public:
+
+ /*! \brief Minimal constructor.
+ *
+ * This version of the constructor performs no initialization. If it is used,
+ * the user must call init() before using the methods in this class.
+ */
+ ZumoReflectanceSensorArray() {}
+
+ /*! \brief Constructor; initializes with given emitter pin and defaults for
+ * other settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char emitterPin)` with the specified
+ * emitter pin and default values for other settings.
+ */
+ ZumoReflectanceSensorArray(unsigned char emitterPin)
+ {
+ init(emitterPin);
+ }
+
+ /*! \brief Constructor; initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char * pins, unsigned char
+ * numSensors, unsigned int timeout, unsigned char emitterPin)` with all
+ * settings as given.
+ */
+ ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+
+ /*! \brief Initializes with given emitter pin and and defaults for other
+ * settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with the
+ * specified emitter pin. The other settings are set to default values: all
+ * six sensors on the array are active, and a timeout of 2000 microseconds is
+ * used.
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The "LED ON" jumper on the Zumo Reflectance Sensor Array must be
+ * configured correctly for this to work.) If the value `QTR_NO_EMITTER_PIN`
+ * (255) is used, you can leave the emitter pin disconnected and the IR
+ * emitters will always be on.
+ */
+ void init(unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ unsigned char sensorPins[] = { 1, 2, 3, 4, 5, 6 };
+ QTRSensorsRC::init(sensorPins, sizeof(sensorPins), 2000, emitterPin);
+ // pc.printf("hello");
+ }
+
+ /*! \brief Initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with all
+ * settings as given.
+ *
+ * The array \a pins should contain the Arduino digital pin numbers for each
+ * sensor.
+ *
+ * \a numSensors specifies the length of the \a pins array (the number of
+ * reflectance sensors you are using).
+ *
+ * \a timeout specifies the length of time in microseconds beyond which you
+ * consider the sensor reading completely black. That is to say, if the pulse
+ * length for a pin exceeds \a timeout, pulse timing will stop and the reading
+ * for that pin will be considered full black. It is recommended that you set
+ * \a timeout to be between 1000 and 3000 us, depending on factors like the
+ * height of your sensors and ambient lighting. This allows you to shorten the
+ * duration of a sensor-reading cycle while maintaining useful measurements of
+ * reflectance. If \a timeout is not specified, it defaults to 2000 us. (See
+ * the [product page](https://www.pololu.com/product/1419) for the Zumo
+ * Reflectance Sensor Array on Pololu's website for an overview of the
+ * sensors' principle of operation.)
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The corresponding connection should be made with the "LED ON"
+ * jumper on the Zumo Reflectance Sensor Array.) If the value
+ * `QTR_NO_EMITTER_PIN` (255) is used, you can leave the emitter pin
+ * disconnected and the IR emitters will always be on.
+ *
+ * This version of `%init()` can be useful if you only want to use a subset
+ * of the six reflectance sensors on the array. For example, using the
+ * outermost two sensors (on pins 4 and 5 by default) is usually enough for
+ * detecting the ring border in sumo competitions:
+ *
+ * ~~~{.ino}
+ * ZumoReflectanceSensorArray reflectanceSensors;
+ *
+ * ...
+ *
+ * reflectanceSensors.init((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ * Alternatively, you can use \ref ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+ * "a different constructor" to declare and initialize the object at the same
+ * time:
+ *
+ * ~~~{.ino}
+ *
+ * ZumoReflectanceSensorArray reflectanceSensors((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ */
+ void init(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+};
+
+// documentation for inherited functions
+
+/*! \fn void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+\memberof ZumoReflectanceSensorArray
+ * \brief Reads the raw sensor values into an array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * There **must** be space in the \a sensor_values array for as many values as
+ * there were sensors specified in the constructor. The values returned are
+ * measures of the reflectance in units of microseconds. They will be raw
+ * readings between 0 and the \a timeout argument (in units of microseconds)
+ * provided in the constructor (which defaults to 2000).
+ *
+ * The \a readMode argument specifies the kind of read that will be performed.
+ * Several options are defined:
+ *
+ * - `QTR_EMITTERS_OFF` specifies that the reading should be made without
+ * turning on the infrared (IR) emitters, in which case the reading represents
+ * ambient light levels near the sensor.
+ * - `QTR_EMITTERS_ON` specifies that the emitters should be turned on for the
+ * reading, which results in a measure of reflectance.
+ * - `QTR_EMITTERS_ON_AND_OFF` specifies that a reading should be made in both
+ * the on and off states. The values returned when this option is used are
+ * given by the formula **on + max − off**, where **on** is the reading
+ * with the emitters on, **off** is the reading with the emitters off, and
+ * **max** is the maximum sensor reading. This option can reduce the amount of
+ * interference from uneven ambient lighting.
+ *
+ * Note that emitter control will only work if you specify a valid emitter pin
+ * in the constructor and make the corresponding connection (with the "LED ON"
+ * jumper or otherwise).
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOff()
+ * \brief Turns the IR LEDs off.
+ *
+ * This is mainly for use by the `read()` method, and calling this function
+ * before or after reading the sensors will have no effect on the readings, but
+ * you might wish to use it for testing purposes. This method will only do
+ * something if the emitter pin specified in the constructor is valid (i.e. not
+ * `QTR_NO_EMITTER_PIN`) and the corresponding connection is made.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOn()
+ * \brief Turns the IR LEDs on.
+ * \copydetails emittersOff
+ */
+
+/*! \fn void QTRSensors::calibrate(unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Reads the sensors for calibration.
+ *
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * The sensor values read by this function are not returned; instead, the
+ * maximum and minimum values found over time are stored internally and used for
+ * the `readCalibrated()` method. You can access the calibration (i.e raw max
+ * and min sensor readings) through the public member pointers
+ * `calibratedMinimumOn`, `calibratedMaximumOn`, `calibratedMinimumOff`, and
+ * `calibratedMaximumOff`. Note that these pointers will point to arrays of
+ * length \a numSensors, as specified in the constructor, and they will only be
+ * allocated **after** `calibrate()` has been called. If you only calibrate with
+ * the emitters on, the calibration arrays that hold the off values will not be
+ * allocated.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::resetCalibration()
+ * \brief Resets all calibration that has been done.
+ *
+ * This function discards the calibration values that have been previously
+ * recorded, resetting the min and max values.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Returns sensor readings normalized to values between 0 and 1000.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * 0 corresponds to a reading that is less than or equal to the minimum value
+ * read by `calibrate()` and 1000 corresponds to a reading that is greater than
+ * or equal to the maximum value. Calibration values are stored separately for
+ * each sensor, so that differences in the sensors are accounted for
+ * automatically.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn int QTRSensors::readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char whiteLine = 0)
+ * \brief Returns an estimated position of a line under the sensor array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ * \param whiteLine 0 to detect a dark line on a light surface; 1 to detect
+ * a light line on a dark surface.
+ * \return An estimated line position.
+ *
+ * This function operates the same as `readCalibrated()`, but with a feature
+ * designed for line following: it returns an estimated position of the line.
+ * The estimate is made using a weighted average of the sensor indices
+ * multiplied by 1000, so that a return value of 0 indicates that the line is
+ * directly below sensor 0 (or was last seen by sensor 0 before being lost), a
+ * return value of 1000 indicates that the line is directly below sensor 1, 2000
+ * indicates that it's below sensor 2, etc. Intermediate values indicate that
+ * the line is between two sensors. The formula is:
+ *
+ * \f[
+ * \newcommand{sv}[1]{\mathtt{sensor_values[#1]}}
+ * \text{return value} =
+ * \frac{(0 \times \sv{0}) + (1000 \times \sv{1}) + (2000 \times \sv{2}) + \ldots}
+ * {\sv{0} + \sv{1} + \sv{2} + \ldots}
+ * \f]
+ *
+ * As long as your sensors aren't spaced too far apart relative to the line,
+ * this returned value is designed to be monotonic, which makes it great for use
+ * in closed-loop PID control. Additionally, this method remembers where it last
+ * saw the line, so if you ever lose the line to the left or the right, its line
+ * position will continue to indicate the direction you need to go to reacquire
+ * the line. For example, if sensor 5 is your rightmost sensor and you end up
+ * completely off the line to the left, this function will continue to return
+ * 5000.
+ *
+ * By default, this function assumes a dark line (high values) on a light
+ * background (low values). If your line is light on dark, set the optional
+ * second argument \a whiteLine to true. In this case, each sensor value will be
+ * replaced by the maximum possible value minus its actual value before the
+ * averaging.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+
+// documentation for inherited member variables
+
+/*!
+ * \property unsigned int * QTRSensors::calibratedMinimumOn
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * on.
+ *
+ * This pointer is unallocated and set to 0 until `calibrate()` is called, and
+ * then allocated to exactly the size required. Depending on the \a readMode
+ * argument to `calibrate()`, only the On or Off values might be allocated, as
+ * required. This variable is made public so that you can use the calibration
+ * values for your own calculations and do things like saving them to EEPROM,
+ * performing sanity checking, etc.
+ *
+ * The ZumoReflectanceSensorArray class inherits this variable from the
+ * QTRSensors class.
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOn
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * on.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMinimumOff
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOff
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ */
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/mbed_app.json Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,7 @@
+{
+ "target_overrides": {
+ "*": {
+ "target.printf_lib": "std"
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/LineFollower/millis.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "millis.h"
+/*
+ millis.cpp
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+volatile unsigned long _millis;
+
+void millisStart(void) {
+ SysTick_Config(SystemCoreClock / 1000);
+}
+
+extern "C" void SysTicks_Handler(void) {
+ _millis++;
+}
+
+unsigned long millis(void) {
+ return _millis;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/LineFollower/millis.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,25 @@ +#ifndef MILLIS_H +#define MILLIS_H +/* + millis.h + Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com> + All rights reserved. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +void millisStart(void); +unsigned long millis(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/MazeSolver.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,511 @@
+/* This example uses the Zumo Reflectance Sensor Array
+ * to navigate a black line maze with no loops. This program
+ * is based off the 3pi maze solving example which can be
+ * found here:
+ *
+ * https://www.pololu.com/docs/0J21/8.a
+ *
+ * The Zumo first calibrates the sensors to account
+ * for differences of the black line on white background.
+ * Calibration is accomplished in setup().
+ *
+ * In loop(), the function solveMaze() is called and navigates
+ * the Zumo until it finds the finish line which is defined as
+ * a large black area that is thick and wide enough to
+ * cover all six sensors at the same time.
+ *
+ * Once the Zumo reaches the finishing line, it will stop and
+ * wait for the user to place the Zumo back at the starting
+ * line. The Zumo can then follow the shortest path to the finish
+ * line.
+ *
+ * The macros SPEED, TURN_SPEED, ABOVE_LINE(), and LINE_THICKNESS
+ * might need to be adjusted on a case by case basis to give better
+ * line following results.
+ */
+
+#include "mbed.h"
+#include "PololuBuzzer.h"
+#include "Pushbutton.h"
+#include "ZumoBuzzer.h"
+#include "ZumoMotors.h"
+#include "ZumoReflectanceSensorArray.h"
+I2C i2c(I2C_SDA, I2C_SCL);
+
+// #include <Wire.h>
+// #include <ZumoShield.h>
+
+// SENSOR_THRESHOLD is a value to compare reflectance sensor
+// readings to to decide if the sensor is over a black line
+#define SENSOR_THRESHOLD 300
+
+// ABOVE_LINE is a helper macro that takes returns
+// 1 if the sensor is over the line and 0 if otherwise
+#define ABOVE_LINE(sensor)((sensor) > SENSOR_THRESHOLD)
+
+// Motor speed when turning. TURN_SPEED should always
+// have a positive value, otherwise the Zumo will turn
+// in the wrong direction.
+#define TURN_SPEED 200
+
+// Motor speed when driving straight. SPEED should always
+// have a positive value, otherwise the Zumo will travel in the
+// wrong direction.
+#define SPEED 200
+
+// Thickness of your line in inches
+#define LINE_THICKNESS .75
+
+// When the motor speed of the zumo is set by
+// motors.setSpeeds(200,200), 200 is in ZUNITs/Second.
+// A ZUNIT is a fictitious measurement of distance
+// and only helps to approximate how far the Zumo has
+// traveled. Experimentally it was observed that for
+// every inch, there were approximately 17142 ZUNITs.
+// This value will differ depending on setup/battery
+// life and may be adjusted accordingly. This value
+// was found using a 75:1 HP Motors with batteries
+// partially discharged.
+#define INCHES_TO_ZUNITS 17142.0
+
+// When the Zumo reaches the end of a segment it needs
+// to find out three things: if it has reached the finish line,
+// if there is a straight segment ahead of it, and which
+// segment to take. OVERSHOOT tells the Zumo how far it needs
+// to overshoot the segment to find out any of these things.
+#define OVERSHOOT(line_thickness)(((INCHES_TO_ZUNITS * (line_thickness)) / SPEED))
+#define LED PTD1
+
+PololuBuzzer buzzer(PTA1);
+ZumoReflectanceSensorArray reflectanceSensors;
+ZumoMotors motors(PTD0, PTC12, PTC4, PTC3);
+Pushbutton button(ZUMO_BUTTON);
+DigitalOut ledPin(LED, 1);
+
+// path[] keeps a log of all the turns made
+// since starting the maze
+char path[100] = "";
+unsigned char path_length = 0; // the length of the path
+
+// Turns according to the parameter dir, which should be
+// 'L' (left), 'R' (right), 'S' (straight), or 'B' (back).
+void turn(char dir)
+{
+
+ // count and last_status help
+ // keep track of how much further
+ // the Zumo needs to turn.
+ unsigned short count = 0;
+ unsigned short last_status = 0;
+ unsigned int sensors[6];
+
+ // dir tests for which direction to turn
+ switch(dir)
+ {
+
+ // Since we're using the sensors to coordinate turns instead of timing them,
+ // we can treat a left turn the same as a direction reversal: they differ only
+ // in whether the zumo will turn 90 degrees or 180 degrees before seeing the
+ // line under the sensor. If 'B' is passed to the turn function when there is a
+ // left turn available, then the Zumo will turn onto the left segment.
+ case 'L':
+ case 'B':
+ // Turn left.
+ motors.setSpeeds(-TURN_SPEED, TURN_SPEED);
+
+ // This while loop monitors line position
+ // until the turn is complete.
+ while(count < 2)
+ {
+ reflectanceSensors.readLine(sensors);
+
+ // Increment count whenever the state of the sensor changes
+ // (white->black and black->white) since the sensor should
+ // pass over 1 line while the robot is turning, the final
+ // count should be 2
+ count += ABOVE_LINE(sensors[1]) ^ last_status;
+ last_status = ABOVE_LINE(sensors[1]);
+ }
+
+ break;
+
+ case 'R':
+ // Turn right.
+ motors.setSpeeds(TURN_SPEED, -TURN_SPEED);
+
+ // This while loop monitors line position
+ // until the turn is complete.
+ while(count < 2)
+ {
+ reflectanceSensors.readLine(sensors);
+ count += ABOVE_LINE(sensors[4]) ^ last_status;
+ last_status = ABOVE_LINE(sensors[4]);
+ }
+
+ break;
+
+ case 'S':
+ // Don't do anything!
+ break;
+ }
+}
+
+void setup()
+{
+
+ unsigned int sensors[6];
+ unsigned short count = 0;
+ unsigned short last_status = 0;
+ int turn_direction = 1;
+
+ buzzer.play(">g32>>c32");
+
+ reflectanceSensors.init();
+
+ wait_us(500000);
+ ledPin.write(1);
+
+ button.waitForButton();
+
+ // Calibrate the Zumo by sweeping it from left to right
+ for(int i = 0; i < 4; i ++)
+ {
+ // Zumo will turn clockwise if turn_direction = 1.
+ // If turn_direction = -1 Zumo will turn counter-clockwise.
+ turn_direction *= -1;
+
+ // Turn direction.
+ motors.setSpeeds(turn_direction * TURN_SPEED, -1*turn_direction * TURN_SPEED);
+
+ // This while loop monitors line position
+ // until the turn is complete.
+ while(count < 2)
+ {
+ reflectanceSensors.calibrate();
+ reflectanceSensors.readLine(sensors);
+ if(turn_direction < 0)
+ {
+ // If the right most sensor changes from (over white space -> over
+ // line or over line -> over white space) add 1 to count.
+ count += ABOVE_LINE(sensors[5]) ^ last_status;
+ last_status = ABOVE_LINE(sensors[5]);
+ }
+ else
+ {
+ // If the left most sensor changes from (over white space -> over
+ // line or over line -> over white space) add 1 to count.
+ count += ABOVE_LINE(sensors[0]) ^ last_status;
+ last_status = ABOVE_LINE(sensors[0]);
+ }
+ }
+
+ count = 0;
+ last_status = 0;
+ }
+
+ // Turn left.
+ turn('L');
+
+ motors.setSpeeds(0, 0);
+
+ // Sound off buzzer to denote Zumo is finished calibrating
+ buzzer.play("L16 cdegreg4");
+
+ // Turn off LED to indicate we are through with calibration
+ ledPin.write(0);
+}
+
+// simplifyPath analyzes the path[] array and reduces all the
+// turns. For example: Right turn + Right turn = (1) Back turn.
+void simplifyPath()
+{
+
+ // only simplify the path if the second-to-last turn was a 'B'
+ if(path_length < 3 || path[path_length - 2] != 'B')
+ return;
+
+ int total_angle = 0;
+ int i;
+
+ for(i = 1; i <= 3; i++)
+ {
+ switch(path[path_length - i])
+ {
+ case 'R':
+ total_angle += 90;
+ break;
+ case 'L':
+ total_angle += 270;
+ break;
+ case 'B':
+ total_angle += 180;
+ break;
+ }
+ }
+
+ // Get the angle as a number between 0 and 360 degrees.
+ total_angle = total_angle % 360;
+
+ // Replace all of those turns with a single one.
+ switch(total_angle)
+ {
+ case 0:
+ path[path_length - 3] = 'S';
+ break;
+ case 90:
+ path[path_length - 3] = 'R';
+ break;
+ case 180:
+ path[path_length - 3] = 'B';
+ break;
+ case 270:
+ path[path_length - 3] = 'L';
+ break;
+ }
+
+ // The path is now two steps shorter.
+ path_length -= 2;
+}
+
+// The maze is broken down into segments. Once the Zumo decides
+// which segment to turn on, it will navigate until it finds another
+// intersection. followSegment() will then return after the
+// intersection is found.
+void followSegment()
+{
+ unsigned int position;
+ unsigned int sensors[6];
+ int offset_from_center;
+ int power_difference;
+
+ while(1)
+ {
+ // Get the position of the line.
+ position = reflectanceSensors.readLine(sensors);
+
+ // The offset_from_center should be 0 when we are on the line.
+ offset_from_center = ((int)position) - 2500;
+
+ // Compute the difference between the two motor power settings,
+ // m1 - m2. If this is a positive number the robot will turn
+ // to the left. If it is a negative number, the robot will
+ // turn to the right, and the magnitude of the number determines
+ // the sharpness of the turn.
+ power_difference = offset_from_center / 3;
+
+ // Compute the actual motor settings. We never set either motor
+ // to a negative value.
+ if(power_difference > SPEED)
+ power_difference = SPEED;
+ if(power_difference < -SPEED)
+ power_difference = -SPEED;
+
+ if(power_difference < 0)
+ motors.setSpeeds(SPEED + power_difference, SPEED);
+ else
+ motors.setSpeeds(SPEED, SPEED - power_difference);
+
+ // We use the inner four sensors (1, 2, 3, and 4) for
+ // determining whether there is a line straight ahead, and the
+ // sensors 0 and 5 for detecting lines going to the left and
+ // right.
+
+ if(!ABOVE_LINE(sensors[0]) && !ABOVE_LINE(sensors[1]) && !ABOVE_LINE(sensors[2]) && !ABOVE_LINE(sensors[3]) && !ABOVE_LINE(sensors[4]) && !ABOVE_LINE(sensors[5]))
+ {
+ // There is no line visible ahead, and we didn't see any
+ // intersection. Must be a dead end.
+ return;
+ }
+ else if(ABOVE_LINE(sensors[0]) || ABOVE_LINE(sensors[5]))
+ {
+ // Found an intersection.
+ return;
+ }
+
+ }
+}
+
+// This function decides which way to turn during the learning phase of
+// maze solving. It uses the variables found_left, found_straight, and
+// found_right, which indicate whether there is an exit in each of the
+// three directions, applying the "left hand on the wall" strategy.
+char selectTurn(unsigned char found_left, unsigned char found_straight,
+ unsigned char found_right)
+{
+ // Make a decision about how to turn. The following code
+ // implements a left-hand-on-the-wall strategy, where we always
+ // turn as far to the left as possible.
+ if(found_left)
+ return 'L';
+ else if(found_straight)
+ return 'S';
+ else if(found_right)
+ return 'R';
+ else
+ return 'B';
+}
+
+// Now enter an infinite loop - we can re-run the maze as many
+// times as we want to.
+void goToFinishLine()
+{
+ unsigned int sensors[6];
+ int i = 0;
+
+ // Turn around if the Zumo is facing the wrong direction.
+ if(path[0] == 'B')
+ {
+ turn('B');
+ i++;
+ }
+
+ for(;i<path_length;i++)
+ {
+
+ followSegment();
+
+ // Drive through the intersection.
+ motors.setSpeeds(SPEED, SPEED);
+ wait_us(OVERSHOOT(LINE_THICKNESS*1000));
+
+ // Make a turn according to the instruction stored in
+ // path[i].
+ turn(path[i]);
+ }
+
+ // Follow the last segment up to the finish.
+ followSegment();
+
+ // The finish line has been reached.
+ // Return and wait for another button push to
+ // restart the maze.
+ reflectanceSensors.readLine(sensors);
+ motors.setSpeeds(0,0);
+
+ return;
+}
+
+// The solveMaze() function works by applying a "left hand on the wall" strategy:
+// the robot follows a segment until it reaches an intersection, where it takes the
+// leftmost fork available to it. It records each turn it makes, and as long as the
+// maze has no loops, this strategy will eventually lead it to the finish. Afterwards,
+// the recorded path is simplified by removing dead ends. More information can be
+// found in the 3pi maze solving example.
+void solveMaze()
+{
+ while(1)
+ {
+ // Navigate current line segment
+ followSegment();
+
+ // These variables record whether the robot has seen a line to the
+ // left, straight ahead, and right, while examining the current
+ // intersection.
+ unsigned char found_left = 0;
+ unsigned char found_straight = 0;
+ unsigned char found_right = 0;
+
+ // Now read the sensors and check the intersection type.
+ unsigned int sensors[6];
+ reflectanceSensors.readLine(sensors);
+
+ // Check for left and right exits.
+ if(ABOVE_LINE(sensors[0]))
+ found_left = 1;
+ if(ABOVE_LINE(sensors[5]))
+ found_right = 1;
+
+ // Drive straight a bit more, until we are
+ // approximately in the middle of intersection.
+ // This should help us better detect if we
+ // have left or right segments.
+ motors.setSpeeds(SPEED, SPEED);
+ wait_us(OVERSHOOT(LINE_THICKNESS)/2*1000);
+
+ reflectanceSensors.readLine(sensors);
+
+ // Check for left and right exits.
+ if(ABOVE_LINE(sensors[0]))
+ found_left = 1;
+ if(ABOVE_LINE(sensors[5]))
+ found_right = 1;
+
+ // After driving a little further, we
+ // should have passed the intersection
+ // and can check to see if we've hit the
+ // finish line or if there is a straight segment
+ // ahead.
+ wait_us(OVERSHOOT(LINE_THICKNESS)/2*1000);
+
+ // Check for a straight exit.
+ reflectanceSensors.readLine(sensors);
+
+ // Check again to see if left or right segment has been found
+ if(ABOVE_LINE(sensors[0]))
+ found_left = 1;
+ if(ABOVE_LINE(sensors[5]))
+ found_right = 1;
+
+ if(ABOVE_LINE(sensors[1]) || ABOVE_LINE(sensors[2]) || ABOVE_LINE(sensors[3]) || ABOVE_LINE(sensors[4]))
+ found_straight = 1;
+
+ // Check for the ending spot.
+ // If all four middle sensors are on dark black, we have
+ // solved the maze.
+ if(ABOVE_LINE(sensors[1]) && ABOVE_LINE(sensors[2]) && ABOVE_LINE(sensors[3]) && ABOVE_LINE(sensors[4]))
+ {
+ motors.setSpeeds(0,0);
+ break;
+ }
+
+ // Intersection identification is complete.
+ unsigned char dir = selectTurn(found_left, found_straight, found_right);
+
+ // Make the turn indicated by the path.
+ turn(dir);
+
+ // Store the intersection in the path variable.
+ path[path_length] = dir;
+ path_length++;
+
+ // You should check to make sure that the path_length does not
+ // exceed the bounds of the array. We'll ignore that in this
+ // example.
+
+ // Simplify the learned path.
+ simplifyPath();
+
+ }
+}
+
+void loop()
+{
+
+ // solveMaze() explores every segment
+ // of the maze until it finds the finish
+ // line.
+ solveMaze();
+
+ // Sound off buzzer to denote Zumo has solved the maze
+ buzzer.play(">>a32");
+
+ // The maze has been solved. When the user
+ // places the Zumo at the starting line
+ // and pushes the Zumo button, the Zumo
+ // knows where the finish line is and
+ // will automatically navigate.
+ while(1)
+ {
+ button.waitForButton();
+ goToFinishLine();
+ // Sound off buzzer to denote Zumo is at the finish line.
+ buzzer.play(">>a32");
+ }
+}
+
+int main(){
+ setup();
+ while(1){
+ loop();
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/PololuBuzzer.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,683 @@
+#include "PololuBuzzer.h"
+#include "mbed.h"
+
+using namespace mbed;
+ // constructor
+ /** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+
+BuzzerValues::BuzzerValues(float frequency, float duration, float volume) : freq(frequency), dur(duration), vol(volume) {}
+BuzzerValues::BuzzerValues() {}
+void BuzzerValues::changeValue(float frequency, float duration, float volume) {
+ freq = frequency;
+ dur = duration;
+ vol = volume;
+}
+float BuzzerValues::returnFrequency() {
+ return freq;
+}
+float BuzzerValues::returnDuration() {
+ return dur;
+}
+float BuzzerValues::returnVolume() {
+ return vol;
+}
+
+#define BUZZER_DDR DDRD
+#define BUZZER (1 << PORTD3)
+
+#define TIMER2_CLK_32 0x3 // 500 kHz
+
+static const unsigned int cs2_divider[] = {0, 1, 8, 32, 64, 128, 256, 1024};
+
+#define ENABLE_TIMER_INTERRUPT() TIMSK2 = (1 << TOIE2)
+#define DISABLE_TIMER_INTERRUPT() TIMSK2 = 0
+
+unsigned char buzzerInitialized = 0;
+volatile unsigned char buzzerFinished = 1; // flag: 0 while playing
+const char * volatile buzzerSequence = 0;
+
+// declaring these globals as static means they won't conflict
+// with globals in other .cpp files that share the same name
+static volatile unsigned int buzzerTimeout = 0; // tracks buzzer time limit
+static volatile char play_mode_setting = PLAY_AUTOMATIC;
+
+extern volatile unsigned char buzzerFinished; // flag: 0 while playing
+extern const char * volatile buzzerSequence;
+
+
+static volatile unsigned char use_program_space; // boolean: true if we should
+ // use program space
+
+// music settings and defaults
+static volatile unsigned char octave = 4; // the current octave
+static volatile unsigned int whole_note_duration = 2000; // the duration of a whole note
+static volatile unsigned int note_type = 4; // 4 for quarter, etc
+static volatile unsigned int duration = 500; // the duration of a note in ms
+static volatile unsigned int volume = 15; // the note volume
+static volatile unsigned char staccato = 0; // true if playing staccato
+
+// staccato handling
+static volatile unsigned char staccato_rest_duration; // duration of a staccato rest,
+ // or zero if it is time to play a note
+
+static void nextNote();
+
+PololuBuzzer::PololuBuzzer(PinName pin) : _pwm(pin), playing(0) {
+ _pwm.write(0.0); // after creating it have to be off
+}
+
+ /** stop the beep instantaneous
+ * usually not used
+ */
+void PololuBuzzer::nobeep() {
+ _pwm.write(0.0);
+ buzzerFinished = 1;
+ buzzerSequence = 0;
+ if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+ nextNote();
+ else
+ isPlaying2(0);
+}
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+
+bool PololuBuzzer::isPlaying2(int request){
+ if (request == 0) {
+ playing = false;
+ }
+ else if (request == 1){
+ playing = true;
+ }
+ return playing;
+}
+
+void PololuBuzzer::beep(float freq, float time, float volume) {
+ _pwm.period(1.0/freq);
+ _pwm.write(0.5); // 50% duty cycle - beep on
+ float newTime = (time/500);
+ toff.attach(callback(this, &PololuBuzzer::nobeep), newTime);
+}
+
+// Timer2 overflow interrupt
+// ISR (TIMER2_OVF_vect)
+// {
+// if (buzzerTimeout-- == 0)
+// {
+// DISABLE_TIMER_INTERRUPT();
+// sei(); // re-enable global interrupts (nextNote() is very slow)
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+// buzzerFinished = 1;
+// if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+// nextNote();
+// }
+// }
+
+
+// this is called by playFrequency()
+inline void PololuBuzzer::init()
+{
+ if (!buzzerInitialized)
+ {
+ buzzerInitialized = 1;
+ //init2();
+ }
+}
+
+// initializes timer4 (32U4) or timer2 (328P) for buzzer control
+// void PololuBuzzer::init2()
+// {
+// DISABLE_TIMER_INTERRUPT();
+
+// TCCR2A = 0x21; // bits 7 and 6 clear: normal port op., OC4A disconnected
+// // bit 5 set, 4 clear: clear OC2B on comp match when upcounting,
+// // set OC2B on comp match when downcounting
+// // bits 3 and 2: not used
+// // bit 1 clear, 0 set: combine with bit 3 of TCCR2B...
+
+// TCCR2B = 0x0B; // bit 7 clear: no force output compare for channel A
+// // bit 6 clear: no force output compare for channel B
+// // bits 5 and 4: not used
+// // bit 3 set: combine with bits 1 and 0 of TCCR2A to
+// // select waveform generation mode 5, phase-correct PWM,
+// // TOP = OCR2A, OCR2B set at TOP, TOV2 flag set at BOTTOM
+// // bit 2 clear, 1-0 set: timer clock = clkT2S/32
+
+ // This sets timer 2 to run in phase-correct PWM mode, where TOP = OCR2A,
+ // OCR2B is updated at TOP, TOV2 Flag is set on BOTTOM. OC2B is cleared
+ // on compare match when upcounting, set on compare match when downcounting;
+ // OC2A is disconnected.
+ // Note: if the PWM frequency and duty cycle are changed, the first
+ // cycle of the new frequency will be at the old duty cycle, since
+ // the duty cycle (OCR2B) is not updated until TOP.
+
+
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// BUZZER_DDR |= BUZZER; // buzzer pin set as an output
+// sei();
+// }
+
+
+// Set up the timer to play the desired frequency (in Hz or .1 Hz) for the
+// the desired duration (in ms). Allowed frequencies are 40 Hz to 10 kHz.
+// volume controls buzzer volume, with 15 being loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playFrequency(unsigned int freq, unsigned int dur,
+ unsigned char volume)
+{
+ init(); // initializes the buzzer if necessary
+ buzzerFinished = 0;
+
+ unsigned int timeout;
+ unsigned char multiplier = 1;
+
+ if (freq & DIV_BY_10) // if frequency's DIV_BY_10 bit is set
+ { // then the true frequency is freq/10
+ multiplier = 10; // (gives higher resolution for small freqs)
+ freq &= ~DIV_BY_10; // clear DIV_BY_10 bit
+ }
+
+ unsigned char min = 40 * multiplier;
+ if (freq < min) // min frequency allowed is 40 Hz
+ freq = min;
+ if (multiplier == 1 && freq > 10000)
+ freq = 10000; // max frequency allowed is 10kHz
+
+// unsigned int top;
+// unsigned char newCS2 = 2; // try prescaler divider of 8 first (minimum necessary for 10 kHz)
+// unsigned int divider = cs2_divider[newCS2];
+
+// // calculate necessary clock source and counter top value to get freq
+// top = (unsigned int)(((F_CPU/16 * multiplier) + (freq >> 1))/ freq);
+
+// while (top > 255)
+// {
+// divider = cs2_divider[++newCS2];
+// top = (unsigned int)(((F_CPU/2/divider * multiplier) + (freq >> 1))/ freq);
+// }
+
+ // set timeout (duration):
+ if (multiplier == 10)
+ freq = (freq + 5) / 10;
+
+ if (freq == 1000)
+ timeout = dur; // duration for silent notes is exact
+ else
+ timeout = (unsigned int)((long)dur * freq / 1000);
+
+ if (volume > 15)
+ volume = 15;
+
+ BuzzerValues buzz(freq,timeout,volume);
+ return buzz;
+
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts while writing to registers
+
+// TCCR2B = (TCCR2B & 0xF8) | newCS2; // select timer 2 clock prescaler
+// OCR2A = top; // set timer 2 pwm frequency
+// OCR2B = top >> (16 - volume); // set duty cycle (volume)
+// buzzerTimeout = timeout; // set buzzer duration
+
+// TIFR2 |= 0xFF; // clear any pending t2 overflow int.
+
+// ENABLE_TIMER_INTERRUPT();
+}
+
+
+
+// Determine the frequency for the specified note, then play that note
+// for the desired duration (in ms). This is done without using floats
+// and without having to loop. volume controls buzzer volume, with 15 being
+// loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playNote(unsigned char note, unsigned int dur,
+ unsigned char volume)
+{
+ // note = key + octave * 12, where 0 <= key < 12
+ // example: A4 = A + 4 * 12, where A = 9 (so A4 = 57)
+ // A note is converted to a frequency by the formula:
+ // Freq(n) = Freq(0) * a^n
+ // where
+ // Freq(0) is chosen as A4, which is 440 Hz
+ // and
+ // a = 2 ^ (1/12)
+ // n is the number of notes you are away from A4.
+ // One can see that the frequency will double every 12 notes.
+ // This function exploits this property by defining the frequencies of the
+ // 12 lowest notes allowed and then doubling the appropriate frequency
+ // the appropriate number of times to get the frequency for the specified
+ // note.
+
+ // if note = 16, freq = 41.2 Hz (E1 - lower limit as freq must be >40 Hz)
+ // if note = 57, freq = 440 Hz (A4 - central value of ET Scale)
+ // if note = 111, freq = 9.96 kHz (D#9 - upper limit, freq must be <10 kHz)
+ // if note = 255, freq = 1 kHz and buzzer is silent (silent note)
+
+ // The most significant bit of freq is the "divide by 10" bit. If set,
+ // the units for frequency are .1 Hz, not Hz, and freq must be divided
+ // by 10 to get the true frequency in Hz. This allows for an extra digit
+ // of resolution for low frequencies without the need for using floats.
+
+ unsigned int freq = 0;
+ unsigned char offset_note = note - 16;
+ BuzzerValues buzz;
+
+ if (note == SILENT_NOTE || volume == 0)
+ {
+ freq = 1000; // silent notes => use 1kHz freq (for cycle counter)
+ buzz.changeValue(freq,dur,volume);
+ // buzz = playFrequency(freq, dur, 0);
+ // return buzz;
+ }
+ else {
+ if (note <= 16)
+ offset_note = 0;
+ else if (offset_note > 95)
+ offset_note = 95;
+
+ unsigned char exponent = offset_note / 12;
+
+ // frequency table for the lowest 12 allowed notes
+ // frequencies are specified in tenths of a Hertz for added resolution
+ switch (offset_note - exponent * 12) // equivalent to (offset_note % 12)
+ {
+ case 0: // note E1 = 41.2 Hz
+ freq = 412;
+ break;
+ case 1: // note F1 = 43.7 Hz
+ freq = 437;
+ break;
+ case 2: // note F#1 = 46.3 Hz
+ freq = 463;
+ break;
+ case 3: // note G1 = 49.0 Hz
+ freq = 490;
+ break;
+ case 4: // note G#1 = 51.9 Hz
+ freq = 519;
+ break;
+ case 5: // note A1 = 55.0 Hz
+ freq = 550;
+ break;
+ case 6: // note A#1 = 58.3 Hz
+ freq = 583;
+ break;
+ case 7: // note B1 = 61.7 Hz
+ freq = 617;
+ break;
+ case 8: // note C2 = 65.4 Hz
+ freq = 654;
+ break;
+ case 9: // note C#2 = 69.3 Hz
+ freq = 693;
+ break;
+ case 10: // note D2 = 73.4 Hz
+ freq = 734;
+ break;
+ case 11: // note D#2 = 77.8 Hz
+ freq = 778;
+ break;
+ }
+ if (exponent < 7)
+ {
+ freq = freq << exponent; // frequency *= 2 ^ exponent
+ if (exponent > 1) // if the frequency is greater than 160 Hz
+ freq = (freq + 5) / 10; // we don't need the extra resolution
+ else
+ freq += DIV_BY_10; // else keep the added digit of resolution
+ }
+ else
+ freq = (freq * 64 + 2) / 5; // == freq * 2^7 / 10 without int overflow
+
+ if (volume > 15)
+ volume = 15;
+ buzz = playFrequency(freq, dur, volume); // set buzzer this freq/duration
+ }
+ return buzz;
+}
+
+
+
+// Returns 1 if the buzzer is currently playing, otherwise it returns 0
+unsigned char PololuBuzzer::isPlaying()
+{
+ return !buzzerFinished || buzzerSequence != 0;
+}
+
+
+// Plays the specified sequence of notes. If the play mode is
+// PLAY_AUTOMATIC, the sequence of notes will play with no further
+// action required by the user. If the play mode is PLAY_CHECK,
+// the user will need to call playCheck() in the main loop to initiate
+// the playing of each new note in the sequence. The play mode can
+// be changed while the sequence is playing.
+// This is modeled after the PLAY commands in GW-BASIC, with just a
+// few differences.
+//
+// The notes are specified by the characters C, D, E, F, G, A, and
+// B, and they are played by default as "quarter notes" with a
+// length of 500 ms. This corresponds to a tempo of 120
+// beats/min. Other durations can be specified by putting a number
+// immediately after the note. For example, C8 specifies C played as an
+// eighth note, with half the duration of a quarter note. The special
+// note R plays a rest (no sound).
+//
+// Various control characters alter the sound:
+// '>' plays the next note one octave higher
+//
+// '<' plays the next note one octave lower
+//
+// '+' or '#' after a note raises any note one half-step
+//
+// '-' after a note lowers any note one half-step
+//
+// '.' after a note "dots" it, increasing the length by
+// 50%. Each additional dot adds half as much as the
+// previous dot, so that "A.." is 1.75 times the length of
+// "A".
+//
+// 'O' followed by a number sets the octave (default: O4).
+//
+// 'T' followed by a number sets the tempo (default: T120).
+//
+// 'L' followed by a number sets the default note duration to
+// the type specified by the number: 4 for quarter notes, 8
+// for eighth notes, 16 for sixteenth notes, etc.
+// (default: L4)
+//
+// 'V' followed by a number from 0-15 sets the music volume.
+// (default: V15)
+//
+// 'MS' sets all subsequent notes to play staccato - each note is played
+// for 1/2 of its allotted time, followed by an equal period
+// of silence.
+//
+// 'ML' sets all subsequent notes to play legato - each note is played
+// for its full length. This is the default setting.
+//
+// '!' resets all persistent settings to their defaults.
+//
+// The following plays a c major scale up and back down:
+// play("L16 V8 cdefgab>cbagfedc");
+//
+// Here is an example from Bach:
+// play("T240 L8 a gafaeada c+adaeafa <aa<bac#ada c#adaeaf4");
+void PololuBuzzer::play(const char *notes)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes;
+ use_program_space = 0;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+void PololuBuzzer::playFromProgramSpace(const char *notes_p)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes_p;
+ use_program_space = 1;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+
+// // stop all sound playback immediately
+// void PololuBuzzer::stopPlaying()
+// {
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts
+
+
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// buzzerFinished = 1;
+// buzzerSequence = 0;
+// }
+
+// Gets the current character, converting to lower-case and skipping
+// spaces. For any spaces, this automatically increments sequence!
+static char currentCharacter()
+{
+ char c = 0;
+ do
+ {
+ // if(use_program_space)
+ // c = pgm_read_byte(buzzerSequence);
+ // else
+ c = *buzzerSequence;
+
+ if(c >= 'A' && c <= 'Z')
+ c += 'a'-'A';
+ } while(c == ' ' && (buzzerSequence ++));
+
+ return c;
+}
+
+// Returns the numerical argument specified at buzzerSequence[0] and
+// increments sequence to point to the character immediately after the
+// argument.
+static unsigned int getNumber()
+{
+ unsigned int arg = 0;
+
+ // read all digits, one at a time
+ char c = currentCharacter();
+ while(c >= '0' && c <= '9')
+ {
+ arg *= 10;
+ arg += c-'0';
+ buzzerSequence ++;
+ c = currentCharacter();
+ }
+
+ return arg;
+}
+
+static void nextNote()
+{
+ unsigned char note = 0;
+ unsigned char rest = 0;
+ unsigned char tmp_octave = octave; // the octave for this note
+ unsigned int tmp_duration; // the duration of this note
+ unsigned int dot_add;
+
+ char c; // temporary variable
+
+ // if we are playing staccato, after every note we play a rest
+ if(staccato && staccato_rest_duration)
+ {
+ PololuBuzzer::playNote(SILENT_NOTE, staccato_rest_duration, 0);
+ staccato_rest_duration = 0;
+ return;
+ }
+
+ parse_character:
+
+ // Get current character
+ c = currentCharacter();
+ buzzerSequence ++;
+
+ // Interpret the character.
+ switch(c)
+ {
+ case '>':
+ // shift the octave temporarily up
+ tmp_octave ++;
+ goto parse_character;
+ case '<':
+ // shift the octave temporarily down
+ tmp_octave --;
+ goto parse_character;
+ case 'a':
+ note = NOTE_A(0);
+ break;
+ case 'b':
+ note = NOTE_B(0);
+ break;
+ case 'c':
+ note = NOTE_C(0);
+ break;
+ case 'd':
+ note = NOTE_D(0);
+ break;
+ case 'e':
+ note = NOTE_E(0);
+ break;
+ case 'f':
+ note = NOTE_F(0);
+ break;
+ case 'g':
+ note = NOTE_G(0);
+ break;
+ case 'l':
+ // set the default note duration
+ note_type = getNumber();
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'm':
+ // set music staccato or legato
+ if(currentCharacter() == 'l')
+ staccato = false;
+ else
+ {
+ staccato = true;
+ staccato_rest_duration = 0;
+ }
+ buzzerSequence ++;
+ goto parse_character;
+ case 'o':
+ // set the octave permanently
+ octave = tmp_octave = getNumber();
+ goto parse_character;
+ case 'r':
+ // Rest - the note value doesn't matter.
+ rest = 1;
+ break;
+ case 't':
+ // set the tempo
+ whole_note_duration = 60*400/getNumber()*10;
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'v':
+ // set the volume
+ volume = getNumber();
+ goto parse_character;
+ case '!':
+ // reset to defaults
+ octave = 4;
+ whole_note_duration = 2000;
+ note_type = 4;
+ duration = 500;
+ volume = 15;
+ staccato = 0;
+ // reset temp variables that depend on the defaults
+ tmp_octave = octave;
+ tmp_duration = duration;
+ goto parse_character;
+ default:
+ buzzerSequence = 0;
+ return;
+ }
+
+ note += tmp_octave*12;
+
+ // handle sharps and flats
+ c = currentCharacter();
+ while(c == '+' || c == '#')
+ {
+ buzzerSequence ++;
+ note ++;
+ c = currentCharacter();
+ }
+ while(c == '-')
+ {
+ buzzerSequence ++;
+ note --;
+ c = currentCharacter();
+ }
+
+ // set the duration of just this note
+ tmp_duration = duration;
+
+ // If the input is 'c16', make it a 16th note, etc.
+ if(c > '0' && c < '9')
+ tmp_duration = whole_note_duration/getNumber();
+
+ // Handle dotted notes - the first dot adds 50%, and each
+ // additional dot adds 50% of the previous dot.
+ dot_add = tmp_duration/2;
+ while(currentCharacter() == '.')
+ {
+ buzzerSequence ++;
+ tmp_duration += dot_add;
+ dot_add /= 2;
+ }
+
+ if(staccato)
+ {
+ staccato_rest_duration = tmp_duration / 2;
+ tmp_duration -= staccato_rest_duration;
+ }
+
+ // this will re-enable the timer overflow interrupt
+ PololuBuzzer::playNote(rest ? SILENT_NOTE : note, tmp_duration, volume);
+// wait_ms(tmp_duration);
+}
+
+
+// This puts play() into a mode where instead of advancing to the
+// next note in the sequence automatically, it waits until the
+// function playCheck() is called. The idea is that you can
+// put playCheck() in your main loop and avoid potential
+// delays due to the note sequence being checked in the middle of
+// a time sensitive calculation. It is recommended that you use
+// this function if you are doing anything that can't tolerate
+// being interrupted for more than a few microseconds.
+// Note that the play mode can be changed while a sequence is being
+// played.
+//
+// Usage: playMode(PLAY_AUTOMATIC) makes it automatic (the
+// default), playMode(PLAY_CHECK) sets it to a mode where you have
+// to call playCheck().
+void PololuBuzzer::playMode(unsigned char mode)
+{
+ play_mode_setting = mode;
+
+ // We want to check to make sure that we didn't miss a note if we
+ // are going out of play-check mode.
+ if(mode == PLAY_AUTOMATIC)
+ playCheck();
+}
+
+
+// Checks whether it is time to start another note, and starts
+// it if so. If it is not yet time to start the next note, this method
+// returns without doing anything. Call this as often as possible
+// in your main loop to avoid delays between notes in the sequence.
+//
+// Returns true if it is still playing.
+unsigned char PololuBuzzer::playCheck()
+{
+ if(buzzerFinished && buzzerSequence != 0)
+ nextNote();
+ return buzzerSequence != 0;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/PololuBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,126 @@
+#pragma once
+#include "mbed.h"
+
+#ifndef MBED_BEEP_H
+#define MBED_BEEP_H
+
+/*! \brief Specifies that the sequence of notes will play with no further action
+ * required by the user. */
+#define PLAY_AUTOMATIC 0
+
+
+/*! \brief Specified that the user will need to call `playCheck()` regularly. */
+#define PLAY_CHECK 1
+
+// n
+// Equal Tempered Scale is given by f = f * a
+// n o
+//
+// where f is chosen as A above middle C (A4) at f = 440 Hz
+// o o
+// and a is given by the twelfth root of 2 (~1.059463094359)
+
+/*! \anchor note_macros
+ *
+ * \name Note Macros
+ * \a x specifies the octave of the note
+ * @{
+ */
+#define NOTE_C(x) ( 0 + (x)*12)
+#define NOTE_C_SHARP(x) ( 1 + (x)*12)
+#define NOTE_D_FLAT(x) ( 1 + (x)*12)
+#define NOTE_D(x) ( 2 + (x)*12)
+#define NOTE_D_SHARP(x) ( 3 + (x)*12)
+#define NOTE_E_FLAT(x) ( 3 + (x)*12)
+#define NOTE_E(x) ( 4 + (x)*12)
+#define NOTE_F(x) ( 5 + (x)*12)
+#define NOTE_F_SHARP(x) ( 6 + (x)*12)
+#define NOTE_G_FLAT(x) ( 6 + (x)*12)
+#define NOTE_G(x) ( 7 + (x)*12)
+#define NOTE_G_SHARP(x) ( 8 + (x)*12)
+#define NOTE_A_FLAT(x) ( 8 + (x)*12)
+#define NOTE_A(x) ( 9 + (x)*12)
+#define NOTE_A_SHARP(x) (10 + (x)*12)
+#define NOTE_B_FLAT(x) (10 + (x)*12)
+#define NOTE_B(x) (11 + (x)*12)
+
+/*! \brief silences buzzer for the note duration */
+#define SILENT_NOTE 0xFF
+
+/*! \brief frequency bit that indicates Hz/10<br>
+ * e.g. \a frequency = `(445 | DIV_BY_10)` gives a frequency of 44.5 Hz
+ */
+#define DIV_BY_10 (1 << 15)
+/*! @} */
+
+namespace mbed {
+
+class BuzzerValues{
+ private:
+ float freq;
+ float dur;
+ float vol;
+ public:
+ BuzzerValues(float frequency, float duration, float volume);
+ BuzzerValues();
+ void changeValue(float frequency, float duration, float volume);
+ float returnFrequency();
+ float returnDuration();
+ float returnVolume();
+};
+
+/* Class: PololuBuzzer
+ * A class which uses pwm to controle a beeper to generate sounds.
+ */
+class PololuBuzzer {
+private:
+ PwmOut _pwm;
+ Timeout toff;
+ bool playing;
+
+public:
+
+/** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+ PololuBuzzer (PinName pin);
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+ void beep (float frequency, float time, float volume);
+
+/** stop the beep instantaneous
+ * usually not used
+ */
+ void nobeep();
+
+ static BuzzerValues playFrequency(unsigned int freq, unsigned int duration,
+ unsigned char volume);
+
+ static BuzzerValues playNote(unsigned char note, unsigned int duration,
+ unsigned char volume);
+
+ static void play(const char *sequence);
+
+ static void playFromProgramSpace(const char *sequence);
+
+ static void playMode(unsigned char mode);
+
+ static unsigned char playCheck();
+
+ static unsigned char isPlaying();
+
+ bool isPlaying2(int request);
+
+ //static void stopPlaying();
+
+ // initializes timer for buzzer control
+// static void init2();
+ static void init();
+ };
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/Pushbutton.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,151 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+#include "Pushbutton.h"
+#include "mbed.h"
+#include "millis.h"
+
+// \cond
+
+PushbuttonStateMachine::PushbuttonStateMachine()
+{
+ state = 0;
+}
+
+// state 0: The value is considered to be true.
+// state 1: The value was considered to be true, but there
+// was a recent false reading so it might be falling.
+// state 2: The value is considered to be false.
+// state 3: The value was considered to be false, but there
+// was a recent true reading so it might be rising.
+//
+// The transition from state 3 to state 0 is the point where we
+// have successfully detected a rising edge an we return true.
+//
+// The prevTimeMillis variable holds the last time that we
+// transitioned to states 1 or 3.
+bool PushbuttonStateMachine::getSingleDebouncedRisingEdge(bool value)
+{
+ uint16_t timeMillis = millis();
+
+ switch (state)
+ {
+ case 0:
+ // If value is false, proceed to the next state.
+ if (!value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 1;
+ }
+ break;
+
+ case 1:
+ if (value)
+ {
+ // The value is true or bouncing, so go back to previous (initial)
+ // state.
+ state = 0;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still false, so
+ // proceed to the next state.
+ state = 2;
+ }
+ break;
+
+ case 2:
+ // If the value is true, proceed to the next state.
+ if (value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 3;
+ }
+ break;
+
+ case 3:
+ if (!value)
+ {
+ // The value is false or bouncing, so go back to previous state.
+ state = 2;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still true, so
+ // go back to the initial state and report this rising edge.
+ state = 0;
+ return true;
+ }
+ break;
+ }
+
+ return false;
+}
+
+// \endcond
+
+void PushbuttonBase::waitForPress()
+{
+ do
+ {
+ while (!isPressed()); // wait for button to be pressed
+ wait_us(10000); // debounce the button press
+ }
+ while (!isPressed()); // if button isn't still pressed, loop
+}
+
+void PushbuttonBase::waitForRelease()
+{
+ do
+ {
+ while (isPressed()); // wait for button to be released
+ wait_us(10000); // debounce the button release
+ }
+ while (isPressed()); // if button isn't still released, loop
+}
+
+void PushbuttonBase::waitForButton()
+{
+ waitForPress();
+ waitForRelease();
+}
+
+bool PushbuttonBase::getSingleDebouncedPress()
+{
+ return pressState.getSingleDebouncedRisingEdge(isPressed());
+}
+
+bool PushbuttonBase::getSingleDebouncedRelease()
+{
+ return releaseState.getSingleDebouncedRisingEdge(!isPressed());
+}
+
+Pushbutton::Pushbutton(PinName pin, uint8_t pullUp, uint8_t defaultState)
+{
+ initialized = false;
+ _pin = pin;
+ _pullUp = pullUp;
+ _defaultState = defaultState;
+}
+
+void Pushbutton::init2()
+{
+ if (_pullUp == PULL_UP_ENABLED)
+ {
+ DigitalIn myPin(_pin, PullUp);
+ }
+ else
+ {
+ DigitalIn myPin(_pin);
+ //pinMode(_pin, INPUT); // high impedance
+ }
+
+ wait_us(5); // give pull-up time to stabilize
+}
+
+bool Pushbutton::isPressed()
+{
+ init(); // initialize if needed
+ DigitalIn myPin(_pin);
+
+ return myPin != _defaultState;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/Pushbutton.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,176 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file Pushbutton.h
+ *
+ * This is the main header file for the %Pushbutton library.
+ *
+ * For an overview of the library's features, see
+ * https://github.com/pololu/pushbutton-arduino. That is the main repository
+ * for the library, though copies of the library may exist in other
+ * repositories. */
+
+#pragma once
+
+#include "mbed.h"
+
+/*! Indicates the that pull-up resistor should be disabled. */
+#define PULL_UP_DISABLED 0
+
+/*! Indicates the that pull-up resistor should be enabled. */
+#define PULL_UP_ENABLED 1
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads low. */
+#define DEFAULT_STATE_LOW 0
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads high. */
+#define DEFAULT_STATE_HIGH 1
+
+/*! The pin used for the button on the
+ * [Zumo Shield for Arduino](http://www.pololu.com/product/2508).
+ *
+ * This does not really belong here in this general pushbutton library and will
+ * probably be removed in the future. */
+#define ZUMO_BUTTON PTD3
+
+// \cond
+/** \brief A state machine that detects when a boolean value changes from false
+ * to true, with debouncing.
+ *
+ * This should be considered a private implementation detail of the library.
+ */
+class PushbuttonStateMachine
+{
+public:
+
+ PushbuttonStateMachine();
+
+ /** This should be called repeatedly in a loop. It will return true once after
+ * each time it detects the given value changing from false to true. */
+ bool getSingleDebouncedRisingEdge(bool value);
+
+private:
+
+ uint8_t state;
+ uint16_t prevTimeMillis;
+};
+// \endcond
+
+/*! \brief General pushbutton class that handles debouncing.
+ *
+ * This is an abstract class used for interfacing with pushbuttons. It knows
+ * about debouncing, but it knows nothing about how to read the current state of
+ * the button. The functions in this class get the current state of the button
+ * by calling isPressed(), a virtual function which must be implemented in a
+ * subclass of PushbuttonBase, such as Pushbutton.
+ *
+ * Most users of this library do not need to directly use PushbuttonBase or even
+ * know that it exists. They can use Pushbutton instead.*/
+class PushbuttonBase
+{
+public:
+
+ /*! \brief Waits until the button is pressed and takes care of debouncing.
+ *
+ * This function waits until the button is in the pressed state and then
+ * returns. Note that if the button is already pressed when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForPress();
+
+ /*! \brief Waits until the button is released and takes care of debouncing.
+ *
+ * This function waits until the button is in the released state and then
+ * returns. Note that if the button is already released when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForRelease();
+
+ /*! \brief Waits until the button is pressed and then waits until the button
+ * is released, taking care of debouncing.
+ *
+ * This is equivalent to calling waitForPress() and then waitForRelease(). */
+ void waitForButton();
+
+ /*! \brief Uses a state machine to return true once after each time it detects
+ * the button moving from the released state to the pressed state.
+ *
+ * This is a non-blocking function that is meant to be called repeatedly in a
+ * loop. Each time it is called, it updates a state machine that monitors the
+ * state of the button. When it detects the button changing from the released
+ * state to the pressed state, with debouncing, it returns true. */
+ bool getSingleDebouncedPress();
+
+ /*! \brief Uses a state machine to return true once after each time it detects the button moving from the pressed state to the released state.
+ *
+ * This is just like getSingleDebouncedPress() except it has a separate state
+ * machine and it watches for when the button goes from the pressed state to
+ * the released state.
+ *
+ * There is no strict guarantee that every debounced button press event
+ * returned by getSingleDebouncedPress() will have a corresponding
+ * button release event returned by getSingleDebouncedRelease(); the two
+ * functions use independent state machines and sample the button at different
+ * times. */
+ bool getSingleDebouncedRelease();
+
+ /*! \brief indicates whether button is currently pressed without any debouncing.
+ *
+ * @return 1 if the button is pressed right now, 0 if it is not.
+ *
+ * This function must be implemented in a subclass of PushbuttonBase, such as
+ * Pushbutton. */
+ virtual bool isPressed() = 0;
+
+private:
+
+ PushbuttonStateMachine pressState;
+ PushbuttonStateMachine releaseState;
+};
+
+/** \brief Main class for interfacing with pushbuttons.
+ *
+ * This class can interface with any pushbutton whose state can be read with
+ * the `digitalRead` function, which is part of the Arduino core.
+ *
+ * See https://github.com/pololu/pushbutton-arduino for an overview
+ * of the different ways to use this class. */
+class Pushbutton : public PushbuttonBase
+{
+public:
+
+ /** Constructs a new instance of Pushbutton.
+ *
+ * @param pin The pin number of the pin. This is used as an argument to
+ * `pinMode` and `digitalRead`.
+ *
+ * @param pullUp Specifies whether the pin's internal pull-up resistor should
+ * be enabled. This should be either #PULL_UP_ENABLED (which is the default if
+ * the argument is omitted) or #PULL_UP_DISABLED.
+ *
+ * @param defaultState Specifies the voltage level that corresponds to the
+ * button's default (released) state. This should be either
+ * #DEFAULT_STATE_HIGH (which is the default if this argument is omitted) or
+ * #DEFAULT_STATE_LOW. */
+ Pushbutton(PinName pin, uint8_t pullUp = PULL_UP_ENABLED,
+ uint8_t defaultState = DEFAULT_STATE_HIGH);
+
+ virtual bool isPressed();
+
+private:
+
+ void init()
+ {
+ if (!initialized)
+ {
+ initialized = true;
+ init2();
+ }
+ }
+
+ void init2();
+
+ bool initialized;
+ PinName _pin;
+ bool _pullUp;
+ bool _defaultState;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/QTRSensors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,813 @@
+/*
+ QTRSensors.cpp - Arduino library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010.
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+
+#include "QTRSensors.h"
+#include <stdlib.h>
+#include "mbed.h"
+#include<string>
+
+Timer timer;
+// static BufferedSerial serial_port(USBTX, USBRX);
+
+// Base class data member initialization (called by derived class init())
+void QTRSensors::init(unsigned char *pins, unsigned char numSensors,
+ unsigned char emitterPin, bool analog = false)
+{
+ calibratedMinimumOn=0;
+ calibratedMaximumOn=0;
+ calibratedMinimumOff=0;
+ calibratedMaximumOff=0;
+
+ _lastValue=0;
+
+ if (numSensors > QTR_MAX_SENSORS)
+ _numSensors = QTR_MAX_SENSORS;
+ else
+ _numSensors = numSensors;
+
+
+ if (_pins == 0) {
+ _pins = (PinName *)malloc(sizeof(PinName)*_numSensors);
+ if (_pins == 0)
+ return;
+ }
+
+ unsigned char i;
+ // Copy parameter values to local storage
+ PinName currentPin;
+ for (i = 0; i < _numSensors; i++) {
+ switch(pins[i]){
+ case 1:
+ currentPin = PTB23;
+ break;
+ case 2:
+ currentPin = PTB11;
+ break;
+ case 3:
+ currentPin = PTD2;
+ break;
+ case 4:
+ currentPin = PTB2;
+ break;
+ case 5:
+ currentPin = PTB10;
+ break;
+ case 6:
+ currentPin = PTA2;
+ break;
+ }
+ _pins[i] = currentPin;
+ }
+
+ _qtrPins.reserve(_numSensors);
+
+ for (int i = 0; i < _numSensors; i++) {
+ _qtrPins.push_back(new DigitalInOut(_pins[i]));
+ }
+
+ // Serial pc(USBTX, USBRX);
+ // unsigned char j;
+ // for (j = 0; j < numSensors; j++)
+ // {
+ // std::string s = std::to_string(_pins[j]);
+ // const char * c = s.c_str();
+ // pc.printf(c);
+ // pc.printf("\n");
+ // }
+
+ // // Allocate the arrays
+ // // Allocate Space for Calibrated Maximum On Values
+ // calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOn == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOn[i] = 0;
+
+ // // Allocate Space for Calibrated Maximum Off Values
+ // calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOff == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOff[i] = 0;
+
+ // // Allocate Space for Calibrated Minimum On Values
+ // calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOn == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOn[i] = _maxValue;
+
+ // // Allocate Space for Calibrated Minimum Off Values
+ // calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOff == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOff[i] = _maxValue;
+
+ // emitter pin is used for DigitalOut
+ // So we create a DigitalOut on that pin
+ _emitterPin = emitterPin;
+ if(_emitterPin != 255){
+ if(emitterPin == 2){
+ _emitter = new DigitalOut(PTB9);
+ }
+ }
+
+ // If we have an Analog Sensor then we wil used AnalogIn on the pins provided
+ // We use a Vector for our collection of pins
+ // Here we reserve space for the pins
+ // _analog = analog;
+ // if (_analog) {
+ // _qtrAIPins.reserve(_numSensors);
+ // } else {
+ // // Not analog - so we use _qtrPins (which is a Vector on DigitalInOut)
+ // _qtrPins.reserve(_numSensors);
+ // }
+ // // Create the pins and push onto the vectors
+ // for (i = 0; i < _numSensors; i++) {
+ // if (analog) {
+ // _qtrAIPins.push_back(new AnalogIn(_pins[i]));
+ // } else {
+ // _qtrPins.push_back(new DigitalInOut(_pins[i]));
+ // }
+ // }
+
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in abstract units,
+// with higher values corresponding to lower reflectance (e.g. a black
+// surface or a void).
+void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode)
+{
+ unsigned int off_values[QTR_MAX_SENSORS];
+ unsigned char i;
+
+
+ if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF) {
+ emittersOn();
+ } else {
+ emittersOff();
+ }
+
+ readPrivate(sensor_values);
+
+ emittersOff();
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF) {
+ readPrivate(off_values);
+
+ for(i=0; i<_numSensors; i++) {
+ sensor_values[i] += _maxValue - off_values[i];
+ }
+ }
+}
+
+
+// Turn the IR LEDs off and on. This is mainly for use by the
+// read method, and calling these functions before or
+// after the reading the sensors will have no effect on the
+// readings, but you may wish to use these for testing purposes.
+void QTRSensors::emittersOff()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(0);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+void QTRSensors::emittersOn()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(1);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+// Resets the calibration.
+void QTRSensors::resetCalibration()
+{
+ unsigned char i;
+ for(i=0; i<_numSensors; i++) {
+ if(calibratedMinimumOn)
+ calibratedMinimumOn[i] = _maxValue;
+ if(calibratedMinimumOff)
+ calibratedMinimumOff[i] = _maxValue;
+ if(calibratedMaximumOn)
+ calibratedMaximumOn[i] = 0;
+ if(calibratedMaximumOff)
+ calibratedMaximumOff[i] = 0;
+ }
+}
+
+// Reads the sensors 10 times and uses the results for
+// calibration. The sensor values are not returned; instead, the
+// maximum and minimum values found over time are stored internally
+// and used for the readCalibrated() method.
+void QTRSensors::calibrate(unsigned char readMode)
+{
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) {
+ calibrateOnOrOff(&calibratedMinimumOn,
+ &calibratedMaximumOn,
+ QTR_EMITTERS_ON);
+ }
+
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) {
+ calibrateOnOrOff(&calibratedMinimumOff,
+ &calibratedMaximumOff,
+ QTR_EMITTERS_OFF);
+ }
+}
+
+void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum,
+ unsigned int **calibratedMaximum,
+ unsigned char readMode)
+{
+ int i;
+ unsigned int sensor_values[16];
+ unsigned int max_sensor_values[16];
+ unsigned int min_sensor_values[16];
+
+ // initialisation of calibrated sensor values moved to init()
+
+ // Allocate the arrays if necessary.
+ if(*calibratedMaximum == 0)
+ {
+ *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMaximum == 0)
+ return;
+
+ // Initialize the max and min calibrated values to values that
+ // will cause the first reading to update them.
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMaximum)[i] = 0;
+ }
+ if(*calibratedMinimum == 0)
+ {
+ *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMinimum == 0)
+ return;
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMinimum)[i] = _maxValue;
+ }
+
+ int j;
+ for(j=0; j<10; j++) {
+ read(sensor_values,readMode);
+ for(i=0; i<_numSensors; i++) {
+ // set the max we found THIS time
+ if(j == 0 || max_sensor_values[i] < sensor_values[i])
+ max_sensor_values[i] = sensor_values[i];
+
+ // set the min we found THIS time
+ if(j == 0 || min_sensor_values[i] > sensor_values[i])
+ min_sensor_values[i] = sensor_values[i];
+ }
+ }
+
+ // record the min and max calibration values
+ for(i=0; i<_numSensors; i++) {
+ if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i]
+ (*calibratedMaximum)[i] = min_sensor_values[i];
+ if(max_sensor_values[i] < (*calibratedMinimum)[i])
+ (*calibratedMinimum)[i] = max_sensor_values[i];
+ }
+
+}
+
+
+// Returns values calibrated to a value between 0 and 1000, where
+// 0 corresponds to the minimum value read by calibrate() and 1000
+// corresponds to the maximum value. Calibration values are
+// stored separately for each sensor, so that differences in the
+// sensors are accounted for automatically.
+void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode)
+{
+ int i;
+
+ // if not calibrated, do nothing
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
+ if(!calibratedMinimumOff || !calibratedMaximumOff)
+ return;
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
+ if(!calibratedMinimumOn || !calibratedMaximumOn)
+ return;
+
+ // read the needed values
+ read(sensor_values,readMode);
+
+ for(i=0; i<_numSensors; i++) {
+ unsigned int calmin,calmax;
+ unsigned int denominator;
+
+ // find the correct calibration
+ if(readMode == QTR_EMITTERS_ON) {
+ calmax = calibratedMaximumOn[i];
+ calmin = calibratedMinimumOn[i];
+ } else if(readMode == QTR_EMITTERS_OFF) {
+ calmax = calibratedMaximumOff[i];
+ calmin = calibratedMinimumOff[i];
+ } else { // QTR_EMITTERS_ON_AND_OFF
+
+ if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
+ calmin = _maxValue;
+ else
+ calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue
+
+ if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal
+ calmax = _maxValue;
+ else
+ calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue
+ }
+
+ denominator = calmax - calmin;
+
+ signed int x = 0;
+ if(denominator != 0)
+ x = (((signed long)sensor_values[i]) - calmin)
+ * 1000 / denominator;
+ if(x < 0)
+ x = 0;
+ else if(x > 1000)
+ x = 1000;
+ sensor_values[i] = x;
+ }
+
+}
+
+
+// Operates the same as read calibrated, but also returns an
+// estimated position of the robot with respect to a line. The
+// estimate is made using a weighted average of the sensor indices
+// multiplied by 1000, so that a return value of 0 indicates that
+// the line is directly below sensor 0, a return value of 1000
+// indicates that the line is directly below sensor 1, 2000
+// indicates that it's below sensor 2000, etc. Intermediate
+// values indicate that the line is between two sensors. The
+// formula is:
+//
+// 0*value0 + 1000*value1 + 2000*value2 + ...
+// --------------------------------------------
+// value0 + value1 + value2 + ...
+//
+// By default, this function assumes a dark line (high values)
+// surrounded by white (low values). If your line is light on
+// black, set the optional second argument white_line to true. In
+// this case, each sensor value will be replaced by (1000-value)
+// before the averaging.
+int QTRSensors::readLine(unsigned int *sensor_values,
+ unsigned char readMode, unsigned char white_line)
+{
+ unsigned char i, on_line = 0;
+ unsigned long avg; // this is for the weighted total, which is long
+ // before division
+ unsigned int sum; // this is for the denominator which is <= 64000
+ // static int last_value=0; // assume initially that the line is left.
+
+ readCalibrated(sensor_values, readMode);
+
+ avg = 0;
+ sum = 0;
+
+ for(i=0; i<_numSensors; i++) {
+ int value = sensor_values[i];
+ // printf("Sensor %d: %d\n", i, value);
+ if(white_line)
+ value = 1000-value;
+
+ // keep track of whether we see the line at all
+ if(value > 200) {
+ on_line = 1;
+ // printf("On line: %d\n", value);
+ }
+
+ // only average in values that are above a noise threshold
+ if(value > 50) {
+ avg += (long)(value) * (i * 1000);
+ sum += value;
+ }
+ }
+
+ if(!on_line) {
+ // If it last read to the left of center, return 0.
+ if(_lastValue < (_numSensors-1)*1000/2)
+ return 0;
+
+ // If it last read to the right of center, return the max.
+ else
+ return (_numSensors-1)*1000;
+
+ }
+
+ // printf("Average: %lu\n", avg);
+ // printf("Sum: %d\n", sum);
+
+ _lastValue = avg/sum;
+
+ // printf("Last value: %d\n", _lastValue);
+
+ return _lastValue;
+}
+
+
+
+// Derived RC class constructors
+QTRSensorsRC::QTRSensorsRC()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsRC::QTRSensorsRC(unsigned char* pins,
+ unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ init(pins, numSensors, timeout, emitterPin);
+}
+
+
+// The array 'pins' contains the Arduino pin number for each sensor.
+
+// 'numSensors' specifies the length of the 'pins' array (i.e. the
+// number of QTR-RC sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'timeout' specifies the length of time in microseconds beyond
+// which you consider the sensor reading completely black. That is to say,
+// if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+// and the reading for that pin will be considered full black.
+// It is recommended that you set timeout to be between 1000 and
+// 3000 us, depending on things like the height of your sensors and
+// ambient lighting. Using timeout allows you to shorten the
+// duration of a sensor-reading cycle while still maintaining
+// useful analog measurements of reflectance
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsRC::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned int timeout, unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin, false);
+
+ _maxValue = timeout;
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// ...
+// The values returned are in microseconds and range from 0 to
+// timeout (as specified in the constructor).
+void QTRSensorsRC::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i;
+ // Serial pc(USBTX, USBRX);
+
+ if (_pins == 0)
+ return;
+
+ // DigitalInOut pinout1(_pins[0]);
+ // DigitalInOut pinout2(_pins[1]);
+ // DigitalInOut pinout3(_pins[2]);
+ // DigitalInOut pinout4(_pins[3]);
+ // DigitalInOut pinout5(_pins[4]);
+ // DigitalInOut pinout6(_pins[5]);
+
+ for(i = 0; i < _numSensors; i++) {
+ sensor_values[i] = _maxValue;
+ _qtrPins[i]->output();
+ _qtrPins[i]->write(1); // make sensor line an output and drive high
+ }
+
+ // for(i = 0; i < _numSensors; i++) {
+ // sensor_values[i] = _maxValue;
+ // switch(i){
+ // case 0:
+ // pinout1.output(); // make sensor line an output and drive high
+ // pinout1.write(1);
+ // break;
+ // case 1:
+ // pinout2.output(); // make sensor line an output and drive high
+ // pinout2.write(1);
+ // break;
+ // case 2:
+ // pinout3.output(); // make sensor line an output and drive high
+ // pinout3.write(1);
+ // break;
+ // case 3:
+ // pinout4.output(); // make sensor line an output and drive high
+ // pinout4.write(1);
+ // break;
+ // case 4:
+ // pinout5.output(); // make sensor line an output and drive high
+ // pinout5.write(1);
+ // break;
+ // case 5:
+ // pinout6.output(); // make sensor line an output and drive high
+ // pinout6.write(1);
+ // break;
+ // }
+ // }
+
+ wait_us(10); // charge lines for 10 us
+
+ for(i = 0; i < _numSensors; i++) {
+ _qtrPins[i]->input();
+ _qtrPins[i]->mode(PullNone);
+ }
+
+ // for(i = 0; i < _numSensors; i++) {
+ // // important: disable internal pull-up!
+ // // ??? do we need to change the mode: _qtrPins[i]->mode(OpenDrain);
+ // // or just change mode to input
+ // // mbed documentation is not clear and I do not have a test sensor
+ // switch(i){
+ // case 0:
+ // pinout1.input(); // make sensor line an output and drive high
+ // pinout1.mode(PullNone);
+ // break;
+ // case 1:
+ // pinout2.input(); // make sensor line an output and drive high
+ // pinout2.mode(PullNone);
+ // break;
+ // case 2:
+ // pinout3.input(); // make sensor line an output and drive high
+ // pinout3.mode(PullNone);
+ // break;
+ // case 3:
+ // pinout4.input(); // make sensor line an output and drive high
+ // pinout4.mode(PullNone);
+ // break;
+ // case 4:
+ // pinout5.input(); // make sensor line an output and drive high
+ // pinout5.mode(PullNone);
+ // break;
+ // case 5:
+ // pinout6.input(); // make sensor line an output and drive high
+ // pinout6.mode(PullNone);
+ // break;
+ // }
+ // }
+
+ timer.start();
+ unsigned long startTime = timer.read_us();
+ while ((timer.read_us() - startTime) < _maxValue) {
+ unsigned int time = timer.read_us() - startTime;
+ for (i = 0; i < _numSensors; i++) {
+ if (_qtrPins[i]->read() == 0 && time < sensor_values[i])
+ sensor_values[i] = time;
+ }
+ }
+
+ // timer.start();
+ // unsigned long startTime = timer.read_us();
+ // while ((timer.read_us() - startTime) < _maxValue) {
+ // unsigned int time = timer.read_us() - startTime;
+ // for (i = 0; i < _numSensors; i++) {
+ // switch(i){
+ // case 0:
+ // if (pinout1.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 1:
+ // if (pinout2.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 2:
+ // if (pinout3.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 3:
+ // if (pinout4.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 4:
+ // if (pinout5.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // case 5:
+ // if (pinout6.read() == 0 && time < sensor_values[i]){
+ // sensor_values[i] = time;
+ // }
+ // break;
+ // }
+ // }
+ // wait_us(1);
+ // wait_us(300);
+ // timer.stop();
+ // pausedTime = pausedTime + 300;
+ // addTime = true;
+ // timer.start();
+ // printf("Timer2 %d\n", timer.read_us());
+ // pausedTime = 0;
+ timer.stop();
+ timer.reset();
+}
+
+
+// Derived Analog class constructors
+QTRSensorsAnalog::QTRSensorsAnalog()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsAnalog::QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ // this is analog - so use analog = true as a parameter
+
+ init(pins, numSensors, numSamplesPerSensor, emitterPin);
+}
+
+
+// the array 'pins' contains the Arduino analog pin assignment for each
+// sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+// Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+// and sensor 3 is on Arduino analog input 7.
+
+// 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+// number of QTR-A sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+// to average per channel (i.e. per sensor) for each reading. The total
+// number of analog-to-digital conversions performed will be equal to
+// numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+// perform a single analog-to-digital conversion, so:
+// if numSamplesPerSensor is 4 and numSensors is 6, it will take
+// 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+// Increasing this parameter increases noise suppression at the cost of
+// sample rate. The recommended value is 4.
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsAnalog::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin);
+
+ _numSamplesPerSensor = numSamplesPerSensor;
+ _maxValue = 1023; // this is the maximum returned by the A/D conversion
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in terms of a
+// 10-bit ADC average with higher values corresponding to lower
+// reflectance (e.g. a black surface or a void).
+void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i, j;
+
+ if (_pins == 0)
+ return;
+
+ // reset the values
+ for(i = 0; i < _numSensors; i++)
+ sensor_values[i] = 0;
+
+ for (j = 0; j < _numSamplesPerSensor; j++) {
+ for (i = 0; i < _numSensors; i++) {
+ sensor_values[i] += static_cast<unsigned int>(AnalogIn(_pins[i])); // add the conversion result
+ }
+ }
+
+ // get the rounded average of the readings for each sensor
+ for (i = 0; i < _numSensors; i++)
+ sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
+ _numSamplesPerSensor;
+}
+
+// the destructor frees up allocated memory
+QTRSensors::~QTRSensors()
+{
+ if(calibratedMinimumOff)
+ free(calibratedMinimumOff);
+ if(calibratedMinimumOn)
+ free(calibratedMinimumOn);
+ if(calibratedMaximumOff)
+ free(calibratedMaximumOff);
+ if(calibratedMaximumOn)
+ free(calibratedMaximumOn);
+ if (_pins)
+ free(_pins);
+
+ unsigned int i;
+ for (i = 0; i < _numSensors; i++) {
+ // if (_analog) {
+ // delete _qtrAIPins[i];
+ // } else {
+ delete _qtrPins[i];
+ // }
+ }
+ // if (_analog) {
+ // _qtrAIPins.clear();
+ // vector<AnalogIn *>().swap(_qtrAIPins);
+ // } else {
+ _qtrPins.clear();
+ vector<DigitalInOut *>().swap(_qtrPins);
+ // }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/QTRSensors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,283 @@
+/*
+ QTRSensors.h - Library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+#include "mbed.h"
+#include <vector>
+
+#ifndef QTRSensors_h
+#define QTRSensors_h
+
+#define QTR_EMITTERS_OFF 0
+#define QTR_EMITTERS_ON 1
+#define QTR_EMITTERS_ON_AND_OFF 2
+
+#define QTR_NO_EMITTER_PIN 255
+
+#define QTR_MAX_SENSORS 16
+#define HIGH 1
+#define LOW 0
+
+// This class cannot be instantiated directly (it has no constructor).
+// Instead, you should instantiate one of its two derived classes (either the
+// QTR-A or QTR-RC version, depending on the type of your sensor).
+class QTRSensors
+{
+ public:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in abstract units,
+ // with higher values corresponding to lower reflectance (e.g. a black
+ // surface or a void).
+ // If measureOffAndOn is true, measures the values with the
+ // emitters on AND off and returns on - (timeout - off). If this
+ // value is less than zero, it returns zero.
+ // This method will call the appropriate derived class's readPrivate(),
+ // which is defined as a virtual function in the base class and
+ // overridden by each derived class's own implementation.
+ void read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Turn the IR LEDs off and on. This is mainly for use by the
+ // read method, and calling these functions before or
+ // after the reading the sensors will have no effect on the
+ // readings, but you may wish to use these for testing purposes.
+ void emittersOff();
+ void emittersOn();
+
+ // Reads the sensors for calibration. The sensor values are
+ // not returned; instead, the maximum and minimum values found
+ // over time are stored internally and used for the
+ // readCalibrated() method.
+ void calibrate(unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Resets all calibration that has been done.
+ void resetCalibration();
+
+ // Returns values calibrated to a value between 0 and 1000, where
+ // 0 corresponds to the minimum value read by calibrate() and 1000
+ // corresponds to the maximum value. Calibration values are
+ // stored separately for each sensor, so that differences in the
+ // sensors are accounted for automatically.
+ void readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Operates the same as read calibrated, but also returns an
+ // estimated position of the robot with respect to a line. The
+ // estimate is made using a weighted average of the sensor indices
+ // multiplied by 1000, so that a return value of 0 indicates that
+ // the line is directly below sensor 0, a return value of 1000
+ // indicates that the line is directly below sensor 1, 2000
+ // indicates that it's below sensor 2000, etc. Intermediate
+ // values indicate that the line is between two sensors. The
+ // formula is:
+ //
+ // 0*value0 + 1000*value1 + 2000*value2 + ...
+ // --------------------------------------------
+ // value0 + value1 + value2 + ...
+ //
+ // By default, this function assumes a dark line (high values)
+ // surrounded by white (low values). If your line is light on
+ // black, set the optional second argument white_line to true. In
+ // this case, each sensor value will be replaced by (1000-value)
+ // before the averaging.
+ int readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char white_line = 0);
+
+ unsigned int *calibratedMinimumOn;
+ unsigned int *calibratedMaximumOn;
+ unsigned int *calibratedMinimumOff;
+ unsigned int *calibratedMaximumOff;
+
+ // Calibrated minumum and maximum values. These start at 1000 and
+ // 0, respectively, so that the very first sensor reading will
+ // update both of them.
+ //
+ // The pointers are unallocated until calibrate() is called, and
+ // then allocated to exactly the size required. Depending on the
+ // readMode argument to calibrate, only the On or Off values may
+ // be allocated, as required.
+ //
+ // These variables are made public so that you can use them for
+ // your own calculations and do things like saving the values to
+ // EEPROM, performing sanity checking, etc.
+
+ ~QTRSensors();
+
+ protected:
+
+ QTRSensors()
+ {
+
+ };
+
+ void init(unsigned char *pins, unsigned char numSensors, unsigned char emitterPin, bool);
+
+ bool _analog;
+ PinName *_pins;
+ unsigned char _numSensors;
+ unsigned char _emitterPin;
+ unsigned int _maxValue; // the maximum value returned by this function#
+ int _lastValue;
+ DigitalOut *_emitter;
+ std::vector<DigitalInOut *> _qtrPins;
+ // std::vector<AnalogIn *> _qtrAIPins;
+
+ private:
+
+ virtual void readPrivate(unsigned int *sensor_values) = 0;
+
+ // Handles the actual calibration. calibratedMinimum and
+ // calibratedMaximum are pointers to the requested calibration
+ // arrays, which will be allocated if necessary.
+ void calibrateOnOrOff(unsigned int **calibratedMaximum,
+ unsigned int **calibratedMinimum,
+ unsigned char readMode);
+};
+
+
+
+// Object to be used for QTR-1RC and QTR-8RC sensors
+class QTRSensorsRC : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsRC();
+
+ // this constructor just calls init()
+ QTRSensorsRC(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 4000, unsigned char emitterPin = 255);
+
+ // The array 'pins' contains the Arduino pin number for each sensor.
+
+ // 'numSensors' specifies the length of the 'pins' array (i.e. the
+ // number of QTR-RC sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'timeout' specifies the length of time in microseconds beyond
+ // which you consider the sensor reading completely black. That is to say,
+ // if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+ // and the reading for that pin will be considered full black.
+ // It is recommended that you set timeout to be between 1000 and
+ // 3000 us, depending on things like the height of your sensors and
+ // ambient lighting. Using timeout allows you to shorten the
+ // duration of a sensor-reading cycle while still maintaining
+ // useful analog measurements of reflectance
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 2000, unsigned char emitterPin = QTR_NO_EMITTER_PIN); // NC = Not Connected
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in microseconds.
+ void readPrivate(unsigned int *sensor_values);
+};
+
+
+
+// Object to be used for QTR-1A and QTR-8A sensors
+class QTRSensorsAnalog : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsAnalog();
+
+ // this constructor just calls init()
+ QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4,
+ unsigned char emitterPin = 255);
+
+ // the array 'pins' contains the Arduino analog pin assignment for each
+ // sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+ // Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+ // and sensor 3 is on Arduino analog input 7.
+
+ // 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+ // number of QTR-A sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+ // to average per channel (i.e. per sensor) for each reading. The total
+ // number of analog-to-digital conversions performed will be equal to
+ // numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+ // perform a single analog-to-digital conversion, so:
+ // if numSamplesPerSensor is 4 and numSensors is 6, it will take
+ // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+ // Increasing this parameter increases noise suppression at the cost of
+ // sample rate. The recommended value is 4.
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* analogPins, unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4, unsigned char emitterPin = QTR_NO_EMITTER_PIN);
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in terms of a
+ // 10-bit ADC average with higher values corresponding to lower
+ // reflectance (e.g. a black surface or a void).
+ void readPrivate(unsigned int *sensor_values);
+
+ unsigned char _numSamplesPerSensor;
+};
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/ZumoBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,29 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file ZumoBuzzer.h */
+
+#pragma once
+
+#include "PololuBuzzer.h"
+
+/*! \brief Plays beeps and music on the buzzer on the Zumo 32U4.
+ *
+ * This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on
+ * the Zumo 32U4 buzzer.
+ *
+ * Note durations are timed using a timer overflow interrupt
+ * (`TIMER4_OVF`), which will briefly interrupt execution of your
+ * main program at the frequency of the sound being played. In most cases, the
+ * interrupt-handling routine is very short (several microseconds). However,
+ * when playing a sequence of notes in `PLAY_AUTOMATIC` mode (the default mode)
+ * with the `play()` command, this interrupt takes much longer than normal
+ * (perhaps several hundred microseconds) every time it starts a new note. It is
+ * important to take this into account when writing timing-critical code.
+ */
+class ZumoBuzzer : public PololuBuzzer
+{
+ // This is a trivial subclass of PololuBuzzer; it exists because
+ // we wanted the ZumoShield class names to be consistent and we
+ // didn't just use a typedef to define it because that would make
+ // the Doxygen documentation harder to understand.
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/ZumoMotors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,84 @@
+/* File: DRV8835.cpp
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+
+#include "mbed.h"
+#include "ZumoMotors.h"
+
+
+ZumoMotors::ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin) :
+pwmL(pinPwmL),
+Lin(pinLin),
+pwmR(pinPwmR),
+Rin(pinRin)
+{
+ Lin = 0;
+ Rin = 0;
+ pwmL.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmL = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ pwmR.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmR = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::stop()
+{
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::motorL_stop(void)
+{
+ pwmL = 0;
+}
+
+void ZumoMotors::motorR_stop(void)
+{
+ pwmR = 0;
+}
+
+void ZumoMotors::setSpeeds(float Left,float Right)
+{
+ //Set Right Speed and Direction
+ if(Right<0)
+ {
+ motorR_rev(Right*-1);
+ } else {
+ motorR_fwd(Right);
+ }
+
+ //Set Left Speed and Direction
+ if(Left<0)
+ {
+ motorL_rev(Left*-1);
+ } else {
+ motorL_fwd(Left);
+ }
+}
+
+void ZumoMotors::motorL_fwd(float fPulsewidth)
+{
+ Lin = 0;
+ pwmL = fPulsewidth;
+}
+void ZumoMotors::motorL_rev(float fPulsewidth)
+{
+ Lin = 1;
+ pwmL = fPulsewidth;
+}
+
+void ZumoMotors::motorR_fwd(float fPulsewidth)
+{
+ Rin = 0;
+ pwmR = fPulsewidth;
+}
+void ZumoMotors::motorR_rev(float fPulsewidth)
+{
+ Rin = 1;
+ pwmR = fPulsewidth;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/ZumoMotors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,38 @@
+/* File: DRV8835.h
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+#ifndef __DRV8835_H__
+#define __DRV8835_H__
+
+#include "mbed.h"
+
+#define DRV8835_PWM_PERIOD_DEFAULT (0.00005) // 2 ms period // 50KHz (0.00002)
+#define DRV8835_PWM_PULSEWIDTH_DEFAULT (0.50) // 50% duty cycle
+
+class ZumoMotors
+{
+public:
+ ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin);
+
+ void motorL_stop(void);
+ void motorL_fwd(float fPulsewidth);
+ void motorL_rev(float fPulsewidth);
+ void motorR_stop(void);
+ void motorR_fwd(float fPulsewidth);
+ void motorR_rev(float fPulsewidth);
+ void setSpeeds(float Left,float Right);
+ void stop(void);
+
+private:
+ PwmOut pwmL;
+ DigitalOut Lin;
+ PwmOut pwmR;
+ DigitalOut Rin;
+};
+
+#endif /* __DRV8835_H__ */
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/ZumoReflectanceSensorArray.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,406 @@
+/*! \file ZumoReflectanceSensorArray.h
+ *
+ * See the ZumoReflectanceSensorArray class reference for more information about
+ * this library.
+ *
+ * \class ZumoReflectanceSensorArray ZumoReflectanceSensorArray.h
+ * \brief Read from reflectance sensor array
+ *
+ * ZumoReflectanceSensorArray provides an interface for using a
+ * [Zumo Reflectance Sensor Array](https://www.pololu.com/product/1419) connected
+ * to a Zumo robot. It provides access to the raw sensor values as well
+ * as to high level functions including calibration and line-tracking.
+ *
+ * For calibration, memory is allocated using the `malloc()` function. This
+ * conserves RAM: if all six sensors are calibrated with the emitters both on
+ * and off, a total of 48 bytes is dedicated to storing calibration values.
+ * However, for an application where only two sensors are used, and the
+ * emitters are always on during reads, only 4 bytes are required.
+ *
+ * Internally, it uses all standard Arduino functions such as
+ * `micros()` for timing and `digitalRead()` for getting the sensor values, so
+ * it should work on all Arduinos without conflicting with other libraries.
+ *
+ * ### Calibration ###
+ *
+ * ZumoReflectanceSensorArray allows you to use the `calibrate()`
+ * method to easily calibrate your sensors for the particular
+ * conditions it will encounter. Calibrating your sensors can lead to
+ * substantially more reliable sensor readings, which in turn can help
+ * simplify your code. As such, we recommend you build a calibration
+ * phase into your Zumo's initialization routine. This can be as
+ * simple as a fixed duration over which you repeatedly call the
+ * `calibrate()` method.
+ *
+ * During this calibration phase, you will need to expose each of your
+ * reflectance sensors to the lightest and darkest readings they will
+ * encounter. For example, if your Zumo is programmed to be a line
+ * follower, you will want to slide it across the line during the
+ * calibration phase so the each sensor can get a reading of how dark
+ * the line is and how light the ground is (or you can program it to
+ * automatically turn back and forth to pass all of the sensors over
+ * the line). The **SensorCalibration** example demonstrates a
+ * calibration routine.
+ *
+ * ### Reading the sensors
+ *
+ *
+ * ZumoReflectanceSensorArray gives you a number of different ways to
+ * read the sensors.
+ *
+ * - You can request raw sensor values using the `read()` method.
+ *
+ * - You can request calibrated sensor values using the `readCalibrated()`
+ * method. Calibrated sensor values will always range from 0 to 1000, with the
+ * extreme values corresponding to the most and least reflective surfaces
+ * encountered during calibration.
+ *
+ * - For line-detection applications, you can request the line location using
+ * the `readLine()` method. This function provides calibrated values
+ * for each sensor and returns an integer that tells you where it thinks the
+ * line is.
+ *
+ * ### Class inheritance ###
+ *
+ * The ZumoReflectanceSensorArray class is derived from the QTRSensorsRC class,
+ * which is in turn derived from the QTRSensors base class. The QTRSensorsRC and
+ * QTRSensors classes are part of the \ref QTRSensors.h "QTRSensors" library,
+ * which provides more general functionality for working with reflectance
+ * sensors and is included in the Zumo Shield Arduino Library as a dependency
+ * for this library.
+ *
+ * We recommend using ZumoReflectanceSensorArray instead of
+ * the \ref QTRSensors.h "QTRSensors" library when programming an Arduino on a
+ * Zumo. For documentation specific to the %QTRSensors library, please see its
+ * [user's guide](https://www.pololu.com/docs/0J19) on Pololu's website.
+ */
+
+#ifndef ZumoReflectanceSensorArray_h
+#define ZumoReflectanceSensorArray_h
+
+#include "QTRSensors.h"
+#include "mbed.h"
+
+#define ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN 2
+
+class ZumoReflectanceSensorArray : public QTRSensorsRC
+{
+ public:
+
+ /*! \brief Minimal constructor.
+ *
+ * This version of the constructor performs no initialization. If it is used,
+ * the user must call init() before using the methods in this class.
+ */
+ ZumoReflectanceSensorArray() {}
+
+ /*! \brief Constructor; initializes with given emitter pin and defaults for
+ * other settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char emitterPin)` with the specified
+ * emitter pin and default values for other settings.
+ */
+ ZumoReflectanceSensorArray(unsigned char emitterPin)
+ {
+ init(emitterPin);
+ }
+
+ /*! \brief Constructor; initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char * pins, unsigned char
+ * numSensors, unsigned int timeout, unsigned char emitterPin)` with all
+ * settings as given.
+ */
+ ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+
+ /*! \brief Initializes with given emitter pin and and defaults for other
+ * settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with the
+ * specified emitter pin. The other settings are set to default values: all
+ * six sensors on the array are active, and a timeout of 2000 microseconds is
+ * used.
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The "LED ON" jumper on the Zumo Reflectance Sensor Array must be
+ * configured correctly for this to work.) If the value `QTR_NO_EMITTER_PIN`
+ * (255) is used, you can leave the emitter pin disconnected and the IR
+ * emitters will always be on.
+ */
+ void init(unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ unsigned char sensorPins[] = { 1, 2, 3, 4, 5, 6 };
+ QTRSensorsRC::init(sensorPins, sizeof(sensorPins), 2000, emitterPin);
+ // pc.printf("hello");
+ }
+
+ /*! \brief Initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with all
+ * settings as given.
+ *
+ * The array \a pins should contain the Arduino digital pin numbers for each
+ * sensor.
+ *
+ * \a numSensors specifies the length of the \a pins array (the number of
+ * reflectance sensors you are using).
+ *
+ * \a timeout specifies the length of time in microseconds beyond which you
+ * consider the sensor reading completely black. That is to say, if the pulse
+ * length for a pin exceeds \a timeout, pulse timing will stop and the reading
+ * for that pin will be considered full black. It is recommended that you set
+ * \a timeout to be between 1000 and 3000 us, depending on factors like the
+ * height of your sensors and ambient lighting. This allows you to shorten the
+ * duration of a sensor-reading cycle while maintaining useful measurements of
+ * reflectance. If \a timeout is not specified, it defaults to 2000 us. (See
+ * the [product page](https://www.pololu.com/product/1419) for the Zumo
+ * Reflectance Sensor Array on Pololu's website for an overview of the
+ * sensors' principle of operation.)
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The corresponding connection should be made with the "LED ON"
+ * jumper on the Zumo Reflectance Sensor Array.) If the value
+ * `QTR_NO_EMITTER_PIN` (255) is used, you can leave the emitter pin
+ * disconnected and the IR emitters will always be on.
+ *
+ * This version of `%init()` can be useful if you only want to use a subset
+ * of the six reflectance sensors on the array. For example, using the
+ * outermost two sensors (on pins 4 and 5 by default) is usually enough for
+ * detecting the ring border in sumo competitions:
+ *
+ * ~~~{.ino}
+ * ZumoReflectanceSensorArray reflectanceSensors;
+ *
+ * ...
+ *
+ * reflectanceSensors.init((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ * Alternatively, you can use \ref ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+ * "a different constructor" to declare and initialize the object at the same
+ * time:
+ *
+ * ~~~{.ino}
+ *
+ * ZumoReflectanceSensorArray reflectanceSensors((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ */
+ void init(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+};
+
+// documentation for inherited functions
+
+/*! \fn void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+\memberof ZumoReflectanceSensorArray
+ * \brief Reads the raw sensor values into an array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * There **must** be space in the \a sensor_values array for as many values as
+ * there were sensors specified in the constructor. The values returned are
+ * measures of the reflectance in units of microseconds. They will be raw
+ * readings between 0 and the \a timeout argument (in units of microseconds)
+ * provided in the constructor (which defaults to 2000).
+ *
+ * The \a readMode argument specifies the kind of read that will be performed.
+ * Several options are defined:
+ *
+ * - `QTR_EMITTERS_OFF` specifies that the reading should be made without
+ * turning on the infrared (IR) emitters, in which case the reading represents
+ * ambient light levels near the sensor.
+ * - `QTR_EMITTERS_ON` specifies that the emitters should be turned on for the
+ * reading, which results in a measure of reflectance.
+ * - `QTR_EMITTERS_ON_AND_OFF` specifies that a reading should be made in both
+ * the on and off states. The values returned when this option is used are
+ * given by the formula **on + max − off**, where **on** is the reading
+ * with the emitters on, **off** is the reading with the emitters off, and
+ * **max** is the maximum sensor reading. This option can reduce the amount of
+ * interference from uneven ambient lighting.
+ *
+ * Note that emitter control will only work if you specify a valid emitter pin
+ * in the constructor and make the corresponding connection (with the "LED ON"
+ * jumper or otherwise).
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOff()
+ * \brief Turns the IR LEDs off.
+ *
+ * This is mainly for use by the `read()` method, and calling this function
+ * before or after reading the sensors will have no effect on the readings, but
+ * you might wish to use it for testing purposes. This method will only do
+ * something if the emitter pin specified in the constructor is valid (i.e. not
+ * `QTR_NO_EMITTER_PIN`) and the corresponding connection is made.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOn()
+ * \brief Turns the IR LEDs on.
+ * \copydetails emittersOff
+ */
+
+/*! \fn void QTRSensors::calibrate(unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Reads the sensors for calibration.
+ *
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * The sensor values read by this function are not returned; instead, the
+ * maximum and minimum values found over time are stored internally and used for
+ * the `readCalibrated()` method. You can access the calibration (i.e raw max
+ * and min sensor readings) through the public member pointers
+ * `calibratedMinimumOn`, `calibratedMaximumOn`, `calibratedMinimumOff`, and
+ * `calibratedMaximumOff`. Note that these pointers will point to arrays of
+ * length \a numSensors, as specified in the constructor, and they will only be
+ * allocated **after** `calibrate()` has been called. If you only calibrate with
+ * the emitters on, the calibration arrays that hold the off values will not be
+ * allocated.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::resetCalibration()
+ * \brief Resets all calibration that has been done.
+ *
+ * This function discards the calibration values that have been previously
+ * recorded, resetting the min and max values.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Returns sensor readings normalized to values between 0 and 1000.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * 0 corresponds to a reading that is less than or equal to the minimum value
+ * read by `calibrate()` and 1000 corresponds to a reading that is greater than
+ * or equal to the maximum value. Calibration values are stored separately for
+ * each sensor, so that differences in the sensors are accounted for
+ * automatically.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn int QTRSensors::readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char whiteLine = 0)
+ * \brief Returns an estimated position of a line under the sensor array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ * \param whiteLine 0 to detect a dark line on a light surface; 1 to detect
+ * a light line on a dark surface.
+ * \return An estimated line position.
+ *
+ * This function operates the same as `readCalibrated()`, but with a feature
+ * designed for line following: it returns an estimated position of the line.
+ * The estimate is made using a weighted average of the sensor indices
+ * multiplied by 1000, so that a return value of 0 indicates that the line is
+ * directly below sensor 0 (or was last seen by sensor 0 before being lost), a
+ * return value of 1000 indicates that the line is directly below sensor 1, 2000
+ * indicates that it's below sensor 2, etc. Intermediate values indicate that
+ * the line is between two sensors. The formula is:
+ *
+ * \f[
+ * \newcommand{sv}[1]{\mathtt{sensor_values[#1]}}
+ * \text{return value} =
+ * \frac{(0 \times \sv{0}) + (1000 \times \sv{1}) + (2000 \times \sv{2}) + \ldots}
+ * {\sv{0} + \sv{1} + \sv{2} + \ldots}
+ * \f]
+ *
+ * As long as your sensors aren't spaced too far apart relative to the line,
+ * this returned value is designed to be monotonic, which makes it great for use
+ * in closed-loop PID control. Additionally, this method remembers where it last
+ * saw the line, so if you ever lose the line to the left or the right, its line
+ * position will continue to indicate the direction you need to go to reacquire
+ * the line. For example, if sensor 5 is your rightmost sensor and you end up
+ * completely off the line to the left, this function will continue to return
+ * 5000.
+ *
+ * By default, this function assumes a dark line (high values) on a light
+ * background (low values). If your line is light on dark, set the optional
+ * second argument \a whiteLine to true. In this case, each sensor value will be
+ * replaced by the maximum possible value minus its actual value before the
+ * averaging.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+
+// documentation for inherited member variables
+
+/*!
+ * \property unsigned int * QTRSensors::calibratedMinimumOn
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * on.
+ *
+ * This pointer is unallocated and set to 0 until `calibrate()` is called, and
+ * then allocated to exactly the size required. Depending on the \a readMode
+ * argument to `calibrate()`, only the On or Off values might be allocated, as
+ * required. This variable is made public so that you can use the calibration
+ * values for your own calculations and do things like saving them to EEPROM,
+ * performing sanity checking, etc.
+ *
+ * The ZumoReflectanceSensorArray class inherits this variable from the
+ * QTRSensors class.
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOn
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * on.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMinimumOff
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOff
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ */
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/MazeSolver/millis.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "millis.h"
+/*
+ millis.cpp
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+volatile unsigned long _millis;
+
+void millisStart(void) {
+ SysTick_Config(SystemCoreClock / 1000);
+}
+
+extern "C" void SysTicks_Handler(void) {
+ _millis++;
+}
+
+unsigned long millis(void) {
+ return _millis;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/MazeSolver/millis.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,25 @@ +#ifndef MILLIS_H +#define MILLIS_H +/* + millis.h + Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com> + All rights reserved. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +void millisStart(void); +unsigned long millis(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/PushButtonExample/Pushbutton.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,151 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+#include "Pushbutton.h"
+#include "mbed.h"
+#include "millis.h"
+
+// \cond
+
+PushbuttonStateMachine::PushbuttonStateMachine()
+{
+ state = 0;
+}
+
+// state 0: The value is considered to be true.
+// state 1: The value was considered to be true, but there
+// was a recent false reading so it might be falling.
+// state 2: The value is considered to be false.
+// state 3: The value was considered to be false, but there
+// was a recent true reading so it might be rising.
+//
+// The transition from state 3 to state 0 is the point where we
+// have successfully detected a rising edge an we return true.
+//
+// The prevTimeMillis variable holds the last time that we
+// transitioned to states 1 or 3.
+bool PushbuttonStateMachine::getSingleDebouncedRisingEdge(bool value)
+{
+ uint16_t timeMillis = millis();
+
+ switch (state)
+ {
+ case 0:
+ // If value is false, proceed to the next state.
+ if (!value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 1;
+ }
+ break;
+
+ case 1:
+ if (value)
+ {
+ // The value is true or bouncing, so go back to previous (initial)
+ // state.
+ state = 0;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still false, so
+ // proceed to the next state.
+ state = 2;
+ }
+ break;
+
+ case 2:
+ // If the value is true, proceed to the next state.
+ if (value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 3;
+ }
+ break;
+
+ case 3:
+ if (!value)
+ {
+ // The value is false or bouncing, so go back to previous state.
+ state = 2;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still true, so
+ // go back to the initial state and report this rising edge.
+ state = 0;
+ return true;
+ }
+ break;
+ }
+
+ return false;
+}
+
+// \endcond
+
+void PushbuttonBase::waitForPress()
+{
+ do
+ {
+ while (!isPressed()); // wait for button to be pressed
+ wait_us(10000); // debounce the button press
+ }
+ while (!isPressed()); // if button isn't still pressed, loop
+}
+
+void PushbuttonBase::waitForRelease()
+{
+ do
+ {
+ while (isPressed()); // wait for button to be released
+ wait_us(10000); // debounce the button release
+ }
+ while (isPressed()); // if button isn't still released, loop
+}
+
+void PushbuttonBase::waitForButton()
+{
+ waitForPress();
+ waitForRelease();
+}
+
+bool PushbuttonBase::getSingleDebouncedPress()
+{
+ return pressState.getSingleDebouncedRisingEdge(isPressed());
+}
+
+bool PushbuttonBase::getSingleDebouncedRelease()
+{
+ return releaseState.getSingleDebouncedRisingEdge(!isPressed());
+}
+
+Pushbutton::Pushbutton(PinName pin, uint8_t pullUp, uint8_t defaultState)
+{
+ initialized = false;
+ _pin = pin;
+ _pullUp = pullUp;
+ _defaultState = defaultState;
+}
+
+void Pushbutton::init2()
+{
+ if (_pullUp == PULL_UP_ENABLED)
+ {
+ DigitalIn myPin(_pin, PullUp);
+ }
+ else
+ {
+ DigitalIn myPin(_pin);
+ //pinMode(_pin, INPUT); // high impedance
+ }
+
+ wait_us(5); // give pull-up time to stabilize
+}
+
+bool Pushbutton::isPressed()
+{
+ init(); // initialize if needed
+ DigitalIn myPin(_pin);
+
+ return myPin != _defaultState;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/PushButtonExample/Pushbutton.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,176 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file Pushbutton.h
+ *
+ * This is the main header file for the %Pushbutton library.
+ *
+ * For an overview of the library's features, see
+ * https://github.com/pololu/pushbutton-arduino. That is the main repository
+ * for the library, though copies of the library may exist in other
+ * repositories. */
+
+#pragma once
+
+#include "mbed.h"
+
+/*! Indicates the that pull-up resistor should be disabled. */
+#define PULL_UP_DISABLED 0
+
+/*! Indicates the that pull-up resistor should be enabled. */
+#define PULL_UP_ENABLED 1
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads low. */
+#define DEFAULT_STATE_LOW 0
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads high. */
+#define DEFAULT_STATE_HIGH 1
+
+/*! The pin used for the button on the
+ * [Zumo Shield for Arduino](http://www.pololu.com/product/2508).
+ *
+ * This does not really belong here in this general pushbutton library and will
+ * probably be removed in the future. */
+#define ZUMO_BUTTON PTD3
+
+// \cond
+/** \brief A state machine that detects when a boolean value changes from false
+ * to true, with debouncing.
+ *
+ * This should be considered a private implementation detail of the library.
+ */
+class PushbuttonStateMachine
+{
+public:
+
+ PushbuttonStateMachine();
+
+ /** This should be called repeatedly in a loop. It will return true once after
+ * each time it detects the given value changing from false to true. */
+ bool getSingleDebouncedRisingEdge(bool value);
+
+private:
+
+ uint8_t state;
+ uint16_t prevTimeMillis;
+};
+// \endcond
+
+/*! \brief General pushbutton class that handles debouncing.
+ *
+ * This is an abstract class used for interfacing with pushbuttons. It knows
+ * about debouncing, but it knows nothing about how to read the current state of
+ * the button. The functions in this class get the current state of the button
+ * by calling isPressed(), a virtual function which must be implemented in a
+ * subclass of PushbuttonBase, such as Pushbutton.
+ *
+ * Most users of this library do not need to directly use PushbuttonBase or even
+ * know that it exists. They can use Pushbutton instead.*/
+class PushbuttonBase
+{
+public:
+
+ /*! \brief Waits until the button is pressed and takes care of debouncing.
+ *
+ * This function waits until the button is in the pressed state and then
+ * returns. Note that if the button is already pressed when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForPress();
+
+ /*! \brief Waits until the button is released and takes care of debouncing.
+ *
+ * This function waits until the button is in the released state and then
+ * returns. Note that if the button is already released when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForRelease();
+
+ /*! \brief Waits until the button is pressed and then waits until the button
+ * is released, taking care of debouncing.
+ *
+ * This is equivalent to calling waitForPress() and then waitForRelease(). */
+ void waitForButton();
+
+ /*! \brief Uses a state machine to return true once after each time it detects
+ * the button moving from the released state to the pressed state.
+ *
+ * This is a non-blocking function that is meant to be called repeatedly in a
+ * loop. Each time it is called, it updates a state machine that monitors the
+ * state of the button. When it detects the button changing from the released
+ * state to the pressed state, with debouncing, it returns true. */
+ bool getSingleDebouncedPress();
+
+ /*! \brief Uses a state machine to return true once after each time it detects the button moving from the pressed state to the released state.
+ *
+ * This is just like getSingleDebouncedPress() except it has a separate state
+ * machine and it watches for when the button goes from the pressed state to
+ * the released state.
+ *
+ * There is no strict guarantee that every debounced button press event
+ * returned by getSingleDebouncedPress() will have a corresponding
+ * button release event returned by getSingleDebouncedRelease(); the two
+ * functions use independent state machines and sample the button at different
+ * times. */
+ bool getSingleDebouncedRelease();
+
+ /*! \brief indicates whether button is currently pressed without any debouncing.
+ *
+ * @return 1 if the button is pressed right now, 0 if it is not.
+ *
+ * This function must be implemented in a subclass of PushbuttonBase, such as
+ * Pushbutton. */
+ virtual bool isPressed() = 0;
+
+private:
+
+ PushbuttonStateMachine pressState;
+ PushbuttonStateMachine releaseState;
+};
+
+/** \brief Main class for interfacing with pushbuttons.
+ *
+ * This class can interface with any pushbutton whose state can be read with
+ * the `digitalRead` function, which is part of the Arduino core.
+ *
+ * See https://github.com/pololu/pushbutton-arduino for an overview
+ * of the different ways to use this class. */
+class Pushbutton : public PushbuttonBase
+{
+public:
+
+ /** Constructs a new instance of Pushbutton.
+ *
+ * @param pin The pin number of the pin. This is used as an argument to
+ * `pinMode` and `digitalRead`.
+ *
+ * @param pullUp Specifies whether the pin's internal pull-up resistor should
+ * be enabled. This should be either #PULL_UP_ENABLED (which is the default if
+ * the argument is omitted) or #PULL_UP_DISABLED.
+ *
+ * @param defaultState Specifies the voltage level that corresponds to the
+ * button's default (released) state. This should be either
+ * #DEFAULT_STATE_HIGH (which is the default if this argument is omitted) or
+ * #DEFAULT_STATE_LOW. */
+ Pushbutton(PinName pin, uint8_t pullUp = PULL_UP_ENABLED,
+ uint8_t defaultState = DEFAULT_STATE_HIGH);
+
+ virtual bool isPressed();
+
+private:
+
+ void init()
+ {
+ if (!initialized)
+ {
+ initialized = true;
+ init2();
+ }
+ }
+
+ void init2();
+
+ bool initialized;
+ PinName _pin;
+ bool _pullUp;
+ bool _defaultState;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/PushButtonExample/PushbuttonExample.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,98 @@
+/*
+ * This example uses the Pushbutton library to demonstrate three different
+ * methods for detecting a button press and release. It blinks the yellow
+ * user LED each time the Zumo user button is pressed and released. You can
+ * still use this demo without a Zumo Shield if you connect a normally-open
+ * momentary pushbutton between digital pin 12 and ground on your Arduino.
+ */
+
+//#include <Wire.h>
+
+#include "mbed.h"
+#include "millis.h"
+#include "ZumoShield.h"
+
+#define LED_PIN PTD1
+I2C i2c(I2C_SDA , I2C_SCL );
+
+/*
+ * Create a Pushbutton object for pin 12 (the Zumo user pushbutton pin)
+ * with default settings, which enables the internal pull-up on the pin
+ * and interprets a HIGH pin value as the default (unpressed) state of the
+ * button. (Optional arguments can be passed to the constructor to specify
+ * other button types and connection methods; see the documentation for
+ * details.)
+ */
+Pushbutton button(ZUMO_BUTTON);
+
+//void setup()
+//{
+// pinMode(LED_PIN, OUTPUT);
+//}
+
+void loop()
+{
+ /*
+ * Method 1: Directly read the state of the button with the isPressed()
+ * function. You must debounce the button yourself with this method.
+ */
+ do
+ {
+ while (!button.isPressed()); // wait for button to be pressed
+ wait_us(10000); // debounce the button press
+ }
+ while (!button.isPressed()); // if button isn't still pressed, loop
+
+ do
+ {
+ while (button.isPressed()); // wait for button to be released
+ wait_us(10000); // debounce the button release
+ }
+ while (button.isPressed()); // if button isn't still released, loop
+
+ // blink LED
+ DigitalOut ledPin(LED_PIN);
+ ledPin.write(1);
+ wait_us(200000);
+ ledPin.write(0);
+
+// /*
+// * Method 2: Use the waitForButton() function, which blocks and doesn't
+// * return until a button press and release are detected. This function
+// * takes care of button debouncing.
+// */
+// button.waitForButton();
+
+// // blink LED
+// ledPin.write(1);
+// wait_us(200000);
+// ledPin.write(0);
+
+// /*
+// * Method 3: Call the getSingleDebouncedRelease() function repeatedly
+// * in a loop, which returns true to report a single button release or false
+// * otherwise. This function takes care of button debouncing. If you have
+// * multiple buttons, you can call getSingleDebouncedRelease() or
+// * getSingleDebouncedPress() in a loop for all of them until one of them
+// * returns true.
+// */
+// while (1)
+// {
+// if (button.getSingleDebouncedRelease())
+// {
+// // blink LED
+// ledPin.write(1);
+// wait_us(200000);
+// ledPin.write(0);
+
+// break;
+// }
+// }
+}
+
+int main(){
+ millisStart();
+ while(1){
+ loop();
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/PushButtonExample/ZumoShield.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,9 @@ +#pragma once + +//#include "LSM303.h" +//#include "L3G.h" +#include "Pushbutton.h" +//#include "QTRSensors.h" +//#include "ZumoBuzzer.h" +//#include "ZumoMotors.h" +//#include "ZumoReflectanceSensorArray.h" \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/PushButtonExample/millis.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,38 @@
+#include "mbed.h"
+#include "millis.h"
+/*
+ millis.cpp
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+volatile unsigned long _millis;
+
+void millisStart(void) {
+ SysTick_Config(SystemCoreClock / 1000);
+}
+
+extern "C" {
+ void SysTicks_Handler(void) {
+ _millis++;
+ }
+}
+
+unsigned long millis(void) {
+ return _millis;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/PushButtonExample/millis.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,25 @@ +#ifndef MILLIS_H +#define MILLIS_H +/* + millis.h + Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com> + All rights reserved. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +void millisStart(void); +unsigned long millis(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SensorCalibration/QTRSensors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,783 @@
+/*
+ QTRSensors.cpp - Arduino library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010.
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+
+#include "QTRSensors.h"
+#include <stdlib.h>
+#include "mbed.h"
+#include<string>
+
+Timer timer;
+// static BufferedSerial serial_port(USBTX, USBRX);
+
+// Base class data member initialization (called by derived class init())
+void QTRSensors::init(unsigned char *pins, unsigned char numSensors,
+ unsigned char emitterPin, bool analog = false)
+{
+ calibratedMinimumOn=0;
+ calibratedMaximumOn=0;
+ calibratedMinimumOff=0;
+ calibratedMaximumOff=0;
+
+ _lastValue=0;
+
+ if (numSensors > QTR_MAX_SENSORS)
+ _numSensors = QTR_MAX_SENSORS;
+ else
+ _numSensors = numSensors;
+
+
+ if (_pins == 0) {
+ _pins = (PinName *)malloc(sizeof(PinName)*_numSensors);
+ if (_pins == 0)
+ return;
+ }
+
+ unsigned char i;
+ // Copy parameter values to local storage
+ PinName currentPin;
+ for (i = 0; i < _numSensors; i++) {
+ switch(pins[i]){
+ case 1:
+ currentPin = PTB23;
+ break;
+ case 2:
+ currentPin = PTB11;
+ break;
+ case 3:
+ currentPin = PTD2;
+ break;
+ case 4:
+ currentPin = PTB2;
+ break;
+ case 5:
+ currentPin = PTB10;
+ break;
+ case 6:
+ currentPin = PTA2;
+ break;
+ }
+ _pins[i] = currentPin;
+ }
+
+ // Serial pc(USBTX, USBRX);
+ // unsigned char j;
+ // for (j = 0; j < numSensors; j++)
+ // {
+ // std::string s = std::to_string(_pins[j]);
+ // const char * c = s.c_str();
+ // pc.printf(c);
+ // pc.printf("\n");
+ // }
+
+ // // Allocate the arrays
+ // // Allocate Space for Calibrated Maximum On Values
+ // calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOn == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOn[i] = 0;
+
+ // // Allocate Space for Calibrated Maximum Off Values
+ // calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOff == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOff[i] = 0;
+
+ // // Allocate Space for Calibrated Minimum On Values
+ // calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOn == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOn[i] = _maxValue;
+
+ // // Allocate Space for Calibrated Minimum Off Values
+ // calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOff == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOff[i] = _maxValue;
+
+ // emitter pin is used for DigitalOut
+ // So we create a DigitalOut on that pin
+ _emitterPin = emitterPin;
+ if(_emitterPin != 255){
+ if(emitterPin == 2){
+ _emitter = new DigitalOut(PTB9);
+ }
+ }
+
+ // If we have an Analog Sensor then we wil used AnalogIn on the pins provided
+ // We use a Vector for our collection of pins
+ // Here we reserve space for the pins
+ // _analog = analog;
+ // if (_analog) {
+ // _qtrAIPins.reserve(_numSensors);
+ // } else {
+ // // Not analog - so we use _qtrPins (which is a Vector on DigitalInOut)
+ // _qtrPins.reserve(_numSensors);
+ // }
+ // // Create the pins and push onto the vectors
+ // for (i = 0; i < _numSensors; i++) {
+ // if (analog) {
+ // _qtrAIPins.push_back(new AnalogIn(_pins[i]));
+ // } else {
+ // _qtrPins.push_back(new DigitalInOut(_pins[i]));
+ // }
+ // }
+
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in abstract units,
+// with higher values corresponding to lower reflectance (e.g. a black
+// surface or a void).
+void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode)
+{
+ unsigned int off_values[QTR_MAX_SENSORS];
+ unsigned char i;
+
+
+ if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF) {
+ emittersOn();
+ } else {
+ emittersOff();
+ }
+
+ readPrivate(sensor_values);
+
+ emittersOff();
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF) {
+ readPrivate(off_values);
+
+ for(i=0; i<_numSensors; i++) {
+ sensor_values[i] += _maxValue - off_values[i];
+ }
+ }
+}
+
+
+// Turn the IR LEDs off and on. This is mainly for use by the
+// read method, and calling these functions before or
+// after the reading the sensors will have no effect on the
+// readings, but you may wish to use these for testing purposes.
+void QTRSensors::emittersOff()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(0);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+void QTRSensors::emittersOn()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(1);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+// Resets the calibration.
+void QTRSensors::resetCalibration()
+{
+ unsigned char i;
+ for(i=0; i<_numSensors; i++) {
+ if(calibratedMinimumOn)
+ calibratedMinimumOn[i] = _maxValue;
+ if(calibratedMinimumOff)
+ calibratedMinimumOff[i] = _maxValue;
+ if(calibratedMaximumOn)
+ calibratedMaximumOn[i] = 0;
+ if(calibratedMaximumOff)
+ calibratedMaximumOff[i] = 0;
+ }
+}
+
+// Reads the sensors 10 times and uses the results for
+// calibration. The sensor values are not returned; instead, the
+// maximum and minimum values found over time are stored internally
+// and used for the readCalibrated() method.
+void QTRSensors::calibrate(unsigned char readMode)
+{
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) {
+ calibrateOnOrOff(&calibratedMinimumOn,
+ &calibratedMaximumOn,
+ QTR_EMITTERS_ON);
+ }
+
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) {
+ calibrateOnOrOff(&calibratedMinimumOff,
+ &calibratedMaximumOff,
+ QTR_EMITTERS_OFF);
+ }
+}
+
+void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum,
+ unsigned int **calibratedMaximum,
+ unsigned char readMode)
+{
+ int i;
+ unsigned int sensor_values[16];
+ unsigned int max_sensor_values[16];
+ unsigned int min_sensor_values[16];
+
+ // initialisation of calibrated sensor values moved to init()
+
+ // Allocate the arrays if necessary.
+ if(*calibratedMaximum == 0)
+ {
+ *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMaximum == 0)
+ return;
+
+ // Initialize the max and min calibrated values to values that
+ // will cause the first reading to update them.
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMaximum)[i] = 0;
+ }
+ if(*calibratedMinimum == 0)
+ {
+ *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMinimum == 0)
+ return;
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMinimum)[i] = _maxValue;
+ }
+
+ int j;
+ for(j=0; j<10; j++) {
+ read(sensor_values,readMode);
+ for(i=0; i<_numSensors; i++) {
+ // set the max we found THIS time
+ if(j == 0 || max_sensor_values[i] < sensor_values[i])
+ max_sensor_values[i] = sensor_values[i];
+
+ // set the min we found THIS time
+ if(j == 0 || min_sensor_values[i] > sensor_values[i])
+ min_sensor_values[i] = sensor_values[i];
+ }
+ }
+
+ // record the min and max calibration values
+ for(i=0; i<_numSensors; i++) {
+ if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i]
+ (*calibratedMaximum)[i] = min_sensor_values[i];
+ if(max_sensor_values[i] < (*calibratedMinimum)[i])
+ (*calibratedMinimum)[i] = max_sensor_values[i];
+ }
+
+}
+
+
+// Returns values calibrated to a value between 0 and 1000, where
+// 0 corresponds to the minimum value read by calibrate() and 1000
+// corresponds to the maximum value. Calibration values are
+// stored separately for each sensor, so that differences in the
+// sensors are accounted for automatically.
+void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode)
+{
+ int i;
+
+ // if not calibrated, do nothing
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
+ if(!calibratedMinimumOff || !calibratedMaximumOff)
+ return;
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
+ if(!calibratedMinimumOn || !calibratedMaximumOn)
+ return;
+
+ // read the needed values
+ read(sensor_values,readMode);
+
+ for(i=0; i<_numSensors; i++) {
+ unsigned int calmin,calmax;
+ unsigned int denominator;
+
+ // find the correct calibration
+ if(readMode == QTR_EMITTERS_ON) {
+ calmax = calibratedMaximumOn[i];
+ calmin = calibratedMinimumOn[i];
+ } else if(readMode == QTR_EMITTERS_OFF) {
+ calmax = calibratedMaximumOff[i];
+ calmin = calibratedMinimumOff[i];
+ } else { // QTR_EMITTERS_ON_AND_OFF
+
+ if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
+ calmin = _maxValue;
+ else
+ calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue
+
+ if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal
+ calmax = _maxValue;
+ else
+ calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue
+ }
+
+ denominator = calmax - calmin;
+
+ signed int x = 0;
+ if(denominator != 0)
+ x = (((signed long)sensor_values[i]) - calmin)
+ * 1000 / denominator;
+ if(x < 0)
+ x = 0;
+ else if(x > 1000)
+ x = 1000;
+ sensor_values[i] = x;
+ }
+
+}
+
+
+// Operates the same as read calibrated, but also returns an
+// estimated position of the robot with respect to a line. The
+// estimate is made using a weighted average of the sensor indices
+// multiplied by 1000, so that a return value of 0 indicates that
+// the line is directly below sensor 0, a return value of 1000
+// indicates that the line is directly below sensor 1, 2000
+// indicates that it's below sensor 2000, etc. Intermediate
+// values indicate that the line is between two sensors. The
+// formula is:
+//
+// 0*value0 + 1000*value1 + 2000*value2 + ...
+// --------------------------------------------
+// value0 + value1 + value2 + ...
+//
+// By default, this function assumes a dark line (high values)
+// surrounded by white (low values). If your line is light on
+// black, set the optional second argument white_line to true. In
+// this case, each sensor value will be replaced by (1000-value)
+// before the averaging.
+int QTRSensors::readLine(unsigned int *sensor_values,
+ unsigned char readMode, unsigned char white_line)
+{
+ unsigned char i, on_line = 0;
+ unsigned long avg; // this is for the weighted total, which is long
+ // before division
+ unsigned int sum; // this is for the denominator which is <= 64000
+ // static int last_value=0; // assume initially that the line is left.
+
+ readCalibrated(sensor_values, readMode);
+
+ avg = 0;
+ sum = 0;
+
+ for(i=0; i<_numSensors; i++) {
+ int value = sensor_values[i];
+ if(white_line)
+ value = 1000-value;
+
+ // keep track of whether we see the line at all
+ if(value > 200) {
+ on_line = 1;
+ }
+
+ // only average in values that are above a noise threshold
+ if(value > 50) {
+ avg += (long)(value) * (i * 1000);
+ sum += value;
+ }
+ }
+
+ if(!on_line) {
+ // If it last read to the left of center, return 0.
+ if(_lastValue < (_numSensors-1)*1000/2)
+ return 0;
+
+ // If it last read to the right of center, return the max.
+ else
+ return (_numSensors-1)*1000;
+
+ }
+
+ _lastValue = avg/sum;
+
+ return _lastValue;
+}
+
+
+
+// Derived RC class constructors
+QTRSensorsRC::QTRSensorsRC()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsRC::QTRSensorsRC(unsigned char* pins,
+ unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ init(pins, numSensors, timeout, emitterPin);
+}
+
+
+// The array 'pins' contains the Arduino pin number for each sensor.
+
+// 'numSensors' specifies the length of the 'pins' array (i.e. the
+// number of QTR-RC sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'timeout' specifies the length of time in microseconds beyond
+// which you consider the sensor reading completely black. That is to say,
+// if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+// and the reading for that pin will be considered full black.
+// It is recommended that you set timeout to be between 1000 and
+// 3000 us, depending on things like the height of your sensors and
+// ambient lighting. Using timeout allows you to shorten the
+// duration of a sensor-reading cycle while still maintaining
+// useful analog measurements of reflectance
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsRC::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned int timeout, unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin, false);
+
+ _maxValue = timeout;
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// ...
+// The values returned are in microseconds and range from 0 to
+// timeout (as specified in the constructor).
+void QTRSensorsRC::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i;
+ // Serial pc(USBTX, USBRX);
+
+ if (_pins == 0)
+ return;
+
+ DigitalInOut pinout1(_pins[0]);
+ DigitalInOut pinout2(_pins[1]);
+ DigitalInOut pinout3(_pins[2]);
+ DigitalInOut pinout4(_pins[3]);
+ DigitalInOut pinout5(_pins[4]);
+ DigitalInOut pinout6(_pins[5]);
+
+ for(i = 0; i < _numSensors; i++) {
+ sensor_values[i] = _maxValue;
+ switch(i){
+ case 0:
+ pinout1.output(); // make sensor line an output and drive high
+ pinout1.write(1);
+ break;
+ case 1:
+ pinout2.output(); // make sensor line an output and drive high
+ pinout2.write(1);
+ break;
+ case 2:
+ pinout3.output(); // make sensor line an output and drive high
+ pinout3.write(1);
+ break;
+ case 3:
+ pinout4.output(); // make sensor line an output and drive high
+ pinout4.write(1);
+ break;
+ case 4:
+ pinout5.output(); // make sensor line an output and drive high
+ pinout5.write(1);
+ break;
+ case 5:
+ pinout6.output(); // make sensor line an output and drive high
+ pinout6.write(1);
+ break;
+ }
+ }
+
+ wait_us(10); // charge lines for 10 us
+
+ for(i = 0; i < _numSensors; i++) {
+ // important: disable internal pull-up!
+ // ??? do we need to change the mode: _qtrPins[i]->mode(OpenDrain);
+ // or just change mode to input
+ // mbed documentation is not clear and I do not have a test sensor
+ switch(i){
+ case 0:
+ pinout1.input(); // make sensor line an output and drive high
+ pinout1.mode(PullNone);
+ break;
+ case 1:
+ pinout2.input(); // make sensor line an output and drive high
+ pinout2.mode(PullNone);
+ break;
+ case 2:
+ pinout3.input(); // make sensor line an output and drive high
+ pinout3.mode(PullNone);
+ break;
+ case 3:
+ pinout4.input(); // make sensor line an output and drive high
+ pinout4.mode(PullNone);
+ break;
+ case 4:
+ pinout5.input(); // make sensor line an output and drive high
+ pinout5.mode(PullNone);
+ break;
+ case 5:
+ pinout6.input(); // make sensor line an output and drive high
+ pinout6.mode(PullNone);
+ break;
+ }
+ }
+
+ // int pausedTime = 0;
+ // bool addTime = false;
+ // bool detectedLine = false;
+ // bool pauseWhile = false;
+ timer.start();
+ unsigned long startTime = timer.read_us();
+ while ((timer.read_us() - startTime) < _maxValue) {
+ unsigned int time = timer.read_us() - startTime;
+ for (i = 0; i < _numSensors; i++) {
+ switch(i){
+ case 0:
+ if (pinout1.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 1:
+ if (pinout2.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 2:
+ if (pinout3.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 3:
+ if (pinout4.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 4:
+ if (pinout5.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 5:
+ if (pinout6.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ }
+ }
+ // wait_us(1);
+ // wait_us(300);
+ // timer.stop();
+ // pausedTime = pausedTime + 300;
+ // addTime = true;
+ // timer.start();
+ }
+ // printf("Timer2 %d\n", timer.read_us());
+ // pausedTime = 0;
+ timer.stop();
+ timer.reset();
+}
+
+
+// Derived Analog class constructors
+QTRSensorsAnalog::QTRSensorsAnalog()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsAnalog::QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ // this is analog - so use analog = true as a parameter
+
+ init(pins, numSensors, numSamplesPerSensor, emitterPin);
+}
+
+
+// the array 'pins' contains the Arduino analog pin assignment for each
+// sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+// Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+// and sensor 3 is on Arduino analog input 7.
+
+// 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+// number of QTR-A sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+// to average per channel (i.e. per sensor) for each reading. The total
+// number of analog-to-digital conversions performed will be equal to
+// numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+// perform a single analog-to-digital conversion, so:
+// if numSamplesPerSensor is 4 and numSensors is 6, it will take
+// 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+// Increasing this parameter increases noise suppression at the cost of
+// sample rate. The recommended value is 4.
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsAnalog::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin);
+
+ _numSamplesPerSensor = numSamplesPerSensor;
+ _maxValue = 1023; // this is the maximum returned by the A/D conversion
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in terms of a
+// 10-bit ADC average with higher values corresponding to lower
+// reflectance (e.g. a black surface or a void).
+void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i, j;
+
+ if (_pins == 0)
+ return;
+
+ // reset the values
+ for(i = 0; i < _numSensors; i++)
+ sensor_values[i] = 0;
+
+ for (j = 0; j < _numSamplesPerSensor; j++) {
+ for (i = 0; i < _numSensors; i++) {
+ sensor_values[i] += static_cast<unsigned int>(AnalogIn(_pins[i])); // add the conversion result
+ }
+ }
+
+ // get the rounded average of the readings for each sensor
+ for (i = 0; i < _numSensors; i++)
+ sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
+ _numSamplesPerSensor;
+}
+
+// the destructor frees up allocated memory
+QTRSensors::~QTRSensors()
+{
+ if(calibratedMinimumOff)
+ free(calibratedMinimumOff);
+ if(calibratedMinimumOn)
+ free(calibratedMinimumOn);
+ if(calibratedMaximumOff)
+ free(calibratedMaximumOff);
+ if(calibratedMaximumOn)
+ free(calibratedMaximumOn);
+ if (_pins)
+ free(_pins);
+ unsigned int i;
+ // for (i = 0; i < _numSensors; i++) {
+ // if (_analog) {
+ // delete _qtrAIPins[i];
+ // } else {
+ // delete _qtrPins[i];
+ // }
+ // }
+ // if (_analog) {
+ // _qtrAIPins.clear();
+ // vector<AnalogIn *>().swap(_qtrAIPins);
+ // } else {
+ // _qtrPins.clear();
+ // vector<DigitalInOut *>().swap(_qtrPins);
+ // }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SensorCalibration/QTRSensors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,283 @@
+/*
+ QTRSensors.h - Library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+#include "mbed.h"
+#include <vector>
+
+#ifndef QTRSensors_h
+#define QTRSensors_h
+
+#define QTR_EMITTERS_OFF 0
+#define QTR_EMITTERS_ON 1
+#define QTR_EMITTERS_ON_AND_OFF 2
+
+#define QTR_NO_EMITTER_PIN 255
+
+#define QTR_MAX_SENSORS 16
+#define HIGH 1
+#define LOW 0
+
+// This class cannot be instantiated directly (it has no constructor).
+// Instead, you should instantiate one of its two derived classes (either the
+// QTR-A or QTR-RC version, depending on the type of your sensor).
+class QTRSensors
+{
+ public:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in abstract units,
+ // with higher values corresponding to lower reflectance (e.g. a black
+ // surface or a void).
+ // If measureOffAndOn is true, measures the values with the
+ // emitters on AND off and returns on - (timeout - off). If this
+ // value is less than zero, it returns zero.
+ // This method will call the appropriate derived class's readPrivate(),
+ // which is defined as a virtual function in the base class and
+ // overridden by each derived class's own implementation.
+ void read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Turn the IR LEDs off and on. This is mainly for use by the
+ // read method, and calling these functions before or
+ // after the reading the sensors will have no effect on the
+ // readings, but you may wish to use these for testing purposes.
+ void emittersOff();
+ void emittersOn();
+
+ // Reads the sensors for calibration. The sensor values are
+ // not returned; instead, the maximum and minimum values found
+ // over time are stored internally and used for the
+ // readCalibrated() method.
+ void calibrate(unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Resets all calibration that has been done.
+ void resetCalibration();
+
+ // Returns values calibrated to a value between 0 and 1000, where
+ // 0 corresponds to the minimum value read by calibrate() and 1000
+ // corresponds to the maximum value. Calibration values are
+ // stored separately for each sensor, so that differences in the
+ // sensors are accounted for automatically.
+ void readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Operates the same as read calibrated, but also returns an
+ // estimated position of the robot with respect to a line. The
+ // estimate is made using a weighted average of the sensor indices
+ // multiplied by 1000, so that a return value of 0 indicates that
+ // the line is directly below sensor 0, a return value of 1000
+ // indicates that the line is directly below sensor 1, 2000
+ // indicates that it's below sensor 2000, etc. Intermediate
+ // values indicate that the line is between two sensors. The
+ // formula is:
+ //
+ // 0*value0 + 1000*value1 + 2000*value2 + ...
+ // --------------------------------------------
+ // value0 + value1 + value2 + ...
+ //
+ // By default, this function assumes a dark line (high values)
+ // surrounded by white (low values). If your line is light on
+ // black, set the optional second argument white_line to true. In
+ // this case, each sensor value will be replaced by (1000-value)
+ // before the averaging.
+ int readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char white_line = 0);
+
+ unsigned int *calibratedMinimumOn;
+ unsigned int *calibratedMaximumOn;
+ unsigned int *calibratedMinimumOff;
+ unsigned int *calibratedMaximumOff;
+
+ // Calibrated minumum and maximum values. These start at 1000 and
+ // 0, respectively, so that the very first sensor reading will
+ // update both of them.
+ //
+ // The pointers are unallocated until calibrate() is called, and
+ // then allocated to exactly the size required. Depending on the
+ // readMode argument to calibrate, only the On or Off values may
+ // be allocated, as required.
+ //
+ // These variables are made public so that you can use them for
+ // your own calculations and do things like saving the values to
+ // EEPROM, performing sanity checking, etc.
+
+ ~QTRSensors();
+
+ protected:
+
+ QTRSensors()
+ {
+
+ };
+
+ void init(unsigned char *pins, unsigned char numSensors, unsigned char emitterPin, bool);
+
+ bool _analog;
+ PinName *_pins;
+ unsigned char _numSensors;
+ unsigned char _emitterPin;
+ unsigned int _maxValue; // the maximum value returned by this function#
+ int _lastValue;
+ DigitalOut *_emitter;
+ // std::vector<DigitalInOut *> _qtrPins;
+ // std::vector<AnalogIn *> _qtrAIPins;
+
+ private:
+
+ virtual void readPrivate(unsigned int *sensor_values) = 0;
+
+ // Handles the actual calibration. calibratedMinimum and
+ // calibratedMaximum are pointers to the requested calibration
+ // arrays, which will be allocated if necessary.
+ void calibrateOnOrOff(unsigned int **calibratedMaximum,
+ unsigned int **calibratedMinimum,
+ unsigned char readMode);
+};
+
+
+
+// Object to be used for QTR-1RC and QTR-8RC sensors
+class QTRSensorsRC : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsRC();
+
+ // this constructor just calls init()
+ QTRSensorsRC(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 4000, unsigned char emitterPin = 255);
+
+ // The array 'pins' contains the Arduino pin number for each sensor.
+
+ // 'numSensors' specifies the length of the 'pins' array (i.e. the
+ // number of QTR-RC sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'timeout' specifies the length of time in microseconds beyond
+ // which you consider the sensor reading completely black. That is to say,
+ // if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+ // and the reading for that pin will be considered full black.
+ // It is recommended that you set timeout to be between 1000 and
+ // 3000 us, depending on things like the height of your sensors and
+ // ambient lighting. Using timeout allows you to shorten the
+ // duration of a sensor-reading cycle while still maintaining
+ // useful analog measurements of reflectance
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 2000, unsigned char emitterPin = QTR_NO_EMITTER_PIN); // NC = Not Connected
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in microseconds.
+ void readPrivate(unsigned int *sensor_values);
+};
+
+
+
+// Object to be used for QTR-1A and QTR-8A sensors
+class QTRSensorsAnalog : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsAnalog();
+
+ // this constructor just calls init()
+ QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4,
+ unsigned char emitterPin = 255);
+
+ // the array 'pins' contains the Arduino analog pin assignment for each
+ // sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+ // Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+ // and sensor 3 is on Arduino analog input 7.
+
+ // 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+ // number of QTR-A sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+ // to average per channel (i.e. per sensor) for each reading. The total
+ // number of analog-to-digital conversions performed will be equal to
+ // numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+ // perform a single analog-to-digital conversion, so:
+ // if numSamplesPerSensor is 4 and numSensors is 6, it will take
+ // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+ // Increasing this parameter increases noise suppression at the cost of
+ // sample rate. The recommended value is 4.
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* analogPins, unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4, unsigned char emitterPin = QTR_NO_EMITTER_PIN);
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in terms of a
+ // 10-bit ADC average with higher values corresponding to lower
+ // reflectance (e.g. a black surface or a void).
+ void readPrivate(unsigned int *sensor_values);
+
+ unsigned char _numSamplesPerSensor;
+};
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SensorCalibration/SensorCalibration.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,70 @@
+#include "mbed.h"
+#include "ZumoReflectanceSensorArray.h"
+
+ZumoReflectanceSensorArray reflectanceSensors;
+
+#define LED PTD1
+#define NUM_SENSORS 6
+unsigned int sensorValues[NUM_SENSORS];
+DigitalOut ledPin(LED, 1);
+Timer t;
+
+// main() runs in its own thread in the OS
+void setup()
+{
+
+ reflectanceSensors.init();
+ wait_us(500000);
+
+ t.start();
+
+ unsigned long startTime = t.read_ms();
+ while(t.read_ms() - startTime < 10000) // make the calibration take 10 seconds
+ {
+ reflectanceSensors.calibrate();
+ }
+ ledPin.write(0);
+
+ // To get raw sensor values instead, call:
+ //reflectanceSensors.read(sensorValues);
+
+ for (unsigned int i = 0; i < NUM_SENSORS; i++)
+ {
+ printf("%d", reflectanceSensors.calibratedMinimumOn[i]);
+ printf(" ");
+ }
+
+ printf("\n");
+
+ for (unsigned int i = 0; i < NUM_SENSORS; i++)
+ {
+ printf("%d", reflectanceSensors.calibratedMaximumOn[i]);
+ printf(" ");
+ }
+ printf("\n");
+ printf("\n");
+ wait_us(1000000);
+}
+
+void loop(){
+ unsigned int position = reflectanceSensors.readLine(sensorValues);
+
+ for (unsigned long i = 0; i < NUM_SENSORS; i++)
+ {
+ printf("%d", sensorValues[i]);
+ printf(" ");
+ }
+ printf(" \n");
+ printf("%d", position);
+
+ wait_us(250000);
+}
+
+int main(){
+ setup();
+ while(1){
+ loop();
+ }
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SensorCalibration/ZumoReflectanceSensorArray.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,406 @@
+/*! \file ZumoReflectanceSensorArray.h
+ *
+ * See the ZumoReflectanceSensorArray class reference for more information about
+ * this library.
+ *
+ * \class ZumoReflectanceSensorArray ZumoReflectanceSensorArray.h
+ * \brief Read from reflectance sensor array
+ *
+ * ZumoReflectanceSensorArray provides an interface for using a
+ * [Zumo Reflectance Sensor Array](https://www.pololu.com/product/1419) connected
+ * to a Zumo robot. It provides access to the raw sensor values as well
+ * as to high level functions including calibration and line-tracking.
+ *
+ * For calibration, memory is allocated using the `malloc()` function. This
+ * conserves RAM: if all six sensors are calibrated with the emitters both on
+ * and off, a total of 48 bytes is dedicated to storing calibration values.
+ * However, for an application where only two sensors are used, and the
+ * emitters are always on during reads, only 4 bytes are required.
+ *
+ * Internally, it uses all standard Arduino functions such as
+ * `micros()` for timing and `digitalRead()` for getting the sensor values, so
+ * it should work on all Arduinos without conflicting with other libraries.
+ *
+ * ### Calibration ###
+ *
+ * ZumoReflectanceSensorArray allows you to use the `calibrate()`
+ * method to easily calibrate your sensors for the particular
+ * conditions it will encounter. Calibrating your sensors can lead to
+ * substantially more reliable sensor readings, which in turn can help
+ * simplify your code. As such, we recommend you build a calibration
+ * phase into your Zumo's initialization routine. This can be as
+ * simple as a fixed duration over which you repeatedly call the
+ * `calibrate()` method.
+ *
+ * During this calibration phase, you will need to expose each of your
+ * reflectance sensors to the lightest and darkest readings they will
+ * encounter. For example, if your Zumo is programmed to be a line
+ * follower, you will want to slide it across the line during the
+ * calibration phase so the each sensor can get a reading of how dark
+ * the line is and how light the ground is (or you can program it to
+ * automatically turn back and forth to pass all of the sensors over
+ * the line). The **SensorCalibration** example demonstrates a
+ * calibration routine.
+ *
+ * ### Reading the sensors
+ *
+ *
+ * ZumoReflectanceSensorArray gives you a number of different ways to
+ * read the sensors.
+ *
+ * - You can request raw sensor values using the `read()` method.
+ *
+ * - You can request calibrated sensor values using the `readCalibrated()`
+ * method. Calibrated sensor values will always range from 0 to 1000, with the
+ * extreme values corresponding to the most and least reflective surfaces
+ * encountered during calibration.
+ *
+ * - For line-detection applications, you can request the line location using
+ * the `readLine()` method. This function provides calibrated values
+ * for each sensor and returns an integer that tells you where it thinks the
+ * line is.
+ *
+ * ### Class inheritance ###
+ *
+ * The ZumoReflectanceSensorArray class is derived from the QTRSensorsRC class,
+ * which is in turn derived from the QTRSensors base class. The QTRSensorsRC and
+ * QTRSensors classes are part of the \ref QTRSensors.h "QTRSensors" library,
+ * which provides more general functionality for working with reflectance
+ * sensors and is included in the Zumo Shield Arduino Library as a dependency
+ * for this library.
+ *
+ * We recommend using ZumoReflectanceSensorArray instead of
+ * the \ref QTRSensors.h "QTRSensors" library when programming an Arduino on a
+ * Zumo. For documentation specific to the %QTRSensors library, please see its
+ * [user's guide](https://www.pololu.com/docs/0J19) on Pololu's website.
+ */
+
+#ifndef ZumoReflectanceSensorArray_h
+#define ZumoReflectanceSensorArray_h
+
+#include "QTRSensors.h"
+#include "mbed.h"
+
+#define ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN 2
+
+class ZumoReflectanceSensorArray : public QTRSensorsRC
+{
+ public:
+
+ /*! \brief Minimal constructor.
+ *
+ * This version of the constructor performs no initialization. If it is used,
+ * the user must call init() before using the methods in this class.
+ */
+ ZumoReflectanceSensorArray() {}
+
+ /*! \brief Constructor; initializes with given emitter pin and defaults for
+ * other settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char emitterPin)` with the specified
+ * emitter pin and default values for other settings.
+ */
+ ZumoReflectanceSensorArray(unsigned char emitterPin)
+ {
+ init(emitterPin);
+ }
+
+ /*! \brief Constructor; initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char * pins, unsigned char
+ * numSensors, unsigned int timeout, unsigned char emitterPin)` with all
+ * settings as given.
+ */
+ ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+
+ /*! \brief Initializes with given emitter pin and and defaults for other
+ * settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with the
+ * specified emitter pin. The other settings are set to default values: all
+ * six sensors on the array are active, and a timeout of 2000 microseconds is
+ * used.
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The "LED ON" jumper on the Zumo Reflectance Sensor Array must be
+ * configured correctly for this to work.) If the value `QTR_NO_EMITTER_PIN`
+ * (255) is used, you can leave the emitter pin disconnected and the IR
+ * emitters will always be on.
+ */
+ void init(unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ unsigned char sensorPins[] = { 1, 2, 3, 4, 5, 6 };
+ QTRSensorsRC::init(sensorPins, sizeof(sensorPins), 2000, emitterPin);
+ // pc.printf("hello");
+ }
+
+ /*! \brief Initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with all
+ * settings as given.
+ *
+ * The array \a pins should contain the Arduino digital pin numbers for each
+ * sensor.
+ *
+ * \a numSensors specifies the length of the \a pins array (the number of
+ * reflectance sensors you are using).
+ *
+ * \a timeout specifies the length of time in microseconds beyond which you
+ * consider the sensor reading completely black. That is to say, if the pulse
+ * length for a pin exceeds \a timeout, pulse timing will stop and the reading
+ * for that pin will be considered full black. It is recommended that you set
+ * \a timeout to be between 1000 and 3000 us, depending on factors like the
+ * height of your sensors and ambient lighting. This allows you to shorten the
+ * duration of a sensor-reading cycle while maintaining useful measurements of
+ * reflectance. If \a timeout is not specified, it defaults to 2000 us. (See
+ * the [product page](https://www.pololu.com/product/1419) for the Zumo
+ * Reflectance Sensor Array on Pololu's website for an overview of the
+ * sensors' principle of operation.)
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The corresponding connection should be made with the "LED ON"
+ * jumper on the Zumo Reflectance Sensor Array.) If the value
+ * `QTR_NO_EMITTER_PIN` (255) is used, you can leave the emitter pin
+ * disconnected and the IR emitters will always be on.
+ *
+ * This version of `%init()` can be useful if you only want to use a subset
+ * of the six reflectance sensors on the array. For example, using the
+ * outermost two sensors (on pins 4 and 5 by default) is usually enough for
+ * detecting the ring border in sumo competitions:
+ *
+ * ~~~{.ino}
+ * ZumoReflectanceSensorArray reflectanceSensors;
+ *
+ * ...
+ *
+ * reflectanceSensors.init((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ * Alternatively, you can use \ref ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+ * "a different constructor" to declare and initialize the object at the same
+ * time:
+ *
+ * ~~~{.ino}
+ *
+ * ZumoReflectanceSensorArray reflectanceSensors((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ */
+ void init(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+};
+
+// documentation for inherited functions
+
+/*! \fn void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+\memberof ZumoReflectanceSensorArray
+ * \brief Reads the raw sensor values into an array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * There **must** be space in the \a sensor_values array for as many values as
+ * there were sensors specified in the constructor. The values returned are
+ * measures of the reflectance in units of microseconds. They will be raw
+ * readings between 0 and the \a timeout argument (in units of microseconds)
+ * provided in the constructor (which defaults to 2000).
+ *
+ * The \a readMode argument specifies the kind of read that will be performed.
+ * Several options are defined:
+ *
+ * - `QTR_EMITTERS_OFF` specifies that the reading should be made without
+ * turning on the infrared (IR) emitters, in which case the reading represents
+ * ambient light levels near the sensor.
+ * - `QTR_EMITTERS_ON` specifies that the emitters should be turned on for the
+ * reading, which results in a measure of reflectance.
+ * - `QTR_EMITTERS_ON_AND_OFF` specifies that a reading should be made in both
+ * the on and off states. The values returned when this option is used are
+ * given by the formula **on + max − off**, where **on** is the reading
+ * with the emitters on, **off** is the reading with the emitters off, and
+ * **max** is the maximum sensor reading. This option can reduce the amount of
+ * interference from uneven ambient lighting.
+ *
+ * Note that emitter control will only work if you specify a valid emitter pin
+ * in the constructor and make the corresponding connection (with the "LED ON"
+ * jumper or otherwise).
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOff()
+ * \brief Turns the IR LEDs off.
+ *
+ * This is mainly for use by the `read()` method, and calling this function
+ * before or after reading the sensors will have no effect on the readings, but
+ * you might wish to use it for testing purposes. This method will only do
+ * something if the emitter pin specified in the constructor is valid (i.e. not
+ * `QTR_NO_EMITTER_PIN`) and the corresponding connection is made.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOn()
+ * \brief Turns the IR LEDs on.
+ * \copydetails emittersOff
+ */
+
+/*! \fn void QTRSensors::calibrate(unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Reads the sensors for calibration.
+ *
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * The sensor values read by this function are not returned; instead, the
+ * maximum and minimum values found over time are stored internally and used for
+ * the `readCalibrated()` method. You can access the calibration (i.e raw max
+ * and min sensor readings) through the public member pointers
+ * `calibratedMinimumOn`, `calibratedMaximumOn`, `calibratedMinimumOff`, and
+ * `calibratedMaximumOff`. Note that these pointers will point to arrays of
+ * length \a numSensors, as specified in the constructor, and they will only be
+ * allocated **after** `calibrate()` has been called. If you only calibrate with
+ * the emitters on, the calibration arrays that hold the off values will not be
+ * allocated.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::resetCalibration()
+ * \brief Resets all calibration that has been done.
+ *
+ * This function discards the calibration values that have been previously
+ * recorded, resetting the min and max values.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Returns sensor readings normalized to values between 0 and 1000.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * 0 corresponds to a reading that is less than or equal to the minimum value
+ * read by `calibrate()` and 1000 corresponds to a reading that is greater than
+ * or equal to the maximum value. Calibration values are stored separately for
+ * each sensor, so that differences in the sensors are accounted for
+ * automatically.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn int QTRSensors::readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char whiteLine = 0)
+ * \brief Returns an estimated position of a line under the sensor array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ * \param whiteLine 0 to detect a dark line on a light surface; 1 to detect
+ * a light line on a dark surface.
+ * \return An estimated line position.
+ *
+ * This function operates the same as `readCalibrated()`, but with a feature
+ * designed for line following: it returns an estimated position of the line.
+ * The estimate is made using a weighted average of the sensor indices
+ * multiplied by 1000, so that a return value of 0 indicates that the line is
+ * directly below sensor 0 (or was last seen by sensor 0 before being lost), a
+ * return value of 1000 indicates that the line is directly below sensor 1, 2000
+ * indicates that it's below sensor 2, etc. Intermediate values indicate that
+ * the line is between two sensors. The formula is:
+ *
+ * \f[
+ * \newcommand{sv}[1]{\mathtt{sensor_values[#1]}}
+ * \text{return value} =
+ * \frac{(0 \times \sv{0}) + (1000 \times \sv{1}) + (2000 \times \sv{2}) + \ldots}
+ * {\sv{0} + \sv{1} + \sv{2} + \ldots}
+ * \f]
+ *
+ * As long as your sensors aren't spaced too far apart relative to the line,
+ * this returned value is designed to be monotonic, which makes it great for use
+ * in closed-loop PID control. Additionally, this method remembers where it last
+ * saw the line, so if you ever lose the line to the left or the right, its line
+ * position will continue to indicate the direction you need to go to reacquire
+ * the line. For example, if sensor 5 is your rightmost sensor and you end up
+ * completely off the line to the left, this function will continue to return
+ * 5000.
+ *
+ * By default, this function assumes a dark line (high values) on a light
+ * background (low values). If your line is light on dark, set the optional
+ * second argument \a whiteLine to true. In this case, each sensor value will be
+ * replaced by the maximum possible value minus its actual value before the
+ * averaging.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+
+// documentation for inherited member variables
+
+/*!
+ * \property unsigned int * QTRSensors::calibratedMinimumOn
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * on.
+ *
+ * This pointer is unallocated and set to 0 until `calibrate()` is called, and
+ * then allocated to exactly the size required. Depending on the \a readMode
+ * argument to `calibrate()`, only the On or Off values might be allocated, as
+ * required. This variable is made public so that you can use the calibration
+ * values for your own calculations and do things like saving them to EEPROM,
+ * performing sanity checking, etc.
+ *
+ * The ZumoReflectanceSensorArray class inherits this variable from the
+ * QTRSensors class.
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOn
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * on.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMinimumOff
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOff
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ */
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/FXOS8700Q.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,231 @@
+/* FXOS8700Q sensor driver
+ * Copyright (c) 2014-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "FXOS8700Q.h"
+
+const uint16_t uint14_max = 0x3FFF;
+void static inline normalize_14bits(int16_t &x)
+{
+ x = ((x) > (uint14_max/2)) ? (x - uint14_max) : (x);
+}
+
+static int16_t dummy_int16_t = 0;
+static float dummy_float = 0.0f;
+
+FXOS8700Q::FXOS8700Q(I2C &i2c, uint8_t addr)
+{
+ _i2c = &i2c;
+ _addr = addr;
+ // activate the peripheral
+ uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+ _i2c->frequency(400000);
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_M_CTRL_REG1;
+ data[1] = 0x1F;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_M_CTRL_REG2;
+ data[1] = 0x20;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_XYZ_DATA_CFG;
+ data[1] = 0x00;
+ writeRegs(data, 2);
+ data[0] = FXOS8700Q_CTRL_REG1;
+ data[1] = 0x1C;
+ writeRegs(data, 2);
+}
+
+FXOS8700Q::~FXOS8700Q()
+{
+ _i2c = 0;
+ _addr = 0;
+}
+
+void FXOS8700Q::readRegs(uint8_t addr, uint8_t *data, uint32_t len) const
+{
+ uint8_t t[1] = {addr};
+ _i2c->write(_addr, (char *)t, sizeof(t), true);
+ _i2c->read(_addr, (char *)data, len);
+}
+
+uint8_t FXOS8700Q::whoAmI() const
+{
+ uint8_t who_am_i = 0;
+ readRegs(FXOS8700Q_WHOAMI, &who_am_i, sizeof(who_am_i));
+ return who_am_i;
+}
+
+void FXOS8700Q::writeRegs(uint8_t * data, uint32_t len) const
+{
+ _i2c->write(_addr, (char *)data, len);
+}
+
+
+int16_t FXOS8700Q::getSensorAxis(uint8_t addr) const
+{
+ uint8_t res[2];
+ readRegs(addr, res, sizeof(res));
+ return static_cast<int16_t>((res[0] << 8) | res[1]);
+}
+
+void FXOS8700Q::enable(void) const
+{
+ uint8_t data[2];
+ readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
+ data[1] |= 0x01;
+ data[0] = FXOS8700Q_CTRL_REG1;
+ writeRegs(data, sizeof(data));
+}
+
+void FXOS8700Q::disable(void) const
+{
+ uint8_t data[2];
+ readRegs(FXOS8700Q_CTRL_REG1, &data[1], 1);
+ data[1] &= 0xFE;
+ data[0] = FXOS8700Q_CTRL_REG1;
+ writeRegs(data, sizeof(data));
+}
+
+uint32_t FXOS8700Q::dataReady(void) const
+{
+ uint8_t stat = 0;
+ readRegs(FXOS8700Q_STATUS, &stat, 1);
+ return (uint32_t)stat;
+}
+
+uint32_t FXOS8700Q::sampleRate(uint32_t frequency) const
+{
+ return(50); // for now sample rate is fixed at 50Hz
+}
+
+int16_t FXOS8700QAccelerometer::getX(int16_t &x = dummy_int16_t) const
+{
+ x = getSensorAxis(FXOS8700Q_OUT_X_MSB) >> 2;
+ normalize_14bits(x);
+ return x;
+}
+
+int16_t FXOS8700QAccelerometer::getY(int16_t &y = dummy_int16_t) const
+{
+ y = getSensorAxis(FXOS8700Q_OUT_Y_MSB) >> 2;
+ normalize_14bits(y);
+ return y;
+}
+
+int16_t FXOS8700QAccelerometer::getZ(int16_t &z = dummy_int16_t) const
+{
+ z = getSensorAxis(FXOS8700Q_OUT_Z_MSB) >> 2;
+ normalize_14bits(z);
+ return z;
+}
+
+float FXOS8700QAccelerometer::getX(float &x = dummy_float) const
+{
+ int16_t val = getSensorAxis(FXOS8700Q_OUT_X_MSB) >> 2;
+ normalize_14bits(val);
+ x = val / 4096.0f;
+ return x;
+}
+
+float FXOS8700QAccelerometer::getY(float &y = dummy_float) const
+{
+ int16_t val = getSensorAxis(FXOS8700Q_OUT_Y_MSB) >> 2;
+ normalize_14bits(val);
+ y = val / 4096.0f;
+ return y;
+}
+
+float FXOS8700QAccelerometer::getZ(float &z = dummy_float) const
+{
+ int16_t val = getSensorAxis(FXOS8700Q_OUT_Z_MSB) >> 2;
+ normalize_14bits(val);
+ z = val / 4096.0f;
+ return z;
+}
+
+void FXOS8700QAccelerometer::getAxis(motion_data_counts_t &xyz) const
+{
+ uint8_t res[6];
+ readRegs(FXOS8700Q_OUT_X_MSB, res, sizeof(res));
+ xyz.x = static_cast<int16_t>((res[0] << 8) | res[1]) >> 2;
+ xyz.y = static_cast<int16_t>((res[2] << 8) | res[3]) >> 2;
+ xyz.z = static_cast<int16_t>((res[4] << 8) | res[5]) >> 2;
+ normalize_14bits(xyz.x);
+ normalize_14bits(xyz.y);
+ normalize_14bits(xyz.z);
+}
+
+void FXOS8700QAccelerometer::getAxis(motion_data_units_t &xyz) const
+{
+ motion_data_counts_t _xyz;
+ FXOS8700QAccelerometer::getAxis(_xyz);
+ xyz.x = _xyz.x / 4096.0f;
+ xyz.y = _xyz.y / 4096.0f;
+ xyz.z = _xyz.z / 4096.0f;
+}
+
+int16_t FXOS8700QMagnetometer::getX(int16_t &x = dummy_int16_t) const
+{
+ x = getSensorAxis(FXOS8700Q_M_OUT_X_MSB);
+ return x;
+}
+
+int16_t FXOS8700QMagnetometer::getY(int16_t &y = dummy_int16_t) const
+{
+ y = getSensorAxis(FXOS8700Q_M_OUT_Y_MSB);
+ return y;
+}
+
+int16_t FXOS8700QMagnetometer::getZ(int16_t &z = dummy_int16_t) const
+{
+ z = getSensorAxis(FXOS8700Q_M_OUT_Z_MSB);
+ return z;
+}
+
+float FXOS8700QMagnetometer::getX(float &x = dummy_float) const
+{
+ x = static_cast<float>(getSensorAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f;
+ return x;
+}
+
+float FXOS8700QMagnetometer::getY(float &y = dummy_float) const
+{
+ y = static_cast<float>(getSensorAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f;
+ return y;
+}
+
+float FXOS8700QMagnetometer::getZ(float &z = dummy_float) const
+{
+ z = static_cast<float>(getSensorAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f;
+ return z;
+}
+
+void FXOS8700QMagnetometer::getAxis(motion_data_counts_t &xyz) const
+{
+ uint8_t res[6];
+ readRegs(FXOS8700Q_M_OUT_X_MSB, res, sizeof(res));
+ xyz.x = (res[0] << 8) | res[1];
+ xyz.y = (res[2] << 8) | res[3];
+ xyz.z = (res[4] << 8) | res[5];
+}
+
+void FXOS8700QMagnetometer::getAxis(motion_data_units_t &xyz) const
+{
+ motion_data_counts_t _xyz;
+ FXOS8700QMagnetometer::getAxis(_xyz);
+ xyz.x = static_cast<float>(_xyz.x * 0.1f);
+ xyz.y = static_cast<float>(_xyz.y * 0.1f);
+ xyz.z = static_cast<float>(_xyz.z * 0.1f);
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/FXOS8700Q.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,171 @@
+/* FXOS8700Q sensor driver
+ * Copyright (c) 2014-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef FXOS8700Q_H
+#define FXOS8700Q_H
+
+#include "mbed.h"
+#include "MotionSensor.h"
+
+// FXOS8700CQ I2C address
+#define FXOS8700CQ_SLAVE_ADDR0 (0x1E<<1) // with pins SA0=0, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR1 (0x1D<<1) // with pins SA0=1, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR2 (0x1C<<1) // with pins SA0=0, SA1=1
+#define FXOS8700CQ_SLAVE_ADDR3 (0x1F<<1) // with pins SA0=1, SA1=1
+// FXOS8700CQ internal register addresses
+#define FXOS8700Q_STATUS 0x00
+#define FXOS8700Q_OUT_X_MSB 0x01
+#define FXOS8700Q_OUT_Y_MSB 0x03
+#define FXOS8700Q_OUT_Z_MSB 0x05
+#define FXOS8700Q_M_OUT_X_MSB 0x33
+#define FXOS8700Q_M_OUT_Y_MSB 0x35
+#define FXOS8700Q_M_OUT_Z_MSB 0x37
+#define FXOS8700Q_WHOAMI 0x0D
+#define FXOS8700Q_XYZ_DATA_CFG 0x0E
+#define FXOS8700Q_CTRL_REG1 0x2A
+#define FXOS8700Q_M_CTRL_REG1 0x5B
+#define FXOS8700Q_M_CTRL_REG2 0x5C
+#define FXOS8700Q_WHOAMI_VAL 0xC7
+
+
+/** FXOS8700Q accelerometer example
+ @code
+ #include "mbed.h"
+ #include "FXOS8700Q.h"
+ I2C i2c(PTE25, PTE24);
+ FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1); // Configured for the FRDM-K64F with onboard sensors
+ FXOS8700QMagnetometer mag(i2c, FXOS8700CQ_SLAVE_ADDR1);
+ int main(void)
+ {
+ motion_data_units_t acc_data, mag_data;
+ motion_data_counts_t acc_raw, mag_raw;
+ float faX, faY, faZ, fmX, fmY, fmZ, tmp_float;
+ int16_t raX, raY, raZ, rmX, rmY, rmZ, tmp_int;
+ acc.enable();
+ mag.enable();
+ while (true) {
+ // counts based results
+ acc.getAxis(acc_raw);
+ mag.getAxis(mag_raw);
+ acc.getX(raX);
+ acc.getY(raY);
+ acc.getZ(raZ);
+ mag.getX(rmX);
+ mag.getY(rmY);
+ mag.getZ(rmZ);
+ // unit based results
+ acc.getAxis(acc_data);
+ mag.getAxis(mag_data);
+ acc.getX(faX);
+ acc.getY(faY);
+ acc.getZ(faZ);
+ mag.getX(fmX);
+ mag.getY(fmY);
+ mag.getZ(fmZ);
+ wait(0.1f);
+ }
+ }
+ @endcode
+ */
+
+/** FXOS8700Q driver class
+ */
+class FXOS8700Q : public MotionSensor
+{
+public:
+
+ template <typename T> struct vector
+ {
+ T x, y, z;
+ };
+
+ vector<int16_t> a;
+
+ /** Read a device register
+ @param addr The address to read from
+ @param data The data to read from it
+ @param len The amount of data to read from it
+ @return 0 if successful, negative number otherwise
+ */
+ void readRegs(uint8_t addr, uint8_t *data, uint32_t len) const;
+
+ /** Read the ID from a whoAmI register
+ @return The device whoAmI register contents
+ */
+ uint8_t whoAmI(void) const;
+
+ virtual void enable(void) const;
+ virtual void disable(void) const;
+ virtual uint32_t sampleRate(uint32_t frequency) const;
+ virtual uint32_t dataReady(void) const;
+
+protected:
+ I2C *_i2c;
+ uint8_t _addr;
+
+ /** FXOS8700Q constructor
+ @param i2c a configured i2c object
+ @param addr addr of the I2C peripheral as wired
+ */
+ FXOS8700Q(I2C &i2c, uint8_t addr);
+
+ /** FXOS8700Q deconstructor
+ */
+ ~FXOS8700Q();
+
+ void writeRegs(uint8_t *data, uint32_t len) const;
+ int16_t getSensorAxis(uint8_t addr) const;
+};
+
+/** FXOS8700QAccelerometer interface
+ */
+class FXOS8700QAccelerometer : public FXOS8700Q
+{
+public:
+
+ FXOS8700QAccelerometer(I2C &i2c, uint8_t addr) : FXOS8700Q(i2c, addr) {}
+
+ virtual int16_t getX(int16_t &x) const;
+ virtual int16_t getY(int16_t &y) const;
+ virtual int16_t getZ(int16_t &z) const;
+ virtual float getX(float &x) const;
+ virtual float getY(float &y) const;
+ virtual float getZ(float &z) const;
+ virtual void getAxis(motion_data_counts_t &xyz) const;
+ virtual void getAxis(motion_data_units_t &xyz) const;
+
+};
+
+/** FXOS8700QMagnetometer interface
+ */
+class FXOS8700QMagnetometer : public FXOS8700Q
+{
+public:
+
+ FXOS8700QMagnetometer(I2C &i2c, uint8_t addr) : FXOS8700Q(i2c, addr) {}
+
+ virtual int16_t getX(int16_t &x) const;
+ virtual int16_t getY(int16_t &y) const;
+ virtual int16_t getZ(int16_t &z) const;
+ virtual float getX(float &x) const;
+ virtual float getY(float &y) const;
+ virtual float getZ(float &z) const;
+ virtual void getAxis(motion_data_counts_t &xyz) const;
+ virtual void getAxis(motion_data_units_t &xyz) const;
+
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/MotionSensor.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,112 @@
+/* MotionSensor Base Class
+ * Copyright (c) 2014-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MOTIONSENSOR_H
+#define MOTIONSENSOR_H
+
+#include "stdint.h"
+
+/** motion_data_counts_t struct
+ */
+typedef struct {
+ int16_t x; /*!< x-axis counts */
+ int16_t y; /*!< y-axis counts */
+ int16_t z; /*!< z-axis counts */
+} motion_data_counts_t;
+
+/** motion_data_units_t struct
+ */
+typedef struct {
+ float x; /*!< x-axis counts */
+ float y; /*!< y-axis counts */
+ float z; /*!< z-axis counts */
+} motion_data_units_t;
+
+
+/** Motion Sensor Base Class
+ Useful for accessing data in a common way
+ */
+class MotionSensor
+{
+public:
+
+ /** Enable the sensor for operation
+ */
+ virtual void enable(void) const = 0;
+
+ /** disable the sensors operation
+ */
+ virtual void disable(void) const = 0;
+
+ /** Set the sensor sample rate
+ @param frequency The desires sample frequency
+ @return The amount of error in Hz between desired and actual frequency
+ */
+ virtual uint32_t sampleRate(uint32_t frequency) const = 0;
+
+ /** Tells of new data is ready
+ @return The amount of data samples ready to be read from a device
+ */
+ virtual uint32_t dataReady(void) const = 0;
+
+ /** Get the x data in counts
+ @param x A referene to the variable to put the data in, 0 denotes not used
+ @return The x data in counts
+ */
+ virtual int16_t getX(int16_t &x) const = 0;
+
+ /** Get the y data in counts
+ @param y A referene to the variable to put the data in, 0 denotes not used
+ @return The y data in counts
+ */
+ virtual int16_t getY(int16_t &y) const = 0;
+
+ /** Get the z data in counts
+ @param z A referene to the variable to put the data in, 0 denotes not used
+ @return The z data in counts
+ */
+ virtual int16_t getZ(int16_t &z) const = 0;
+
+ /** Get the x data in units
+ @param x A referene to the variable to put the data in, 0 denotes not used
+ @return The x data in units
+ */
+ virtual float getX(float &x) const = 0;
+
+ /** Get the y data in units
+ @param y A referene to the variable to put the data in, 0 denotes not used
+ @return The y data in units
+ */
+ virtual float getY(float &y) const = 0;
+
+ /** Get the z data in units
+ @param z A referene to the variable to put the data in, 0 denotes not used
+ @return The z data in units
+ */
+ virtual float getZ(float &z) const = 0;
+
+ /** Get the x,y,z data in counts
+ @param xyz A referene to the variable to put the data in, 0 denotes not used
+ */
+ virtual void getAxis(motion_data_counts_t &xyz) const = 0;
+
+ /** Get the x,y,z data in units
+ @param xyz A referene to the variable to put the data in, 0 denotes not used
+ */
+ virtual void getAxis(motion_data_units_t &xyz) const = 0;
+};
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/PololuBuzzer.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,683 @@
+#include "PololuBuzzer.h"
+#include "mbed.h"
+
+using namespace mbed;
+ // constructor
+ /** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+
+BuzzerValues::BuzzerValues(float frequency, float duration, float volume) : freq(frequency), dur(duration), vol(volume) {}
+BuzzerValues::BuzzerValues() {}
+void BuzzerValues::changeValue(float frequency, float duration, float volume) {
+ freq = frequency;
+ dur = duration;
+ vol = volume;
+}
+float BuzzerValues::returnFrequency() {
+ return freq;
+}
+float BuzzerValues::returnDuration() {
+ return dur;
+}
+float BuzzerValues::returnVolume() {
+ return vol;
+}
+
+#define BUZZER_DDR DDRD
+#define BUZZER (1 << PORTD3)
+
+#define TIMER2_CLK_32 0x3 // 500 kHz
+
+static const unsigned int cs2_divider[] = {0, 1, 8, 32, 64, 128, 256, 1024};
+
+#define ENABLE_TIMER_INTERRUPT() TIMSK2 = (1 << TOIE2)
+#define DISABLE_TIMER_INTERRUPT() TIMSK2 = 0
+
+unsigned char buzzerInitialized = 0;
+volatile unsigned char buzzerFinished = 1; // flag: 0 while playing
+const char * volatile buzzerSequence = 0;
+
+// declaring these globals as static means they won't conflict
+// with globals in other .cpp files that share the same name
+static volatile unsigned int buzzerTimeout = 0; // tracks buzzer time limit
+static volatile char play_mode_setting = PLAY_AUTOMATIC;
+
+extern volatile unsigned char buzzerFinished; // flag: 0 while playing
+extern const char * volatile buzzerSequence;
+
+
+static volatile unsigned char use_program_space; // boolean: true if we should
+ // use program space
+
+// music settings and defaults
+static volatile unsigned char octave = 4; // the current octave
+static volatile unsigned int whole_note_duration = 2000; // the duration of a whole note
+static volatile unsigned int note_type = 4; // 4 for quarter, etc
+static volatile unsigned int duration = 500; // the duration of a note in ms
+static volatile unsigned int volume = 15; // the note volume
+static volatile unsigned char staccato = 0; // true if playing staccato
+
+// staccato handling
+static volatile unsigned char staccato_rest_duration; // duration of a staccato rest,
+ // or zero if it is time to play a note
+
+static void nextNote();
+
+PololuBuzzer::PololuBuzzer(PinName pin) : _pwm(pin), playing(0) {
+ _pwm.write(0.0); // after creating it have to be off
+}
+
+ /** stop the beep instantaneous
+ * usually not used
+ */
+void PololuBuzzer::nobeep() {
+ _pwm.write(0.0);
+ buzzerFinished = 1;
+ buzzerSequence = 0;
+ if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+ nextNote();
+ else
+ isPlaying2(0);
+}
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+
+bool PololuBuzzer::isPlaying2(int request){
+ if (request == 0) {
+ playing = false;
+ }
+ else if (request == 1){
+ playing = true;
+ }
+ return playing;
+}
+
+void PololuBuzzer::beep(float freq, float time, float volume) {
+ _pwm.period(1.0/freq);
+ _pwm.write(0.5); // 50% duty cycle - beep on
+ float newTime = (time/500);
+ toff.attach(callback(this, &PololuBuzzer::nobeep), newTime);
+}
+
+// Timer2 overflow interrupt
+// ISR (TIMER2_OVF_vect)
+// {
+// if (buzzerTimeout-- == 0)
+// {
+// DISABLE_TIMER_INTERRUPT();
+// sei(); // re-enable global interrupts (nextNote() is very slow)
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+// buzzerFinished = 1;
+// if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+// nextNote();
+// }
+// }
+
+
+// this is called by playFrequency()
+inline void PololuBuzzer::init()
+{
+ if (!buzzerInitialized)
+ {
+ buzzerInitialized = 1;
+ //init2();
+ }
+}
+
+// initializes timer4 (32U4) or timer2 (328P) for buzzer control
+// void PololuBuzzer::init2()
+// {
+// DISABLE_TIMER_INTERRUPT();
+
+// TCCR2A = 0x21; // bits 7 and 6 clear: normal port op., OC4A disconnected
+// // bit 5 set, 4 clear: clear OC2B on comp match when upcounting,
+// // set OC2B on comp match when downcounting
+// // bits 3 and 2: not used
+// // bit 1 clear, 0 set: combine with bit 3 of TCCR2B...
+
+// TCCR2B = 0x0B; // bit 7 clear: no force output compare for channel A
+// // bit 6 clear: no force output compare for channel B
+// // bits 5 and 4: not used
+// // bit 3 set: combine with bits 1 and 0 of TCCR2A to
+// // select waveform generation mode 5, phase-correct PWM,
+// // TOP = OCR2A, OCR2B set at TOP, TOV2 flag set at BOTTOM
+// // bit 2 clear, 1-0 set: timer clock = clkT2S/32
+
+ // This sets timer 2 to run in phase-correct PWM mode, where TOP = OCR2A,
+ // OCR2B is updated at TOP, TOV2 Flag is set on BOTTOM. OC2B is cleared
+ // on compare match when upcounting, set on compare match when downcounting;
+ // OC2A is disconnected.
+ // Note: if the PWM frequency and duty cycle are changed, the first
+ // cycle of the new frequency will be at the old duty cycle, since
+ // the duty cycle (OCR2B) is not updated until TOP.
+
+
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// BUZZER_DDR |= BUZZER; // buzzer pin set as an output
+// sei();
+// }
+
+
+// Set up the timer to play the desired frequency (in Hz or .1 Hz) for the
+// the desired duration (in ms). Allowed frequencies are 40 Hz to 10 kHz.
+// volume controls buzzer volume, with 15 being loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playFrequency(unsigned int freq, unsigned int dur,
+ unsigned char volume)
+{
+ init(); // initializes the buzzer if necessary
+ buzzerFinished = 0;
+
+ unsigned int timeout;
+ unsigned char multiplier = 1;
+
+ if (freq & DIV_BY_10) // if frequency's DIV_BY_10 bit is set
+ { // then the true frequency is freq/10
+ multiplier = 10; // (gives higher resolution for small freqs)
+ freq &= ~DIV_BY_10; // clear DIV_BY_10 bit
+ }
+
+ unsigned char min = 40 * multiplier;
+ if (freq < min) // min frequency allowed is 40 Hz
+ freq = min;
+ if (multiplier == 1 && freq > 10000)
+ freq = 10000; // max frequency allowed is 10kHz
+
+// unsigned int top;
+// unsigned char newCS2 = 2; // try prescaler divider of 8 first (minimum necessary for 10 kHz)
+// unsigned int divider = cs2_divider[newCS2];
+
+// // calculate necessary clock source and counter top value to get freq
+// top = (unsigned int)(((F_CPU/16 * multiplier) + (freq >> 1))/ freq);
+
+// while (top > 255)
+// {
+// divider = cs2_divider[++newCS2];
+// top = (unsigned int)(((F_CPU/2/divider * multiplier) + (freq >> 1))/ freq);
+// }
+
+ // set timeout (duration):
+ if (multiplier == 10)
+ freq = (freq + 5) / 10;
+
+ if (freq == 1000)
+ timeout = dur; // duration for silent notes is exact
+ else
+ timeout = (unsigned int)((long)dur * freq / 1000);
+
+ if (volume > 15)
+ volume = 15;
+
+ BuzzerValues buzz(freq,timeout,volume);
+ return buzz;
+
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts while writing to registers
+
+// TCCR2B = (TCCR2B & 0xF8) | newCS2; // select timer 2 clock prescaler
+// OCR2A = top; // set timer 2 pwm frequency
+// OCR2B = top >> (16 - volume); // set duty cycle (volume)
+// buzzerTimeout = timeout; // set buzzer duration
+
+// TIFR2 |= 0xFF; // clear any pending t2 overflow int.
+
+// ENABLE_TIMER_INTERRUPT();
+}
+
+
+
+// Determine the frequency for the specified note, then play that note
+// for the desired duration (in ms). This is done without using floats
+// and without having to loop. volume controls buzzer volume, with 15 being
+// loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playNote(unsigned char note, unsigned int dur,
+ unsigned char volume)
+{
+ // note = key + octave * 12, where 0 <= key < 12
+ // example: A4 = A + 4 * 12, where A = 9 (so A4 = 57)
+ // A note is converted to a frequency by the formula:
+ // Freq(n) = Freq(0) * a^n
+ // where
+ // Freq(0) is chosen as A4, which is 440 Hz
+ // and
+ // a = 2 ^ (1/12)
+ // n is the number of notes you are away from A4.
+ // One can see that the frequency will double every 12 notes.
+ // This function exploits this property by defining the frequencies of the
+ // 12 lowest notes allowed and then doubling the appropriate frequency
+ // the appropriate number of times to get the frequency for the specified
+ // note.
+
+ // if note = 16, freq = 41.2 Hz (E1 - lower limit as freq must be >40 Hz)
+ // if note = 57, freq = 440 Hz (A4 - central value of ET Scale)
+ // if note = 111, freq = 9.96 kHz (D#9 - upper limit, freq must be <10 kHz)
+ // if note = 255, freq = 1 kHz and buzzer is silent (silent note)
+
+ // The most significant bit of freq is the "divide by 10" bit. If set,
+ // the units for frequency are .1 Hz, not Hz, and freq must be divided
+ // by 10 to get the true frequency in Hz. This allows for an extra digit
+ // of resolution for low frequencies without the need for using floats.
+
+ unsigned int freq = 0;
+ unsigned char offset_note = note - 16;
+ BuzzerValues buzz;
+
+ if (note == SILENT_NOTE || volume == 0)
+ {
+ freq = 1000; // silent notes => use 1kHz freq (for cycle counter)
+ buzz.changeValue(freq,dur,volume);
+ // buzz = playFrequency(freq, dur, 0);
+ // return buzz;
+ }
+ else {
+ if (note <= 16)
+ offset_note = 0;
+ else if (offset_note > 95)
+ offset_note = 95;
+
+ unsigned char exponent = offset_note / 12;
+
+ // frequency table for the lowest 12 allowed notes
+ // frequencies are specified in tenths of a Hertz for added resolution
+ switch (offset_note - exponent * 12) // equivalent to (offset_note % 12)
+ {
+ case 0: // note E1 = 41.2 Hz
+ freq = 412;
+ break;
+ case 1: // note F1 = 43.7 Hz
+ freq = 437;
+ break;
+ case 2: // note F#1 = 46.3 Hz
+ freq = 463;
+ break;
+ case 3: // note G1 = 49.0 Hz
+ freq = 490;
+ break;
+ case 4: // note G#1 = 51.9 Hz
+ freq = 519;
+ break;
+ case 5: // note A1 = 55.0 Hz
+ freq = 550;
+ break;
+ case 6: // note A#1 = 58.3 Hz
+ freq = 583;
+ break;
+ case 7: // note B1 = 61.7 Hz
+ freq = 617;
+ break;
+ case 8: // note C2 = 65.4 Hz
+ freq = 654;
+ break;
+ case 9: // note C#2 = 69.3 Hz
+ freq = 693;
+ break;
+ case 10: // note D2 = 73.4 Hz
+ freq = 734;
+ break;
+ case 11: // note D#2 = 77.8 Hz
+ freq = 778;
+ break;
+ }
+ if (exponent < 7)
+ {
+ freq = freq << exponent; // frequency *= 2 ^ exponent
+ if (exponent > 1) // if the frequency is greater than 160 Hz
+ freq = (freq + 5) / 10; // we don't need the extra resolution
+ else
+ freq += DIV_BY_10; // else keep the added digit of resolution
+ }
+ else
+ freq = (freq * 64 + 2) / 5; // == freq * 2^7 / 10 without int overflow
+
+ if (volume > 15)
+ volume = 15;
+ buzz = playFrequency(freq, dur, volume); // set buzzer this freq/duration
+ }
+ return buzz;
+}
+
+
+
+// Returns 1 if the buzzer is currently playing, otherwise it returns 0
+unsigned char PololuBuzzer::isPlaying()
+{
+ return !buzzerFinished || buzzerSequence != 0;
+}
+
+
+// Plays the specified sequence of notes. If the play mode is
+// PLAY_AUTOMATIC, the sequence of notes will play with no further
+// action required by the user. If the play mode is PLAY_CHECK,
+// the user will need to call playCheck() in the main loop to initiate
+// the playing of each new note in the sequence. The play mode can
+// be changed while the sequence is playing.
+// This is modeled after the PLAY commands in GW-BASIC, with just a
+// few differences.
+//
+// The notes are specified by the characters C, D, E, F, G, A, and
+// B, and they are played by default as "quarter notes" with a
+// length of 500 ms. This corresponds to a tempo of 120
+// beats/min. Other durations can be specified by putting a number
+// immediately after the note. For example, C8 specifies C played as an
+// eighth note, with half the duration of a quarter note. The special
+// note R plays a rest (no sound).
+//
+// Various control characters alter the sound:
+// '>' plays the next note one octave higher
+//
+// '<' plays the next note one octave lower
+//
+// '+' or '#' after a note raises any note one half-step
+//
+// '-' after a note lowers any note one half-step
+//
+// '.' after a note "dots" it, increasing the length by
+// 50%. Each additional dot adds half as much as the
+// previous dot, so that "A.." is 1.75 times the length of
+// "A".
+//
+// 'O' followed by a number sets the octave (default: O4).
+//
+// 'T' followed by a number sets the tempo (default: T120).
+//
+// 'L' followed by a number sets the default note duration to
+// the type specified by the number: 4 for quarter notes, 8
+// for eighth notes, 16 for sixteenth notes, etc.
+// (default: L4)
+//
+// 'V' followed by a number from 0-15 sets the music volume.
+// (default: V15)
+//
+// 'MS' sets all subsequent notes to play staccato - each note is played
+// for 1/2 of its allotted time, followed by an equal period
+// of silence.
+//
+// 'ML' sets all subsequent notes to play legato - each note is played
+// for its full length. This is the default setting.
+//
+// '!' resets all persistent settings to their defaults.
+//
+// The following plays a c major scale up and back down:
+// play("L16 V8 cdefgab>cbagfedc");
+//
+// Here is an example from Bach:
+// play("T240 L8 a gafaeada c+adaeafa <aa<bac#ada c#adaeaf4");
+void PololuBuzzer::play(const char *notes)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes;
+ use_program_space = 0;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+void PololuBuzzer::playFromProgramSpace(const char *notes_p)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes_p;
+ use_program_space = 1;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+
+// // stop all sound playback immediately
+// void PololuBuzzer::stopPlaying()
+// {
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts
+
+
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// buzzerFinished = 1;
+// buzzerSequence = 0;
+// }
+
+// Gets the current character, converting to lower-case and skipping
+// spaces. For any spaces, this automatically increments sequence!
+static char currentCharacter()
+{
+ char c = 0;
+ do
+ {
+ // if(use_program_space)
+ // c = pgm_read_byte(buzzerSequence);
+ // else
+ c = *buzzerSequence;
+
+ if(c >= 'A' && c <= 'Z')
+ c += 'a'-'A';
+ } while(c == ' ' && (buzzerSequence ++));
+
+ return c;
+}
+
+// Returns the numerical argument specified at buzzerSequence[0] and
+// increments sequence to point to the character immediately after the
+// argument.
+static unsigned int getNumber()
+{
+ unsigned int arg = 0;
+
+ // read all digits, one at a time
+ char c = currentCharacter();
+ while(c >= '0' && c <= '9')
+ {
+ arg *= 10;
+ arg += c-'0';
+ buzzerSequence ++;
+ c = currentCharacter();
+ }
+
+ return arg;
+}
+
+static void nextNote()
+{
+ unsigned char note = 0;
+ unsigned char rest = 0;
+ unsigned char tmp_octave = octave; // the octave for this note
+ unsigned int tmp_duration; // the duration of this note
+ unsigned int dot_add;
+
+ char c; // temporary variable
+
+ // if we are playing staccato, after every note we play a rest
+ if(staccato && staccato_rest_duration)
+ {
+ PololuBuzzer::playNote(SILENT_NOTE, staccato_rest_duration, 0);
+ staccato_rest_duration = 0;
+ return;
+ }
+
+ parse_character:
+
+ // Get current character
+ c = currentCharacter();
+ buzzerSequence ++;
+
+ // Interpret the character.
+ switch(c)
+ {
+ case '>':
+ // shift the octave temporarily up
+ tmp_octave ++;
+ goto parse_character;
+ case '<':
+ // shift the octave temporarily down
+ tmp_octave --;
+ goto parse_character;
+ case 'a':
+ note = NOTE_A(0);
+ break;
+ case 'b':
+ note = NOTE_B(0);
+ break;
+ case 'c':
+ note = NOTE_C(0);
+ break;
+ case 'd':
+ note = NOTE_D(0);
+ break;
+ case 'e':
+ note = NOTE_E(0);
+ break;
+ case 'f':
+ note = NOTE_F(0);
+ break;
+ case 'g':
+ note = NOTE_G(0);
+ break;
+ case 'l':
+ // set the default note duration
+ note_type = getNumber();
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'm':
+ // set music staccato or legato
+ if(currentCharacter() == 'l')
+ staccato = false;
+ else
+ {
+ staccato = true;
+ staccato_rest_duration = 0;
+ }
+ buzzerSequence ++;
+ goto parse_character;
+ case 'o':
+ // set the octave permanently
+ octave = tmp_octave = getNumber();
+ goto parse_character;
+ case 'r':
+ // Rest - the note value doesn't matter.
+ rest = 1;
+ break;
+ case 't':
+ // set the tempo
+ whole_note_duration = 60*400/getNumber()*10;
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'v':
+ // set the volume
+ volume = getNumber();
+ goto parse_character;
+ case '!':
+ // reset to defaults
+ octave = 4;
+ whole_note_duration = 2000;
+ note_type = 4;
+ duration = 500;
+ volume = 15;
+ staccato = 0;
+ // reset temp variables that depend on the defaults
+ tmp_octave = octave;
+ tmp_duration = duration;
+ goto parse_character;
+ default:
+ buzzerSequence = 0;
+ return;
+ }
+
+ note += tmp_octave*12;
+
+ // handle sharps and flats
+ c = currentCharacter();
+ while(c == '+' || c == '#')
+ {
+ buzzerSequence ++;
+ note ++;
+ c = currentCharacter();
+ }
+ while(c == '-')
+ {
+ buzzerSequence ++;
+ note --;
+ c = currentCharacter();
+ }
+
+ // set the duration of just this note
+ tmp_duration = duration;
+
+ // If the input is 'c16', make it a 16th note, etc.
+ if(c > '0' && c < '9')
+ tmp_duration = whole_note_duration/getNumber();
+
+ // Handle dotted notes - the first dot adds 50%, and each
+ // additional dot adds 50% of the previous dot.
+ dot_add = tmp_duration/2;
+ while(currentCharacter() == '.')
+ {
+ buzzerSequence ++;
+ tmp_duration += dot_add;
+ dot_add /= 2;
+ }
+
+ if(staccato)
+ {
+ staccato_rest_duration = tmp_duration / 2;
+ tmp_duration -= staccato_rest_duration;
+ }
+
+ // this will re-enable the timer overflow interrupt
+ PololuBuzzer::playNote(rest ? SILENT_NOTE : note, tmp_duration, volume);
+// wait_ms(tmp_duration);
+}
+
+
+// This puts play() into a mode where instead of advancing to the
+// next note in the sequence automatically, it waits until the
+// function playCheck() is called. The idea is that you can
+// put playCheck() in your main loop and avoid potential
+// delays due to the note sequence being checked in the middle of
+// a time sensitive calculation. It is recommended that you use
+// this function if you are doing anything that can't tolerate
+// being interrupted for more than a few microseconds.
+// Note that the play mode can be changed while a sequence is being
+// played.
+//
+// Usage: playMode(PLAY_AUTOMATIC) makes it automatic (the
+// default), playMode(PLAY_CHECK) sets it to a mode where you have
+// to call playCheck().
+void PololuBuzzer::playMode(unsigned char mode)
+{
+ play_mode_setting = mode;
+
+ // We want to check to make sure that we didn't miss a note if we
+ // are going out of play-check mode.
+ if(mode == PLAY_AUTOMATIC)
+ playCheck();
+}
+
+
+// Checks whether it is time to start another note, and starts
+// it if so. If it is not yet time to start the next note, this method
+// returns without doing anything. Call this as often as possible
+// in your main loop to avoid delays between notes in the sequence.
+//
+// Returns true if it is still playing.
+unsigned char PololuBuzzer::playCheck()
+{
+ if(buzzerFinished && buzzerSequence != 0)
+ nextNote();
+ return buzzerSequence != 0;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/PololuBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,126 @@
+#pragma once
+#include "mbed.h"
+
+#ifndef MBED_BEEP_H
+#define MBED_BEEP_H
+
+/*! \brief Specifies that the sequence of notes will play with no further action
+ * required by the user. */
+#define PLAY_AUTOMATIC 0
+
+
+/*! \brief Specified that the user will need to call `playCheck()` regularly. */
+#define PLAY_CHECK 1
+
+// n
+// Equal Tempered Scale is given by f = f * a
+// n o
+//
+// where f is chosen as A above middle C (A4) at f = 440 Hz
+// o o
+// and a is given by the twelfth root of 2 (~1.059463094359)
+
+/*! \anchor note_macros
+ *
+ * \name Note Macros
+ * \a x specifies the octave of the note
+ * @{
+ */
+#define NOTE_C(x) ( 0 + (x)*12)
+#define NOTE_C_SHARP(x) ( 1 + (x)*12)
+#define NOTE_D_FLAT(x) ( 1 + (x)*12)
+#define NOTE_D(x) ( 2 + (x)*12)
+#define NOTE_D_SHARP(x) ( 3 + (x)*12)
+#define NOTE_E_FLAT(x) ( 3 + (x)*12)
+#define NOTE_E(x) ( 4 + (x)*12)
+#define NOTE_F(x) ( 5 + (x)*12)
+#define NOTE_F_SHARP(x) ( 6 + (x)*12)
+#define NOTE_G_FLAT(x) ( 6 + (x)*12)
+#define NOTE_G(x) ( 7 + (x)*12)
+#define NOTE_G_SHARP(x) ( 8 + (x)*12)
+#define NOTE_A_FLAT(x) ( 8 + (x)*12)
+#define NOTE_A(x) ( 9 + (x)*12)
+#define NOTE_A_SHARP(x) (10 + (x)*12)
+#define NOTE_B_FLAT(x) (10 + (x)*12)
+#define NOTE_B(x) (11 + (x)*12)
+
+/*! \brief silences buzzer for the note duration */
+#define SILENT_NOTE 0xFF
+
+/*! \brief frequency bit that indicates Hz/10<br>
+ * e.g. \a frequency = `(445 | DIV_BY_10)` gives a frequency of 44.5 Hz
+ */
+#define DIV_BY_10 (1 << 15)
+/*! @} */
+
+namespace mbed {
+
+class BuzzerValues{
+ private:
+ float freq;
+ float dur;
+ float vol;
+ public:
+ BuzzerValues(float frequency, float duration, float volume);
+ BuzzerValues();
+ void changeValue(float frequency, float duration, float volume);
+ float returnFrequency();
+ float returnDuration();
+ float returnVolume();
+};
+
+/* Class: PololuBuzzer
+ * A class which uses pwm to controle a beeper to generate sounds.
+ */
+class PololuBuzzer {
+private:
+ PwmOut _pwm;
+ Timeout toff;
+ bool playing;
+
+public:
+
+/** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+ PololuBuzzer (PinName pin);
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+ void beep (float frequency, float time, float volume);
+
+/** stop the beep instantaneous
+ * usually not used
+ */
+ void nobeep();
+
+ static BuzzerValues playFrequency(unsigned int freq, unsigned int duration,
+ unsigned char volume);
+
+ static BuzzerValues playNote(unsigned char note, unsigned int duration,
+ unsigned char volume);
+
+ static void play(const char *sequence);
+
+ static void playFromProgramSpace(const char *sequence);
+
+ static void playMode(unsigned char mode);
+
+ static unsigned char playCheck();
+
+ static unsigned char isPlaying();
+
+ bool isPlaying2(int request);
+
+ //static void stopPlaying();
+
+ // initializes timer for buzzer control
+// static void init2();
+ static void init();
+ };
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/Pushbutton.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,151 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+#include "Pushbutton.h"
+#include "mbed.h"
+#include "millis.h"
+
+// \cond
+
+PushbuttonStateMachine::PushbuttonStateMachine()
+{
+ state = 0;
+}
+
+// state 0: The value is considered to be true.
+// state 1: The value was considered to be true, but there
+// was a recent false reading so it might be falling.
+// state 2: The value is considered to be false.
+// state 3: The value was considered to be false, but there
+// was a recent true reading so it might be rising.
+//
+// The transition from state 3 to state 0 is the point where we
+// have successfully detected a rising edge an we return true.
+//
+// The prevTimeMillis variable holds the last time that we
+// transitioned to states 1 or 3.
+bool PushbuttonStateMachine::getSingleDebouncedRisingEdge(bool value)
+{
+ uint16_t timeMillis = millis();
+
+ switch (state)
+ {
+ case 0:
+ // If value is false, proceed to the next state.
+ if (!value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 1;
+ }
+ break;
+
+ case 1:
+ if (value)
+ {
+ // The value is true or bouncing, so go back to previous (initial)
+ // state.
+ state = 0;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still false, so
+ // proceed to the next state.
+ state = 2;
+ }
+ break;
+
+ case 2:
+ // If the value is true, proceed to the next state.
+ if (value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 3;
+ }
+ break;
+
+ case 3:
+ if (!value)
+ {
+ // The value is false or bouncing, so go back to previous state.
+ state = 2;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still true, so
+ // go back to the initial state and report this rising edge.
+ state = 0;
+ return true;
+ }
+ break;
+ }
+
+ return false;
+}
+
+// \endcond
+
+void PushbuttonBase::waitForPress()
+{
+ do
+ {
+ while (!isPressed()); // wait for button to be pressed
+ wait_us(10000); // debounce the button press
+ }
+ while (!isPressed()); // if button isn't still pressed, loop
+}
+
+void PushbuttonBase::waitForRelease()
+{
+ do
+ {
+ while (isPressed()); // wait for button to be released
+ wait_us(10000); // debounce the button release
+ }
+ while (isPressed()); // if button isn't still released, loop
+}
+
+void PushbuttonBase::waitForButton()
+{
+ waitForPress();
+ waitForRelease();
+}
+
+bool PushbuttonBase::getSingleDebouncedPress()
+{
+ return pressState.getSingleDebouncedRisingEdge(isPressed());
+}
+
+bool PushbuttonBase::getSingleDebouncedRelease()
+{
+ return releaseState.getSingleDebouncedRisingEdge(!isPressed());
+}
+
+Pushbutton::Pushbutton(PinName pin, uint8_t pullUp, uint8_t defaultState)
+{
+ initialized = false;
+ _pin = pin;
+ _pullUp = pullUp;
+ _defaultState = defaultState;
+}
+
+void Pushbutton::init2()
+{
+ if (_pullUp == PULL_UP_ENABLED)
+ {
+ DigitalIn myPin(_pin, PullUp);
+ }
+ else
+ {
+ DigitalIn myPin(_pin);
+ //pinMode(_pin, INPUT); // high impedance
+ }
+
+ wait_us(5); // give pull-up time to stabilize
+}
+
+bool Pushbutton::isPressed()
+{
+ init(); // initialize if needed
+ DigitalIn myPin(_pin);
+
+ return myPin != _defaultState;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/Pushbutton.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,176 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file Pushbutton.h
+ *
+ * This is the main header file for the %Pushbutton library.
+ *
+ * For an overview of the library's features, see
+ * https://github.com/pololu/pushbutton-arduino. That is the main repository
+ * for the library, though copies of the library may exist in other
+ * repositories. */
+
+#pragma once
+
+#include "mbed.h"
+
+/*! Indicates the that pull-up resistor should be disabled. */
+#define PULL_UP_DISABLED 0
+
+/*! Indicates the that pull-up resistor should be enabled. */
+#define PULL_UP_ENABLED 1
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads low. */
+#define DEFAULT_STATE_LOW 0
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads high. */
+#define DEFAULT_STATE_HIGH 1
+
+/*! The pin used for the button on the
+ * [Zumo Shield for Arduino](http://www.pololu.com/product/2508).
+ *
+ * This does not really belong here in this general pushbutton library and will
+ * probably be removed in the future. */
+#define ZUMO_BUTTON PTD3
+
+// \cond
+/** \brief A state machine that detects when a boolean value changes from false
+ * to true, with debouncing.
+ *
+ * This should be considered a private implementation detail of the library.
+ */
+class PushbuttonStateMachine
+{
+public:
+
+ PushbuttonStateMachine();
+
+ /** This should be called repeatedly in a loop. It will return true once after
+ * each time it detects the given value changing from false to true. */
+ bool getSingleDebouncedRisingEdge(bool value);
+
+private:
+
+ uint8_t state;
+ uint16_t prevTimeMillis;
+};
+// \endcond
+
+/*! \brief General pushbutton class that handles debouncing.
+ *
+ * This is an abstract class used for interfacing with pushbuttons. It knows
+ * about debouncing, but it knows nothing about how to read the current state of
+ * the button. The functions in this class get the current state of the button
+ * by calling isPressed(), a virtual function which must be implemented in a
+ * subclass of PushbuttonBase, such as Pushbutton.
+ *
+ * Most users of this library do not need to directly use PushbuttonBase or even
+ * know that it exists. They can use Pushbutton instead.*/
+class PushbuttonBase
+{
+public:
+
+ /*! \brief Waits until the button is pressed and takes care of debouncing.
+ *
+ * This function waits until the button is in the pressed state and then
+ * returns. Note that if the button is already pressed when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForPress();
+
+ /*! \brief Waits until the button is released and takes care of debouncing.
+ *
+ * This function waits until the button is in the released state and then
+ * returns. Note that if the button is already released when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForRelease();
+
+ /*! \brief Waits until the button is pressed and then waits until the button
+ * is released, taking care of debouncing.
+ *
+ * This is equivalent to calling waitForPress() and then waitForRelease(). */
+ void waitForButton();
+
+ /*! \brief Uses a state machine to return true once after each time it detects
+ * the button moving from the released state to the pressed state.
+ *
+ * This is a non-blocking function that is meant to be called repeatedly in a
+ * loop. Each time it is called, it updates a state machine that monitors the
+ * state of the button. When it detects the button changing from the released
+ * state to the pressed state, with debouncing, it returns true. */
+ bool getSingleDebouncedPress();
+
+ /*! \brief Uses a state machine to return true once after each time it detects the button moving from the pressed state to the released state.
+ *
+ * This is just like getSingleDebouncedPress() except it has a separate state
+ * machine and it watches for when the button goes from the pressed state to
+ * the released state.
+ *
+ * There is no strict guarantee that every debounced button press event
+ * returned by getSingleDebouncedPress() will have a corresponding
+ * button release event returned by getSingleDebouncedRelease(); the two
+ * functions use independent state machines and sample the button at different
+ * times. */
+ bool getSingleDebouncedRelease();
+
+ /*! \brief indicates whether button is currently pressed without any debouncing.
+ *
+ * @return 1 if the button is pressed right now, 0 if it is not.
+ *
+ * This function must be implemented in a subclass of PushbuttonBase, such as
+ * Pushbutton. */
+ virtual bool isPressed() = 0;
+
+private:
+
+ PushbuttonStateMachine pressState;
+ PushbuttonStateMachine releaseState;
+};
+
+/** \brief Main class for interfacing with pushbuttons.
+ *
+ * This class can interface with any pushbutton whose state can be read with
+ * the `digitalRead` function, which is part of the Arduino core.
+ *
+ * See https://github.com/pololu/pushbutton-arduino for an overview
+ * of the different ways to use this class. */
+class Pushbutton : public PushbuttonBase
+{
+public:
+
+ /** Constructs a new instance of Pushbutton.
+ *
+ * @param pin The pin number of the pin. This is used as an argument to
+ * `pinMode` and `digitalRead`.
+ *
+ * @param pullUp Specifies whether the pin's internal pull-up resistor should
+ * be enabled. This should be either #PULL_UP_ENABLED (which is the default if
+ * the argument is omitted) or #PULL_UP_DISABLED.
+ *
+ * @param defaultState Specifies the voltage level that corresponds to the
+ * button's default (released) state. This should be either
+ * #DEFAULT_STATE_HIGH (which is the default if this argument is omitted) or
+ * #DEFAULT_STATE_LOW. */
+ Pushbutton(PinName pin, uint8_t pullUp = PULL_UP_ENABLED,
+ uint8_t defaultState = DEFAULT_STATE_HIGH);
+
+ virtual bool isPressed();
+
+private:
+
+ void init()
+ {
+ if (!initialized)
+ {
+ initialized = true;
+ init2();
+ }
+ }
+
+ void init2();
+
+ bool initialized;
+ PinName _pin;
+ bool _pullUp;
+ bool _defaultState;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/QTRSensors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,783 @@
+/*
+ QTRSensors.cpp - Arduino library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010.
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+
+#include "QTRSensors.h"
+#include <stdlib.h>
+#include "mbed.h"
+#include<string>
+
+Timer timer;
+// static BufferedSerial serial_port(USBTX, USBRX);
+
+// Base class data member initialization (called by derived class init())
+void QTRSensors::init(unsigned char *pins, unsigned char numSensors,
+ unsigned char emitterPin, bool analog = false)
+{
+ calibratedMinimumOn=0;
+ calibratedMaximumOn=0;
+ calibratedMinimumOff=0;
+ calibratedMaximumOff=0;
+
+ _lastValue=0;
+
+ if (numSensors > QTR_MAX_SENSORS)
+ _numSensors = QTR_MAX_SENSORS;
+ else
+ _numSensors = numSensors;
+
+
+ if (_pins == 0) {
+ _pins = (PinName *)malloc(sizeof(PinName)*_numSensors);
+ if (_pins == 0)
+ return;
+ }
+
+ unsigned char i;
+ // Copy parameter values to local storage
+ PinName currentPin;
+ for (i = 0; i < _numSensors; i++) {
+ switch(pins[i]){
+ case 1:
+ currentPin = PTB23;
+ break;
+ case 2:
+ currentPin = PTB11;
+ break;
+ case 3:
+ currentPin = PTD2;
+ break;
+ case 4:
+ currentPin = PTB2;
+ break;
+ case 5:
+ currentPin = PTB10;
+ break;
+ case 6:
+ currentPin = PTA2;
+ break;
+ }
+ _pins[i] = currentPin;
+ }
+
+ // Serial pc(USBTX, USBRX);
+ // unsigned char j;
+ // for (j = 0; j < numSensors; j++)
+ // {
+ // std::string s = std::to_string(_pins[j]);
+ // const char * c = s.c_str();
+ // pc.printf(c);
+ // pc.printf("\n");
+ // }
+
+ // // Allocate the arrays
+ // // Allocate Space for Calibrated Maximum On Values
+ // calibratedMaximumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOn == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOn[i] = 0;
+
+ // // Allocate Space for Calibrated Maximum Off Values
+ // calibratedMaximumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMaximumOff == 0)
+ // return;
+
+ // // Initialize the max and min calibrated values to values that
+ // // will cause the first reading to update them.
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMaximumOff[i] = 0;
+
+ // // Allocate Space for Calibrated Minimum On Values
+ // calibratedMinimumOn = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOn == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOn[i] = _maxValue;
+
+ // // Allocate Space for Calibrated Minimum Off Values
+ // calibratedMinimumOff = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // // If the malloc failed, don't continue.
+ // if(calibratedMinimumOff == 0)
+ // return;
+
+ // for(i=0; i<_numSensors; i++)
+ // calibratedMinimumOff[i] = _maxValue;
+
+ // emitter pin is used for DigitalOut
+ // So we create a DigitalOut on that pin
+ _emitterPin = emitterPin;
+ if(_emitterPin != 255){
+ if(emitterPin == 2){
+ _emitter = new DigitalOut(PTB9);
+ }
+ }
+
+ // If we have an Analog Sensor then we wil used AnalogIn on the pins provided
+ // We use a Vector for our collection of pins
+ // Here we reserve space for the pins
+ // _analog = analog;
+ // if (_analog) {
+ // _qtrAIPins.reserve(_numSensors);
+ // } else {
+ // // Not analog - so we use _qtrPins (which is a Vector on DigitalInOut)
+ // _qtrPins.reserve(_numSensors);
+ // }
+ // // Create the pins and push onto the vectors
+ // for (i = 0; i < _numSensors; i++) {
+ // if (analog) {
+ // _qtrAIPins.push_back(new AnalogIn(_pins[i]));
+ // } else {
+ // _qtrPins.push_back(new DigitalInOut(_pins[i]));
+ // }
+ // }
+
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in abstract units,
+// with higher values corresponding to lower reflectance (e.g. a black
+// surface or a void).
+void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode)
+{
+ unsigned int off_values[QTR_MAX_SENSORS];
+ unsigned char i;
+
+
+ if(readMode == QTR_EMITTERS_ON || readMode == QTR_EMITTERS_ON_AND_OFF) {
+ emittersOn();
+ } else {
+ emittersOff();
+ }
+
+ readPrivate(sensor_values);
+
+ emittersOff();
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF) {
+ readPrivate(off_values);
+
+ for(i=0; i<_numSensors; i++) {
+ sensor_values[i] += _maxValue - off_values[i];
+ }
+ }
+}
+
+
+// Turn the IR LEDs off and on. This is mainly for use by the
+// read method, and calling these functions before or
+// after the reading the sensors will have no effect on the
+// readings, but you may wish to use these for testing purposes.
+void QTRSensors::emittersOff()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(0);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+void QTRSensors::emittersOn()
+{
+ if (_emitterPin == QTR_NO_EMITTER_PIN)
+ return;
+
+ _emitter->write(1);
+ wait_us(200); // wait 200 microseconds for the emitters to settle
+}
+
+// Resets the calibration.
+void QTRSensors::resetCalibration()
+{
+ unsigned char i;
+ for(i=0; i<_numSensors; i++) {
+ if(calibratedMinimumOn)
+ calibratedMinimumOn[i] = _maxValue;
+ if(calibratedMinimumOff)
+ calibratedMinimumOff[i] = _maxValue;
+ if(calibratedMaximumOn)
+ calibratedMaximumOn[i] = 0;
+ if(calibratedMaximumOff)
+ calibratedMaximumOff[i] = 0;
+ }
+}
+
+// Reads the sensors 10 times and uses the results for
+// calibration. The sensor values are not returned; instead, the
+// maximum and minimum values found over time are stored internally
+// and used for the readCalibrated() method.
+void QTRSensors::calibrate(unsigned char readMode)
+{
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON) {
+ calibrateOnOrOff(&calibratedMinimumOn,
+ &calibratedMaximumOn,
+ QTR_EMITTERS_ON);
+ }
+
+
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF) {
+ calibrateOnOrOff(&calibratedMinimumOff,
+ &calibratedMaximumOff,
+ QTR_EMITTERS_OFF);
+ }
+}
+
+void QTRSensors::calibrateOnOrOff(unsigned int **calibratedMinimum,
+ unsigned int **calibratedMaximum,
+ unsigned char readMode)
+{
+ int i;
+ unsigned int sensor_values[16];
+ unsigned int max_sensor_values[16];
+ unsigned int min_sensor_values[16];
+
+ // initialisation of calibrated sensor values moved to init()
+
+ // Allocate the arrays if necessary.
+ if(*calibratedMaximum == 0)
+ {
+ *calibratedMaximum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMaximum == 0)
+ return;
+
+ // Initialize the max and min calibrated values to values that
+ // will cause the first reading to update them.
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMaximum)[i] = 0;
+ }
+ if(*calibratedMinimum == 0)
+ {
+ *calibratedMinimum = (unsigned int*)malloc(sizeof(unsigned int)*_numSensors);
+
+ // If the malloc failed, don't continue.
+ if(*calibratedMinimum == 0)
+ return;
+
+ for(i=0;i<_numSensors;i++)
+ (*calibratedMinimum)[i] = _maxValue;
+ }
+
+ int j;
+ for(j=0; j<10; j++) {
+ read(sensor_values,readMode);
+ for(i=0; i<_numSensors; i++) {
+ // set the max we found THIS time
+ if(j == 0 || max_sensor_values[i] < sensor_values[i])
+ max_sensor_values[i] = sensor_values[i];
+
+ // set the min we found THIS time
+ if(j == 0 || min_sensor_values[i] > sensor_values[i])
+ min_sensor_values[i] = sensor_values[i];
+ }
+ }
+
+ // record the min and max calibration values
+ for(i=0; i<_numSensors; i++) {
+ if(min_sensor_values[i] > (*calibratedMaximum)[i]) // this was min_sensor_values[i] > (*calibratedMaximum)[i]
+ (*calibratedMaximum)[i] = min_sensor_values[i];
+ if(max_sensor_values[i] < (*calibratedMinimum)[i])
+ (*calibratedMinimum)[i] = max_sensor_values[i];
+ }
+
+}
+
+
+// Returns values calibrated to a value between 0 and 1000, where
+// 0 corresponds to the minimum value read by calibrate() and 1000
+// corresponds to the maximum value. Calibration values are
+// stored separately for each sensor, so that differences in the
+// sensors are accounted for automatically.
+void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode)
+{
+ int i;
+
+ // if not calibrated, do nothing
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_OFF)
+ if(!calibratedMinimumOff || !calibratedMaximumOff)
+ return;
+ if(readMode == QTR_EMITTERS_ON_AND_OFF || readMode == QTR_EMITTERS_ON)
+ if(!calibratedMinimumOn || !calibratedMaximumOn)
+ return;
+
+ // read the needed values
+ read(sensor_values,readMode);
+
+ for(i=0; i<_numSensors; i++) {
+ unsigned int calmin,calmax;
+ unsigned int denominator;
+
+ // find the correct calibration
+ if(readMode == QTR_EMITTERS_ON) {
+ calmax = calibratedMaximumOn[i];
+ calmin = calibratedMinimumOn[i];
+ } else if(readMode == QTR_EMITTERS_OFF) {
+ calmax = calibratedMaximumOff[i];
+ calmin = calibratedMinimumOff[i];
+ } else { // QTR_EMITTERS_ON_AND_OFF
+
+ if(calibratedMinimumOff[i] < calibratedMinimumOn[i]) // no meaningful signal
+ calmin = _maxValue;
+ else
+ calmin = calibratedMinimumOn[i] + _maxValue - calibratedMinimumOff[i]; // this won't go past _maxValue
+
+ if(calibratedMaximumOff[i] < calibratedMaximumOn[i]) // no meaningful signal
+ calmax = _maxValue;
+ else
+ calmax = calibratedMaximumOn[i] + _maxValue - calibratedMaximumOff[i]; // this won't go past _maxValue
+ }
+
+ denominator = calmax - calmin;
+
+ signed int x = 0;
+ if(denominator != 0)
+ x = (((signed long)sensor_values[i]) - calmin)
+ * 1000 / denominator;
+ if(x < 0)
+ x = 0;
+ else if(x > 1000)
+ x = 1000;
+ sensor_values[i] = x;
+ }
+
+}
+
+
+// Operates the same as read calibrated, but also returns an
+// estimated position of the robot with respect to a line. The
+// estimate is made using a weighted average of the sensor indices
+// multiplied by 1000, so that a return value of 0 indicates that
+// the line is directly below sensor 0, a return value of 1000
+// indicates that the line is directly below sensor 1, 2000
+// indicates that it's below sensor 2000, etc. Intermediate
+// values indicate that the line is between two sensors. The
+// formula is:
+//
+// 0*value0 + 1000*value1 + 2000*value2 + ...
+// --------------------------------------------
+// value0 + value1 + value2 + ...
+//
+// By default, this function assumes a dark line (high values)
+// surrounded by white (low values). If your line is light on
+// black, set the optional second argument white_line to true. In
+// this case, each sensor value will be replaced by (1000-value)
+// before the averaging.
+int QTRSensors::readLine(unsigned int *sensor_values,
+ unsigned char readMode, unsigned char white_line)
+{
+ unsigned char i, on_line = 0;
+ unsigned long avg; // this is for the weighted total, which is long
+ // before division
+ unsigned int sum; // this is for the denominator which is <= 64000
+ // static int last_value=0; // assume initially that the line is left.
+
+ readCalibrated(sensor_values, readMode);
+
+ avg = 0;
+ sum = 0;
+
+ for(i=0; i<_numSensors; i++) {
+ int value = sensor_values[i];
+ if(white_line)
+ value = 1000-value;
+
+ // keep track of whether we see the line at all
+ if(value > 200) {
+ on_line = 1;
+ }
+
+ // only average in values that are above a noise threshold
+ if(value > 50) {
+ avg += (long)(value) * (i * 1000);
+ sum += value;
+ }
+ }
+
+ if(!on_line) {
+ // If it last read to the left of center, return 0.
+ if(_lastValue < (_numSensors-1)*1000/2)
+ return 0;
+
+ // If it last read to the right of center, return the max.
+ else
+ return (_numSensors-1)*1000;
+
+ }
+
+ _lastValue = avg/sum;
+
+ return _lastValue;
+}
+
+
+
+// Derived RC class constructors
+QTRSensorsRC::QTRSensorsRC()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsRC::QTRSensorsRC(unsigned char* pins,
+ unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ init(pins, numSensors, timeout, emitterPin);
+}
+
+
+// The array 'pins' contains the Arduino pin number for each sensor.
+
+// 'numSensors' specifies the length of the 'pins' array (i.e. the
+// number of QTR-RC sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'timeout' specifies the length of time in microseconds beyond
+// which you consider the sensor reading completely black. That is to say,
+// if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+// and the reading for that pin will be considered full black.
+// It is recommended that you set timeout to be between 1000 and
+// 3000 us, depending on things like the height of your sensors and
+// ambient lighting. Using timeout allows you to shorten the
+// duration of a sensor-reading cycle while still maintaining
+// useful analog measurements of reflectance
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsRC::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned int timeout, unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin, false);
+
+ _maxValue = timeout;
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// ...
+// The values returned are in microseconds and range from 0 to
+// timeout (as specified in the constructor).
+void QTRSensorsRC::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i;
+ // Serial pc(USBTX, USBRX);
+
+ if (_pins == 0)
+ return;
+
+ DigitalInOut pinout1(_pins[0]);
+ DigitalInOut pinout2(_pins[1]);
+ DigitalInOut pinout3(_pins[2]);
+ DigitalInOut pinout4(_pins[3]);
+ DigitalInOut pinout5(_pins[4]);
+ DigitalInOut pinout6(_pins[5]);
+
+ for(i = 0; i < _numSensors; i++) {
+ sensor_values[i] = _maxValue;
+ switch(i){
+ case 0:
+ pinout1.output(); // make sensor line an output and drive high
+ pinout1.write(1);
+ break;
+ case 1:
+ pinout2.output(); // make sensor line an output and drive high
+ pinout2.write(1);
+ break;
+ case 2:
+ pinout3.output(); // make sensor line an output and drive high
+ pinout3.write(1);
+ break;
+ case 3:
+ pinout4.output(); // make sensor line an output and drive high
+ pinout4.write(1);
+ break;
+ case 4:
+ pinout5.output(); // make sensor line an output and drive high
+ pinout5.write(1);
+ break;
+ case 5:
+ pinout6.output(); // make sensor line an output and drive high
+ pinout6.write(1);
+ break;
+ }
+ }
+
+ wait_us(10); // charge lines for 10 us
+
+ for(i = 0; i < _numSensors; i++) {
+ // important: disable internal pull-up!
+ // ??? do we need to change the mode: _qtrPins[i]->mode(OpenDrain);
+ // or just change mode to input
+ // mbed documentation is not clear and I do not have a test sensor
+ switch(i){
+ case 0:
+ pinout1.input(); // make sensor line an output and drive high
+ pinout1.mode(PullNone);
+ break;
+ case 1:
+ pinout2.input(); // make sensor line an output and drive high
+ pinout2.mode(PullNone);
+ break;
+ case 2:
+ pinout3.input(); // make sensor line an output and drive high
+ pinout3.mode(PullNone);
+ break;
+ case 3:
+ pinout4.input(); // make sensor line an output and drive high
+ pinout4.mode(PullNone);
+ break;
+ case 4:
+ pinout5.input(); // make sensor line an output and drive high
+ pinout5.mode(PullNone);
+ break;
+ case 5:
+ pinout6.input(); // make sensor line an output and drive high
+ pinout6.mode(PullNone);
+ break;
+ }
+ }
+
+ // int pausedTime = 0;
+ // bool addTime = false;
+ // bool detectedLine = false;
+ // bool pauseWhile = false;
+ timer.start();
+ unsigned long startTime = timer.read_us();
+ while ((timer.read_us() - startTime) < _maxValue) {
+ unsigned int time = timer.read_us() - startTime;
+ for (i = 0; i < _numSensors; i++) {
+ switch(i){
+ case 0:
+ if (pinout1.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 1:
+ if (pinout2.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 2:
+ if (pinout3.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 3:
+ if (pinout4.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 4:
+ if (pinout5.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ case 5:
+ if (pinout6.read() == 0 && time < sensor_values[i]){
+ sensor_values[i] = time;
+ }
+ break;
+ }
+ }
+ // wait_us(1);
+ // wait_us(300);
+ // timer.stop();
+ // pausedTime = pausedTime + 300;
+ // addTime = true;
+ // timer.start();
+ }
+ // printf("Timer2 %d\n", timer.read_us());
+ // pausedTime = 0;
+ timer.stop();
+ timer.reset();
+}
+
+
+// Derived Analog class constructors
+QTRSensorsAnalog::QTRSensorsAnalog()
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+}
+
+QTRSensorsAnalog::QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ calibratedMinimumOn = 0;
+ calibratedMaximumOn = 0;
+ calibratedMinimumOff = 0;
+ calibratedMaximumOff = 0;
+ _pins = 0;
+ // this is analog - so use analog = true as a parameter
+
+ init(pins, numSensors, numSamplesPerSensor, emitterPin);
+}
+
+
+// the array 'pins' contains the Arduino analog pin assignment for each
+// sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+// Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+// and sensor 3 is on Arduino analog input 7.
+
+// 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+// number of QTR-A sensors you are using). numSensors must be
+// no greater than 16.
+
+// 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+// to average per channel (i.e. per sensor) for each reading. The total
+// number of analog-to-digital conversions performed will be equal to
+// numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+// perform a single analog-to-digital conversion, so:
+// if numSamplesPerSensor is 4 and numSensors is 6, it will take
+// 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+// Increasing this parameter increases noise suppression at the cost of
+// sample rate. The recommended value is 4.
+
+// 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+// modules. If you are using a 1RC (i.e. if there is no emitter pin),
+// or if you just want the emitters on all the time and don't want to
+// use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+void QTRSensorsAnalog::init(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor,
+ unsigned char emitterPin)
+{
+ QTRSensors::init(pins, numSensors, emitterPin);
+
+ _numSamplesPerSensor = numSamplesPerSensor;
+ _maxValue = 1023; // this is the maximum returned by the A/D conversion
+}
+
+
+// Reads the sensor values into an array. There *MUST* be space
+// for as many values as there were sensors specified in the constructor.
+// Example usage:
+// unsigned int sensor_values[8];
+// sensors.read(sensor_values);
+// The values returned are a measure of the reflectance in terms of a
+// 10-bit ADC average with higher values corresponding to lower
+// reflectance (e.g. a black surface or a void).
+void QTRSensorsAnalog::readPrivate(unsigned int *sensor_values)
+{
+ unsigned char i, j;
+
+ if (_pins == 0)
+ return;
+
+ // reset the values
+ for(i = 0; i < _numSensors; i++)
+ sensor_values[i] = 0;
+
+ for (j = 0; j < _numSamplesPerSensor; j++) {
+ for (i = 0; i < _numSensors; i++) {
+ sensor_values[i] += static_cast<unsigned int>(AnalogIn(_pins[i])); // add the conversion result
+ }
+ }
+
+ // get the rounded average of the readings for each sensor
+ for (i = 0; i < _numSensors; i++)
+ sensor_values[i] = (sensor_values[i] + (_numSamplesPerSensor >> 1)) /
+ _numSamplesPerSensor;
+}
+
+// the destructor frees up allocated memory
+QTRSensors::~QTRSensors()
+{
+ if(calibratedMinimumOff)
+ free(calibratedMinimumOff);
+ if(calibratedMinimumOn)
+ free(calibratedMinimumOn);
+ if(calibratedMaximumOff)
+ free(calibratedMaximumOff);
+ if(calibratedMaximumOn)
+ free(calibratedMaximumOn);
+ if (_pins)
+ free(_pins);
+ unsigned int i;
+ // for (i = 0; i < _numSensors; i++) {
+ // if (_analog) {
+ // delete _qtrAIPins[i];
+ // } else {
+ // delete _qtrPins[i];
+ // }
+ // }
+ // if (_analog) {
+ // _qtrAIPins.clear();
+ // vector<AnalogIn *>().swap(_qtrAIPins);
+ // } else {
+ // _qtrPins.clear();
+ // vector<DigitalInOut *>().swap(_qtrPins);
+ // }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/QTRSensors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,283 @@
+/*
+ QTRSensors.h - Library for using Pololu QTR reflectance
+ sensors and reflectance sensor arrays: QTR-1A, QTR-8A, QTR-1RC, and
+ QTR-8RC. The object used will determine the type of the sensor (either
+ QTR-xA or QTR-xRC). Then simply specify in the constructor which
+ Arduino I/O pins are connected to a QTR sensor, and the read() method
+ will obtain reflectance measurements for those sensors. Smaller sensor
+ values correspond to higher reflectance (e.g. white) while larger
+ sensor values correspond to lower reflectance (e.g. black or a void).
+
+ * QTRSensorsRC should be used for QTR-1RC and QTR-8RC sensors.
+ * QTRSensorsAnalog should be used for QTR-1A and QTR-8A sensors.
+*/
+
+/*
+ * Written by Ben Schmidel et al., October 4, 2010
+ * Copyright (c) 2008-2012 Pololu Corporation. For more information, see
+ *
+ * http://www.pololu.com
+ * http://forum.pololu.com
+ * http://www.pololu.com/docs/0J19
+ *
+ * You may freely modify and share this code, as long as you keep this
+ * notice intact (including the two links above). Licensed under the
+ * Creative Commons BY-SA 3.0 license:
+ *
+ * http://creativecommons.org/licenses/by-sa/3.0/
+ *
+ * Disclaimer: To the extent permitted by law, Pololu provides this work
+ * without any warranty. It might be defective, in which case you agree
+ * to be responsible for all resulting costs and damages.
+ *
+ * Modified by Matthew Phillipps, August 24, 2015
+ * Adapted to mbed platform (especially STM Nucleo boards)
+ * Some changes to memory management
+ */
+#include "mbed.h"
+#include <vector>
+
+#ifndef QTRSensors_h
+#define QTRSensors_h
+
+#define QTR_EMITTERS_OFF 0
+#define QTR_EMITTERS_ON 1
+#define QTR_EMITTERS_ON_AND_OFF 2
+
+#define QTR_NO_EMITTER_PIN 255
+
+#define QTR_MAX_SENSORS 16
+#define HIGH 1
+#define LOW 0
+
+// This class cannot be instantiated directly (it has no constructor).
+// Instead, you should instantiate one of its two derived classes (either the
+// QTR-A or QTR-RC version, depending on the type of your sensor).
+class QTRSensors
+{
+ public:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in abstract units,
+ // with higher values corresponding to lower reflectance (e.g. a black
+ // surface or a void).
+ // If measureOffAndOn is true, measures the values with the
+ // emitters on AND off and returns on - (timeout - off). If this
+ // value is less than zero, it returns zero.
+ // This method will call the appropriate derived class's readPrivate(),
+ // which is defined as a virtual function in the base class and
+ // overridden by each derived class's own implementation.
+ void read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Turn the IR LEDs off and on. This is mainly for use by the
+ // read method, and calling these functions before or
+ // after the reading the sensors will have no effect on the
+ // readings, but you may wish to use these for testing purposes.
+ void emittersOff();
+ void emittersOn();
+
+ // Reads the sensors for calibration. The sensor values are
+ // not returned; instead, the maximum and minimum values found
+ // over time are stored internally and used for the
+ // readCalibrated() method.
+ void calibrate(unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Resets all calibration that has been done.
+ void resetCalibration();
+
+ // Returns values calibrated to a value between 0 and 1000, where
+ // 0 corresponds to the minimum value read by calibrate() and 1000
+ // corresponds to the maximum value. Calibration values are
+ // stored separately for each sensor, so that differences in the
+ // sensors are accounted for automatically.
+ void readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON);
+
+ // Operates the same as read calibrated, but also returns an
+ // estimated position of the robot with respect to a line. The
+ // estimate is made using a weighted average of the sensor indices
+ // multiplied by 1000, so that a return value of 0 indicates that
+ // the line is directly below sensor 0, a return value of 1000
+ // indicates that the line is directly below sensor 1, 2000
+ // indicates that it's below sensor 2000, etc. Intermediate
+ // values indicate that the line is between two sensors. The
+ // formula is:
+ //
+ // 0*value0 + 1000*value1 + 2000*value2 + ...
+ // --------------------------------------------
+ // value0 + value1 + value2 + ...
+ //
+ // By default, this function assumes a dark line (high values)
+ // surrounded by white (low values). If your line is light on
+ // black, set the optional second argument white_line to true. In
+ // this case, each sensor value will be replaced by (1000-value)
+ // before the averaging.
+ int readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char white_line = 0);
+
+ unsigned int *calibratedMinimumOn;
+ unsigned int *calibratedMaximumOn;
+ unsigned int *calibratedMinimumOff;
+ unsigned int *calibratedMaximumOff;
+
+ // Calibrated minumum and maximum values. These start at 1000 and
+ // 0, respectively, so that the very first sensor reading will
+ // update both of them.
+ //
+ // The pointers are unallocated until calibrate() is called, and
+ // then allocated to exactly the size required. Depending on the
+ // readMode argument to calibrate, only the On or Off values may
+ // be allocated, as required.
+ //
+ // These variables are made public so that you can use them for
+ // your own calculations and do things like saving the values to
+ // EEPROM, performing sanity checking, etc.
+
+ ~QTRSensors();
+
+ protected:
+
+ QTRSensors()
+ {
+
+ };
+
+ void init(unsigned char *pins, unsigned char numSensors, unsigned char emitterPin, bool);
+
+ bool _analog;
+ PinName *_pins;
+ unsigned char _numSensors;
+ unsigned char _emitterPin;
+ unsigned int _maxValue; // the maximum value returned by this function#
+ int _lastValue;
+ DigitalOut *_emitter;
+ // std::vector<DigitalInOut *> _qtrPins;
+ // std::vector<AnalogIn *> _qtrAIPins;
+
+ private:
+
+ virtual void readPrivate(unsigned int *sensor_values) = 0;
+
+ // Handles the actual calibration. calibratedMinimum and
+ // calibratedMaximum are pointers to the requested calibration
+ // arrays, which will be allocated if necessary.
+ void calibrateOnOrOff(unsigned int **calibratedMaximum,
+ unsigned int **calibratedMinimum,
+ unsigned char readMode);
+};
+
+
+
+// Object to be used for QTR-1RC and QTR-8RC sensors
+class QTRSensorsRC : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsRC();
+
+ // this constructor just calls init()
+ QTRSensorsRC(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 4000, unsigned char emitterPin = 255);
+
+ // The array 'pins' contains the Arduino pin number for each sensor.
+
+ // 'numSensors' specifies the length of the 'pins' array (i.e. the
+ // number of QTR-RC sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'timeout' specifies the length of time in microseconds beyond
+ // which you consider the sensor reading completely black. That is to say,
+ // if the pulse length for a pin exceeds 'timeout', pulse timing will stop
+ // and the reading for that pin will be considered full black.
+ // It is recommended that you set timeout to be between 1000 and
+ // 3000 us, depending on things like the height of your sensors and
+ // ambient lighting. Using timeout allows you to shorten the
+ // duration of a sensor-reading cycle while still maintaining
+ // useful analog measurements of reflectance
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* pins, unsigned char numSensors,
+ unsigned int timeout = 2000, unsigned char emitterPin = QTR_NO_EMITTER_PIN); // NC = Not Connected
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in microseconds.
+ void readPrivate(unsigned int *sensor_values);
+};
+
+
+
+// Object to be used for QTR-1A and QTR-8A sensors
+class QTRSensorsAnalog : public QTRSensors
+{
+ public:
+
+ // if this constructor is used, the user must call init() before using
+ // the methods in this class
+ QTRSensorsAnalog();
+
+ // this constructor just calls init()
+ QTRSensorsAnalog(unsigned char* pins,
+ unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4,
+ unsigned char emitterPin = 255);
+
+ // the array 'pins' contains the Arduino analog pin assignment for each
+ // sensor. For example, if pins is {0, 1, 7}, sensor 1 is on
+ // Arduino analog input 0, sensor 2 is on Arduino analog input 1,
+ // and sensor 3 is on Arduino analog input 7.
+
+ // 'numSensors' specifies the length of the 'analogPins' array (i.e. the
+ // number of QTR-A sensors you are using). numSensors must be
+ // no greater than 16.
+
+ // 'numSamplesPerSensor' indicates the number of 10-bit analog samples
+ // to average per channel (i.e. per sensor) for each reading. The total
+ // number of analog-to-digital conversions performed will be equal to
+ // numSensors*numSamplesPerSensor. Note that it takes about 100 us to
+ // perform a single analog-to-digital conversion, so:
+ // if numSamplesPerSensor is 4 and numSensors is 6, it will take
+ // 4 * 6 * 100 us = ~2.5 ms to perform a full readLine().
+ // Increasing this parameter increases noise suppression at the cost of
+ // sample rate. The recommended value is 4.
+
+ // 'emitterPin' is the Arduino pin that controls the IR LEDs on the 8RC
+ // modules. If you are using a 1RC (i.e. if there is no emitter pin),
+ // or if you just want the emitters on all the time and don't want to
+ // use an I/O pin to control it, use a value of 255 (QTR_NO_EMITTER_PIN).
+ void init(unsigned char* analogPins, unsigned char numSensors,
+ unsigned char numSamplesPerSensor = 4, unsigned char emitterPin = QTR_NO_EMITTER_PIN);
+
+
+
+ private:
+
+ // Reads the sensor values into an array. There *MUST* be space
+ // for as many values as there were sensors specified in the constructor.
+ // Example usage:
+ // unsigned int sensor_values[8];
+ // sensors.read(sensor_values);
+ // The values returned are a measure of the reflectance in terms of a
+ // 10-bit ADC average with higher values corresponding to lower
+ // reflectance (e.g. a black surface or a void).
+ void readPrivate(unsigned int *sensor_values);
+
+ unsigned char _numSamplesPerSensor;
+};
+
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/SumoCollisionDetect.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,506 @@
+#include "PololuBuzzer.h"
+#include "Pushbutton.h"
+#include "ZumoMotors.h"
+#include "ZumoReflectanceSensorArray.h"
+#include "millis.h"
+#include "FXOS8700Q.h"
+#include "sdCard.h"
+#include "mbed.h"
+#include<string>
+
+I2C i2c(I2C_SDA, I2C_SCL);
+#define LED PTD1
+#define PI 3.14159265358979323846
+// #define LOG_SERIAL // write log output to serial port
+
+// Accelerometer Settings
+#define RA_SIZE 3 // number of readings to include in running average of accelerometer readings
+#define XY_ACCELERATION_THRESHOLD 2400 // for detection of contact (~16000 = magnitude of acceleration due to gravity)
+
+// this might need to be tuned for different lighting conditions, surfaces, etc.
+#define QTR_THRESHOLD 1500 // microseconds 1500
+
+// these might need to be tuned for different motor types
+#define REVERSE_SPEED 0.5 // 0 is stopped, 400 is full speed
+#define TURN_SPEED 0.5
+#define FORWARD_SPEED 0.5
+#define SEARCH_SPEED 0.5
+#define SUSTAINED_SPEED 1 // switches to SUSTAINED_SPEED from FULL_SPEED after FULL_SPEED_DURATION_LIMIT ms
+#define FULL_SPEED 1
+#define STOP_DURATION 100000 // ms
+#define REVERSE_DURATION 200000 // microseconds
+#define TURN_DURATION 300000 // microseconds
+
+#define RIGHT 1
+#define LEFT -1
+
+enum ForwardSpeed { SearchSpeed, SustainedSpeed, FullSpeed };
+ForwardSpeed _forwardSpeed; // current forward speed setting
+unsigned long full_speed_start_time;
+#define FULL_SPEED_DURATION_LIMIT 250000 // ms
+
+
+PololuBuzzer buzzer(PTA1);
+ZumoMotors motors(PTD0, PTC12, PTC4, PTC3);
+Pushbutton button(ZUMO_BUTTON); // pushbutton on pin 12
+Timer timerI2C;
+sdCard SD;
+// static BufferedSerial pc(USBTX, USBRX);
+
+unsigned long loop_start_time;
+unsigned long last_turn_time;
+unsigned long contact_made_time;
+#define MIN_DELAY_AFTER_TURN 400000 // ms = min delay before detecting contact event
+#define MIN_DELAY_BETWEEN_CONTACTS 1000000 // ms = min delay between detecting new contact event
+
+#define NUM_SENSORS 6
+unsigned int sensorValues[NUM_SENSORS];
+
+ZumoReflectanceSensorArray sensors(QTR_NO_EMITTER_PIN);
+DigitalOut ledPin(LED, 1);
+// Timer t;
+
+
+// RunningAverage class
+// based on RunningAverage library for Arduino
+// source: https://playground.arduino.cc/Main/RunningAverage
+template <typename T>
+class RunningAverage
+{
+ public:
+ RunningAverage(void);
+ RunningAverage(int);
+ ~RunningAverage();
+ void clear();
+ void addValue(T);
+ T getAverage() const;
+ void fillValue(T, int);
+ protected:
+ int _size;
+ int _cnt;
+ int _idx;
+ T _sum;
+ T * _ar;
+ static T zero;
+};
+
+// Accelerometer Class -- extends the LSM303 Library to support reading and averaging the x-y acceleration
+// vectors from the onboard LSM303DLHC accelerometer/magnetometer
+class Accelerometer : public FXOS8700QAccelerometer
+{
+ typedef struct acc_data_xy
+ {
+ unsigned long timestamp;
+ int x;
+ int y;
+ float dir;
+ } acc_data_xy;
+
+ public:
+ Accelerometer(I2C &i2c, uint8_t addr) : FXOS8700QAccelerometer(i2c, addr), ra_x(RA_SIZE), ra_y(RA_SIZE) {};
+ ~Accelerometer() {};
+ // void enable(void);
+ void getLogHeader(void);
+ void readAcceleration(unsigned long timestamp);
+ float len_xy() const;
+ float dir_xy() const;
+ int x_avg(void) const;
+ int y_avg(void) const;
+ long ss_xy_avg(void) const;
+ float dir_xy_avg(void) const;
+ private:
+ acc_data_xy last;
+ RunningAverage<int> ra_x;
+ RunningAverage<int> ra_y;
+};
+
+// FXOS8700QAccelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1);
+Accelerometer acc(i2c, FXOS8700CQ_SLAVE_ADDR1);
+bool in_contact; // set when accelerometer detects contact with opposing robot
+
+void setForwardSpeed(ForwardSpeed speed);
+
+void waitForButtonAndCountDown()
+{
+ ledPin.write(1);
+ button.waitForButton();
+ ledPin.write(0);
+
+ BuzzerValues buzzing;
+
+ // play audible countdown
+ for (int i = 0; i < 3; i++)
+ {
+ wait_us(1000000);
+ buzzing = buzzer.playNote(NOTE_G(3), 200, 15);
+ buzzer.beep(buzzing.returnFrequency(), buzzing.returnDuration(), buzzing.returnVolume());
+
+ }
+ wait_us(1000000);
+ buzzing = buzzer.playNote(NOTE_G(4), 500, 15);
+ buzzer.beep(buzzing.returnFrequency(), buzzing.returnDuration(), buzzing.returnVolume());
+ wait_us(1000000);
+
+ in_contact = false; // 1 if contact made; 0 if no contact or contact lost
+ contact_made_time = 0;
+ last_turn_time = (timerI2C.read_us()); // prevents false contact detection on initial acceleration
+ _forwardSpeed = SearchSpeed;
+ full_speed_start_time = 0;
+}
+
+template <typename T>
+T RunningAverage<T>::zero = static_cast<T>(0);
+
+template <typename T>
+RunningAverage<T>::RunningAverage(int n)
+{
+ _size = n;
+ _ar = (T*) malloc(_size * sizeof(T));
+ clear();
+}
+
+template <typename T>
+RunningAverage<T>::~RunningAverage()
+{
+ free(_ar);
+}
+
+// resets all counters
+template <typename T>
+void RunningAverage<T>::clear()
+{
+ _cnt = 0;
+ _idx = 0;
+ _sum = zero;
+ for (int i = 0; i< _size; i++) _ar[i] = zero; // needed to keep addValue simple
+}
+
+// adds a new value to the data-set
+template <typename T>
+void RunningAverage<T>::addValue(T f)
+{
+ _sum -= _ar[_idx];
+ _ar[_idx] = f;
+ _sum += _ar[_idx];
+ _idx++;
+ if (_idx == _size) _idx = 0; // faster than %
+ if (_cnt < _size) _cnt++;
+}
+
+// returns the average of the data-set added so far
+template <typename T>
+T RunningAverage<T>::getAverage() const
+{
+ if (_cnt == 0) return zero; // NaN ? math.h
+ return _sum / _cnt;
+}
+
+// fill the average with a value
+// the param number determines how often value is added (weight)
+// number should preferably be between 1 and size
+template <typename T>
+void RunningAverage<T>::fillValue(T value, int number)
+{
+ clear();
+ for (int i = 0; i < number; i++)
+ {
+ addValue(value);
+ }
+}
+
+void setForwardSpeed(ForwardSpeed speed)
+{
+ _forwardSpeed = speed;
+ if (speed == FullSpeed) full_speed_start_time = loop_start_time;
+}
+
+float getForwardSpeed()
+{
+ float speed;
+ switch (_forwardSpeed)
+ {
+ case FullSpeed:
+ speed = FULL_SPEED;
+ // for(int i=0; i<3; i++){
+ // printf("Full Speed %f\n\n\n\n\n", speed);
+ // }
+ break;
+ case SustainedSpeed:
+ speed = SUSTAINED_SPEED;
+ // for(int i=0; i<3; i++){
+ // printf("Sustained Speed %f\n\n\n\n\n", speed);
+ // }
+ break;
+ default:
+ speed = SEARCH_SPEED;
+ // for(int i=0; i<3; i++){
+ // printf("Search Speed %f\n\n\n\n\n", speed);
+ // }
+ break;
+ }
+// for(int i=0; i<100; i++){
+// printf("Speed that should be returned: %f\n", speed);
+// }
+ return speed;
+}
+
+// check for contact, but ignore readings immediately after turning or losing contact
+bool check_for_contact()
+{
+ static long threshold_squared = (long) XY_ACCELERATION_THRESHOLD * (long) XY_ACCELERATION_THRESHOLD;
+// printf("%lu\n", acc.ss_xy_avg());
+ return (acc.ss_xy_avg() > threshold_squared) && \
+ (loop_start_time - last_turn_time > MIN_DELAY_AFTER_TURN) && \
+ (loop_start_time - contact_made_time > MIN_DELAY_BETWEEN_CONTACTS);
+}
+
+// sound horn and accelerate on contact -- fight or flight
+void on_contact_made()
+{
+#ifdef LOG_SERIAL
+ SD.writeToSDCard("contact made\n");
+// printf("contact made");
+// printf("\n");
+#endif
+ in_contact = true;
+ contact_made_time = loop_start_time;
+ setForwardSpeed(FullSpeed);
+// buzzer.playFromProgramSpace(sound_effect);
+}
+
+// reset forward speed
+void on_contact_lost()
+{
+#ifdef LOG_SERIAL
+ SD.writeToSDCard("contact lost\n");
+// printf("contact lost");
+// printf("\n");
+#endif
+ in_contact = false;
+ setForwardSpeed(SearchSpeed);
+}
+
+// execute turn
+// direction: RIGHT or LEFT
+// randomize: to improve searching
+void turn(char direction, bool randomize)
+{
+#ifdef LOG_SERIAL
+ SD.writeToSDCard("turning ...\n");
+// printf("turning ...");
+// printf("\n");
+#endif
+
+ // assume contact lost
+ on_contact_lost();
+
+ static unsigned int duration_increment = TURN_DURATION / 4;
+
+ // motors.setSpeeds(0,0);
+ // delay(STOP_DURATION);
+ motors.setSpeeds(-REVERSE_SPEED, -REVERSE_SPEED);
+ wait_us(REVERSE_DURATION);
+ motors.setSpeeds(TURN_SPEED * direction, -TURN_SPEED * direction);
+ wait_us(randomize ? TURN_DURATION + ((rand() % 8) - 2) * duration_increment : TURN_DURATION);
+ float speed = getForwardSpeed();
+ motors.setSpeeds(speed, speed);
+ last_turn_time = (timerI2C.read_us());
+}
+
+void setup()
+{
+ // uncomment if necessary to correct motor directions
+ //motors.flipLeftMotor(true);
+ //motors.flipRightMotor(true);
+// wait_us(500000); //?? we dont need this it's only for callibration
+ SD.mountFileSystem();
+
+ timerI2C.start();
+
+ // Initiate FXOS8700Q
+ acc.enable();
+
+#ifdef LOG_SERIAL
+// Serial.begin(9600);
+ acc.getLogHeader();
+#endif
+
+ srand((unsigned int)timerI2C.read_us());
+
+ // uncomment if necessary to correct motor directions
+ //motors.flipLeftMotor(true);
+ //motors.flipRightMotor(true);
+
+ ledPin.write(1);
+ waitForButtonAndCountDown();
+}
+
+void loop()
+{
+ if (button.isPressed())
+ {
+ // if button is pressed, stop and wait for another press to go again
+ motors.setSpeeds(0, 0);
+ SD.closeAndUnmount();
+ button.waitForRelease();
+ waitForButtonAndCountDown();
+ }
+
+ loop_start_time = timerI2C.read_us();
+
+ acc.readAcceleration(loop_start_time);
+
+
+ sensors.read(sensorValues);
+
+// int i;
+// for (i = 0; i < NUM_SENSORS; i++) {
+// printf("%d\n", sensorValues[i]);
+// }
+
+ if ((_forwardSpeed == FullSpeed) && (loop_start_time - full_speed_start_time > FULL_SPEED_DURATION_LIMIT))
+ {
+ // for(int i=0; i<100; i++){
+ // printf("set fwd\n");
+ // }
+ setForwardSpeed(SustainedSpeed);
+ }
+
+ if (sensorValues[0] < QTR_THRESHOLD)
+ {
+ // if leftmost sensor detects line, reverse and turn to the right
+ turn(RIGHT, true);
+ }
+ else if (sensorValues[5] < QTR_THRESHOLD)
+ {
+ // if rightmost sensor detects line, reverse and turn to the left
+ turn(LEFT, true);
+ }
+ else // otherwise, go straight
+ {
+ if (check_for_contact()) on_contact_made();
+ float speed = getForwardSpeed();
+ motors.setSpeeds(speed, speed);
+ }
+}
+
+// class Accelerometer -- member function definitions
+
+// enable accelerometer only
+// to enable both accelerometer and magnetometer, call enableDefault() instead
+void Accelerometer::getLogHeader(void)
+{
+ SD.writeToSDCard("millis x y len dir | len_avg dir_avg | avg_len\n");
+// printf("millis x y len dir | len_avg dir_avg | avg_len");
+// printf("\n");
+}
+
+void Accelerometer::readAcceleration(unsigned long timestamp)
+{
+ int16_t x, y, z;
+
+// printf("%d\n", acc.getX(x));
+// printf("%d\n", acc.getY(y));
+
+ a.x = acc.getX(x);
+ a.y = acc.getY(y);
+ a.z = acc.getZ(z);
+// readAcc();
+
+// for(int i=0; i<100; i++){
+// printf("acc");
+// }
+
+ // wait_us(1000000);
+
+
+ // printf("%d\n", a.x);
+ // printf("%d\n", a.y);
+
+ if (a.x == last.x && a.y == last.y) return;
+
+ last.timestamp = timestamp;
+ last.x = a.x;
+ last.y = a.y;
+
+ ra_x.addValue(last.x);
+ ra_y.addValue(last.y);
+
+#ifdef LOG_SERIAL
+ char array[10];
+ sprintf(array, "%lu", last.timestamp);
+ SD.writeToSDCard(array);
+// printf("%lu", last.timestamp);
+ SD.writeToSDCard(" ");
+// printf(" ");
+ sprintf(array, "%d", last.x);
+ SD.writeToSDCard(array);
+// printf("%d", last.x);
+ SD.writeToSDCard(" ");
+// printf(" ");
+ sprintf(array, "%d", last.y);
+ SD.writeToSDCard(array);
+// printf("%d", last.y);
+ SD.writeToSDCard(" ");
+// printf(" ");
+ sprintf(array, "%f", len_xy());
+ SD.writeToSDCard(array);
+// printf("%f", len_xy());
+ SD.writeToSDCard(" ");
+// printf(" ");
+ sprintf(array, "%f", dir_xy());
+ SD.writeToSDCard(array);
+// printf("%f", dir_xy());
+ SD.writeToSDCard(" | ");
+// printf(" | ");
+ sprintf(array, "%f", sqrt(static_cast<float>(ss_xy_avg())));
+ SD.writeToSDCard(array);
+// printf("%f", sqrt(static_cast<float>(ss_xy_avg())));
+ SD.writeToSDCard(" ");
+// printf(" ");
+ sprintf(array, "%f", dir_xy_avg());
+ SD.writeToSDCard(array);
+// printf("%f", dir_xy_avg());
+ SD.writeToSDCard("\n");
+// printf("\n");
+#endif
+}
+
+float Accelerometer::len_xy() const
+{
+ return sqrt((double)last.x*a.x + last.y*a.y);
+}
+
+float Accelerometer::dir_xy() const
+{
+ return atan2((double)last.x, last.y) * 180.0 / PI;
+}
+
+int Accelerometer::x_avg(void) const
+{
+ return ra_x.getAverage();
+}
+
+int Accelerometer::y_avg(void) const
+{
+ return ra_y.getAverage();
+}
+
+long Accelerometer::ss_xy_avg(void) const
+{
+ long x_avg_long = static_cast<long>(x_avg());
+ long y_avg_long = static_cast<long>(y_avg());
+ return x_avg_long*x_avg_long + y_avg_long*y_avg_long;
+}
+
+float Accelerometer::dir_xy_avg(void) const
+{
+ return atan2(static_cast<float>(x_avg()), static_cast<float>(y_avg())) * 180.0 / PI;
+}
+
+int main(){
+ setup();
+ while(1){
+ loop();
+ }
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/ZumoBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,29 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file ZumoBuzzer.h */
+
+#pragma once
+
+#include "PololuBuzzer.h"
+
+/*! \brief Plays beeps and music on the buzzer on the Zumo 32U4.
+ *
+ * This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on
+ * the Zumo 32U4 buzzer.
+ *
+ * Note durations are timed using a timer overflow interrupt
+ * (`TIMER4_OVF`), which will briefly interrupt execution of your
+ * main program at the frequency of the sound being played. In most cases, the
+ * interrupt-handling routine is very short (several microseconds). However,
+ * when playing a sequence of notes in `PLAY_AUTOMATIC` mode (the default mode)
+ * with the `play()` command, this interrupt takes much longer than normal
+ * (perhaps several hundred microseconds) every time it starts a new note. It is
+ * important to take this into account when writing timing-critical code.
+ */
+class ZumoBuzzer : public PololuBuzzer
+{
+ // This is a trivial subclass of PololuBuzzer; it exists because
+ // we wanted the ZumoShield class names to be consistent and we
+ // didn't just use a typedef to define it because that would make
+ // the Doxygen documentation harder to understand.
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/ZumoMotors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,84 @@
+/* File: DRV8835.cpp
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+
+#include "mbed.h"
+#include "ZumoMotors.h"
+
+
+ZumoMotors::ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin) :
+pwmL(pinPwmL),
+Lin(pinLin),
+pwmR(pinPwmR),
+Rin(pinRin)
+{
+ Lin = 0;
+ Rin = 0;
+ pwmL.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmL = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ pwmR.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmR = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::stop()
+{
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::motorL_stop(void)
+{
+ pwmL = 0;
+}
+
+void ZumoMotors::motorR_stop(void)
+{
+ pwmR = 0;
+}
+
+void ZumoMotors::setSpeeds(float Left,float Right)
+{
+ //Set Right Speed and Direction
+ if(Right<0)
+ {
+ motorR_rev(Right*-1);
+ } else {
+ motorR_fwd(Right);
+ }
+
+ //Set Left Speed and Direction
+ if(Left<0)
+ {
+ motorL_rev(Left*-1);
+ } else {
+ motorL_fwd(Left);
+ }
+}
+
+void ZumoMotors::motorL_fwd(float fPulsewidth)
+{
+ Lin = 0;
+ pwmL = fPulsewidth;
+}
+void ZumoMotors::motorL_rev(float fPulsewidth)
+{
+ Lin = 1;
+ pwmL = fPulsewidth;
+}
+
+void ZumoMotors::motorR_fwd(float fPulsewidth)
+{
+ Rin = 0;
+ pwmR = fPulsewidth;
+}
+void ZumoMotors::motorR_rev(float fPulsewidth)
+{
+ Rin = 1;
+ pwmR = fPulsewidth;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/ZumoMotors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,38 @@
+/* File: DRV8835.h
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+#ifndef __DRV8835_H__
+#define __DRV8835_H__
+
+#include "mbed.h"
+
+#define DRV8835_PWM_PERIOD_DEFAULT (0.00005) // 2 ms period // 50KHz (0.00002)
+#define DRV8835_PWM_PULSEWIDTH_DEFAULT (0.50) // 50% duty cycle
+
+class ZumoMotors
+{
+public:
+ ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin);
+
+ void motorL_stop(void);
+ void motorL_fwd(float fPulsewidth);
+ void motorL_rev(float fPulsewidth);
+ void motorR_stop(void);
+ void motorR_fwd(float fPulsewidth);
+ void motorR_rev(float fPulsewidth);
+ void setSpeeds(float Left,float Right);
+ void stop(void);
+
+private:
+ PwmOut pwmL;
+ DigitalOut Lin;
+ PwmOut pwmR;
+ DigitalOut Rin;
+};
+
+#endif /* __DRV8835_H__ */
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/ZumoReflectanceSensorArray.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,406 @@
+/*! \file ZumoReflectanceSensorArray.h
+ *
+ * See the ZumoReflectanceSensorArray class reference for more information about
+ * this library.
+ *
+ * \class ZumoReflectanceSensorArray ZumoReflectanceSensorArray.h
+ * \brief Read from reflectance sensor array
+ *
+ * ZumoReflectanceSensorArray provides an interface for using a
+ * [Zumo Reflectance Sensor Array](https://www.pololu.com/product/1419) connected
+ * to a Zumo robot. It provides access to the raw sensor values as well
+ * as to high level functions including calibration and line-tracking.
+ *
+ * For calibration, memory is allocated using the `malloc()` function. This
+ * conserves RAM: if all six sensors are calibrated with the emitters both on
+ * and off, a total of 48 bytes is dedicated to storing calibration values.
+ * However, for an application where only two sensors are used, and the
+ * emitters are always on during reads, only 4 bytes are required.
+ *
+ * Internally, it uses all standard Arduino functions such as
+ * `micros()` for timing and `digitalRead()` for getting the sensor values, so
+ * it should work on all Arduinos without conflicting with other libraries.
+ *
+ * ### Calibration ###
+ *
+ * ZumoReflectanceSensorArray allows you to use the `calibrate()`
+ * method to easily calibrate your sensors for the particular
+ * conditions it will encounter. Calibrating your sensors can lead to
+ * substantially more reliable sensor readings, which in turn can help
+ * simplify your code. As such, we recommend you build a calibration
+ * phase into your Zumo's initialization routine. This can be as
+ * simple as a fixed duration over which you repeatedly call the
+ * `calibrate()` method.
+ *
+ * During this calibration phase, you will need to expose each of your
+ * reflectance sensors to the lightest and darkest readings they will
+ * encounter. For example, if your Zumo is programmed to be a line
+ * follower, you will want to slide it across the line during the
+ * calibration phase so the each sensor can get a reading of how dark
+ * the line is and how light the ground is (or you can program it to
+ * automatically turn back and forth to pass all of the sensors over
+ * the line). The **SensorCalibration** example demonstrates a
+ * calibration routine.
+ *
+ * ### Reading the sensors
+ *
+ *
+ * ZumoReflectanceSensorArray gives you a number of different ways to
+ * read the sensors.
+ *
+ * - You can request raw sensor values using the `read()` method.
+ *
+ * - You can request calibrated sensor values using the `readCalibrated()`
+ * method. Calibrated sensor values will always range from 0 to 1000, with the
+ * extreme values corresponding to the most and least reflective surfaces
+ * encountered during calibration.
+ *
+ * - For line-detection applications, you can request the line location using
+ * the `readLine()` method. This function provides calibrated values
+ * for each sensor and returns an integer that tells you where it thinks the
+ * line is.
+ *
+ * ### Class inheritance ###
+ *
+ * The ZumoReflectanceSensorArray class is derived from the QTRSensorsRC class,
+ * which is in turn derived from the QTRSensors base class. The QTRSensorsRC and
+ * QTRSensors classes are part of the \ref QTRSensors.h "QTRSensors" library,
+ * which provides more general functionality for working with reflectance
+ * sensors and is included in the Zumo Shield Arduino Library as a dependency
+ * for this library.
+ *
+ * We recommend using ZumoReflectanceSensorArray instead of
+ * the \ref QTRSensors.h "QTRSensors" library when programming an Arduino on a
+ * Zumo. For documentation specific to the %QTRSensors library, please see its
+ * [user's guide](https://www.pololu.com/docs/0J19) on Pololu's website.
+ */
+
+#ifndef ZumoReflectanceSensorArray_h
+#define ZumoReflectanceSensorArray_h
+
+#include "QTRSensors.h"
+#include "mbed.h"
+
+#define ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN 2
+
+class ZumoReflectanceSensorArray : public QTRSensorsRC
+{
+ public:
+
+ /*! \brief Minimal constructor.
+ *
+ * This version of the constructor performs no initialization. If it is used,
+ * the user must call init() before using the methods in this class.
+ */
+ ZumoReflectanceSensorArray() {}
+
+ /*! \brief Constructor; initializes with given emitter pin and defaults for
+ * other settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char emitterPin)` with the specified
+ * emitter pin and default values for other settings.
+ */
+ ZumoReflectanceSensorArray(unsigned char emitterPin)
+ {
+ init(emitterPin);
+ }
+
+ /*! \brief Constructor; initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This constructor calls `init(unsigned char * pins, unsigned char
+ * numSensors, unsigned int timeout, unsigned char emitterPin)` with all
+ * settings as given.
+ */
+ ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+
+ /*! \brief Initializes with given emitter pin and and defaults for other
+ * settings.
+ *
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with the
+ * specified emitter pin. The other settings are set to default values: all
+ * six sensors on the array are active, and a timeout of 2000 microseconds is
+ * used.
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The "LED ON" jumper on the Zumo Reflectance Sensor Array must be
+ * configured correctly for this to work.) If the value `QTR_NO_EMITTER_PIN`
+ * (255) is used, you can leave the emitter pin disconnected and the IR
+ * emitters will always be on.
+ */
+ void init(unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ unsigned char sensorPins[] = { 1, 2, 3, 4, 5, 6 };
+ QTRSensorsRC::init(sensorPins, sizeof(sensorPins), 2000, emitterPin);
+ // pc.printf("hello");
+ }
+
+ /*! \brief Initializes with all settings as given.
+ *
+ * \param pins Array of pin numbers for sensors.
+ * \param numSensors Number of sensors.
+ * \param timeout Maximum duration of reflectance reading in microseconds.
+ * \param emitterPin Pin that turns IR emitters on or off.
+ *
+ * This function initializes the ZumoReflectanceSensorArray object with all
+ * settings as given.
+ *
+ * The array \a pins should contain the Arduino digital pin numbers for each
+ * sensor.
+ *
+ * \a numSensors specifies the length of the \a pins array (the number of
+ * reflectance sensors you are using).
+ *
+ * \a timeout specifies the length of time in microseconds beyond which you
+ * consider the sensor reading completely black. That is to say, if the pulse
+ * length for a pin exceeds \a timeout, pulse timing will stop and the reading
+ * for that pin will be considered full black. It is recommended that you set
+ * \a timeout to be between 1000 and 3000 us, depending on factors like the
+ * height of your sensors and ambient lighting. This allows you to shorten the
+ * duration of a sensor-reading cycle while maintaining useful measurements of
+ * reflectance. If \a timeout is not specified, it defaults to 2000 us. (See
+ * the [product page](https://www.pololu.com/product/1419) for the Zumo
+ * Reflectance Sensor Array on Pololu's website for an overview of the
+ * sensors' principle of operation.)
+ *
+ * \a emitterPin is the Arduino digital pin that controls whether the IR LEDs
+ * are on or off. This pin is optional; if a valid pin is specified, the
+ * emitters will only be turned on during a reading. If \a emitterPin is not
+ * specified, the emitters will be controlled with pin 2 on the Uno (and other
+ * ATmega328/168 boards) or pin A4 on the Leonardo (and other ATmega32U4
+ * boards). (The corresponding connection should be made with the "LED ON"
+ * jumper on the Zumo Reflectance Sensor Array.) If the value
+ * `QTR_NO_EMITTER_PIN` (255) is used, you can leave the emitter pin
+ * disconnected and the IR emitters will always be on.
+ *
+ * This version of `%init()` can be useful if you only want to use a subset
+ * of the six reflectance sensors on the array. For example, using the
+ * outermost two sensors (on pins 4 and 5 by default) is usually enough for
+ * detecting the ring border in sumo competitions:
+ *
+ * ~~~{.ino}
+ * ZumoReflectanceSensorArray reflectanceSensors;
+ *
+ * ...
+ *
+ * reflectanceSensors.init((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ * Alternatively, you can use \ref ZumoReflectanceSensorArray(unsigned char * pins, unsigned char numSensors, unsigned int timeout, unsigned char emitterPin)
+ * "a different constructor" to declare and initialize the object at the same
+ * time:
+ *
+ * ~~~{.ino}
+ *
+ * ZumoReflectanceSensorArray reflectanceSensors((unsigned char[]) {4, 5}, 2);
+ * ~~~
+ *
+ */
+ void init(unsigned char * pins, unsigned char numSensors, unsigned int timeout = 2000,
+ unsigned char emitterPin = ZUMO_SENSOR_ARRAY_DEFAULT_EMITTER_PIN)
+ {
+ QTRSensorsRC::init(pins, numSensors, timeout, emitterPin);
+ }
+};
+
+// documentation for inherited functions
+
+/*! \fn void QTRSensors::read(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+\memberof ZumoReflectanceSensorArray
+ * \brief Reads the raw sensor values into an array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * There **must** be space in the \a sensor_values array for as many values as
+ * there were sensors specified in the constructor. The values returned are
+ * measures of the reflectance in units of microseconds. They will be raw
+ * readings between 0 and the \a timeout argument (in units of microseconds)
+ * provided in the constructor (which defaults to 2000).
+ *
+ * The \a readMode argument specifies the kind of read that will be performed.
+ * Several options are defined:
+ *
+ * - `QTR_EMITTERS_OFF` specifies that the reading should be made without
+ * turning on the infrared (IR) emitters, in which case the reading represents
+ * ambient light levels near the sensor.
+ * - `QTR_EMITTERS_ON` specifies that the emitters should be turned on for the
+ * reading, which results in a measure of reflectance.
+ * - `QTR_EMITTERS_ON_AND_OFF` specifies that a reading should be made in both
+ * the on and off states. The values returned when this option is used are
+ * given by the formula **on + max − off**, where **on** is the reading
+ * with the emitters on, **off** is the reading with the emitters off, and
+ * **max** is the maximum sensor reading. This option can reduce the amount of
+ * interference from uneven ambient lighting.
+ *
+ * Note that emitter control will only work if you specify a valid emitter pin
+ * in the constructor and make the corresponding connection (with the "LED ON"
+ * jumper or otherwise).
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOff()
+ * \brief Turns the IR LEDs off.
+ *
+ * This is mainly for use by the `read()` method, and calling this function
+ * before or after reading the sensors will have no effect on the readings, but
+ * you might wish to use it for testing purposes. This method will only do
+ * something if the emitter pin specified in the constructor is valid (i.e. not
+ * `QTR_NO_EMITTER_PIN`) and the corresponding connection is made.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::emittersOn()
+ * \brief Turns the IR LEDs on.
+ * \copydetails emittersOff
+ */
+
+/*! \fn void QTRSensors::calibrate(unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Reads the sensors for calibration.
+ *
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * The sensor values read by this function are not returned; instead, the
+ * maximum and minimum values found over time are stored internally and used for
+ * the `readCalibrated()` method. You can access the calibration (i.e raw max
+ * and min sensor readings) through the public member pointers
+ * `calibratedMinimumOn`, `calibratedMaximumOn`, `calibratedMinimumOff`, and
+ * `calibratedMaximumOff`. Note that these pointers will point to arrays of
+ * length \a numSensors, as specified in the constructor, and they will only be
+ * allocated **after** `calibrate()` has been called. If you only calibrate with
+ * the emitters on, the calibration arrays that hold the off values will not be
+ * allocated.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::resetCalibration()
+ * \brief Resets all calibration that has been done.
+ *
+ * This function discards the calibration values that have been previously
+ * recorded, resetting the min and max values.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn void QTRSensors::readCalibrated(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON)
+ * \brief Returns sensor readings normalized to values between 0 and 1000.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ *
+ * 0 corresponds to a reading that is less than or equal to the minimum value
+ * read by `calibrate()` and 1000 corresponds to a reading that is greater than
+ * or equal to the maximum value. Calibration values are stored separately for
+ * each sensor, so that differences in the sensors are accounted for
+ * automatically.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+/*! \fn int QTRSensors::readLine(unsigned int *sensor_values, unsigned char readMode = QTR_EMITTERS_ON, unsigned char whiteLine = 0)
+ * \brief Returns an estimated position of a line under the sensor array.
+ *
+ * \param sensor_values Array to populate with sensor readings.
+ * \param readMode Read mode (`QTR_EMITTERS_OFF`, `QTR_EMITTERS_ON`, or
+ * `QTR_EMITTERS_ON_AND_OFF`).
+ * \param whiteLine 0 to detect a dark line on a light surface; 1 to detect
+ * a light line on a dark surface.
+ * \return An estimated line position.
+ *
+ * This function operates the same as `readCalibrated()`, but with a feature
+ * designed for line following: it returns an estimated position of the line.
+ * The estimate is made using a weighted average of the sensor indices
+ * multiplied by 1000, so that a return value of 0 indicates that the line is
+ * directly below sensor 0 (or was last seen by sensor 0 before being lost), a
+ * return value of 1000 indicates that the line is directly below sensor 1, 2000
+ * indicates that it's below sensor 2, etc. Intermediate values indicate that
+ * the line is between two sensors. The formula is:
+ *
+ * \f[
+ * \newcommand{sv}[1]{\mathtt{sensor_values[#1]}}
+ * \text{return value} =
+ * \frac{(0 \times \sv{0}) + (1000 \times \sv{1}) + (2000 \times \sv{2}) + \ldots}
+ * {\sv{0} + \sv{1} + \sv{2} + \ldots}
+ * \f]
+ *
+ * As long as your sensors aren't spaced too far apart relative to the line,
+ * this returned value is designed to be monotonic, which makes it great for use
+ * in closed-loop PID control. Additionally, this method remembers where it last
+ * saw the line, so if you ever lose the line to the left or the right, its line
+ * position will continue to indicate the direction you need to go to reacquire
+ * the line. For example, if sensor 5 is your rightmost sensor and you end up
+ * completely off the line to the left, this function will continue to return
+ * 5000.
+ *
+ * By default, this function assumes a dark line (high values) on a light
+ * background (low values). If your line is light on dark, set the optional
+ * second argument \a whiteLine to true. In this case, each sensor value will be
+ * replaced by the maximum possible value minus its actual value before the
+ * averaging.
+ *
+ * The ZumoReflectanceSensorArray class inherits this function from the
+ * QTRSensors class.
+ */
+
+
+// documentation for inherited member variables
+
+/*!
+ * \property unsigned int * QTRSensors::calibratedMinimumOn
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * on.
+ *
+ * This pointer is unallocated and set to 0 until `calibrate()` is called, and
+ * then allocated to exactly the size required. Depending on the \a readMode
+ * argument to `calibrate()`, only the On or Off values might be allocated, as
+ * required. This variable is made public so that you can use the calibration
+ * values for your own calculations and do things like saving them to EEPROM,
+ * performing sanity checking, etc.
+ *
+ * The ZumoReflectanceSensorArray class inherits this variable from the
+ * QTRSensors class.
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOn
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * on.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMinimumOff
+ * \brief The calibrated minimum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ *
+ * \property unsigned int * QTRSensors::calibratedMaximumOff
+ * \brief The calibrated maximum values measured for each sensor, with emitters
+ * off.
+ * \copydetails QTRSensors::calibratedMinimumOn
+ */
+
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/mbed_app.json Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,7 @@
+{
+ "target_overrides": {
+ "*": {
+ "target.printf_lib": "std"
+ }
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/millis.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,36 @@
+#include "mbed.h"
+#include "millis.h"
+/*
+ millis.cpp
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+volatile unsigned long _millis;
+
+void millisStart(void) {
+ SysTick_Config(SystemCoreClock / 1000);
+}
+
+extern "C" void SysTicks_Handler(void) {
+ _millis++;
+}
+
+unsigned long millis(void) {
+ return _millis;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/SumoCollisionDetect/millis.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,25 @@ +#ifndef MILLIS_H +#define MILLIS_H +/* + millis.h + Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com> + All rights reserved. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +void millisStart(void); +unsigned long millis(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/sdCard.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,100 @@
+/*
+Breadboard recorder program
+
+Copyright 2018 Mollusc Micros Ltd.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+*/
+
+#include "sdCard.h"
+#include "mbed.h"
+#include "SDBlockDevice.h"
+//#include "LittleFileSystem.h"
+#include "FATFileSystem.h"
+#include <string>
+
+#define pin_mosi PTE3
+#define pin_miso PTE1
+#define pin_clk PTE2
+#define pin_cs PTE4
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+SDBlockDevice bd(pin_mosi, pin_miso, pin_clk, pin_cs);
+
+//LittleFileSystem fs("fs");
+FATFileSystem fs("fs");
+
+FILE *f;
+
+sdCard::sdCard(){
+ //does nothing
+}
+
+void sdCard::mountFileSystem(){
+ fflush(stdout);
+ led1 = false;
+ led2 = true;
+ led3 = false;
+ led4 = false;
+
+ int err = fs.mount(&bd);
+ if (err) {
+ printf("Failed to mount filesystem (%d, %s), formatting...\n", err, strerror(-err));
+ fflush(stdout);
+ err = fs.reformat(&bd);
+ printf("err = %d, %s\n", err, strerror(-err));
+ fflush(stdout);
+ // if (err > 0) {
+ // return 1;
+ // }
+ }
+ led1 = true;
+ printf("Mounted filesystem\n");
+ fflush(stdout);
+
+ f = fopen("/fs/SumoCollisionData.txt", "w");
+}
+
+void sdCard::writeToSDCard(const char *__restrict data){
+ if (f) {
+ int err = fprintf(f, data);
+ if (err > 0) {
+ led3 = true;
+ printf("Wrote %d bytes\n", err);
+ } else {
+ printf("Error writing = %d (%s)\n", err, strerror(-err));
+ fflush(stdout);
+ }
+ }
+}
+
+void sdCard::closeAndUnmount(){
+ int err = fclose(f);
+ printf("Error closing = %d (%s)\n", err, strerror(err));
+ fflush(stdout);
+
+ err = fs.unmount();
+ printf("Error unmounting = %d (%s)\n", err, strerror(err));
+ fflush(stdout);
+ led4 = true;
+
+ while (true) {
+ led1 = !led1;
+ wait_us(1000000);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/SumoCollisionDetect/sdCard.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,10 @@
+#include "mbed.h"
+
+class sdCard
+{
+public:
+ sdCard();
+ void mountFileSystem();
+ void writeToSDCard(const char *__restrict data);
+ void closeAndUnmount();
+};
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoBuzzerExample/PololuBuzzer.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,683 @@
+#include "PololuBuzzer.h"
+#include "mbed.h"
+
+using namespace mbed;
+ // constructor
+ /** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+
+BuzzerValues::BuzzerValues(float frequency, float duration, float volume) : freq(frequency), dur(duration), vol(volume) {}
+BuzzerValues::BuzzerValues() {}
+void BuzzerValues::changeValue(float frequency, float duration, float volume) {
+ freq = frequency;
+ dur = duration;
+ vol = volume;
+}
+float BuzzerValues::returnFrequency() {
+ return freq;
+}
+float BuzzerValues::returnDuration() {
+ return dur;
+}
+float BuzzerValues::returnVolume() {
+ return vol;
+}
+
+#define BUZZER_DDR DDRD
+#define BUZZER (1 << PORTD3)
+
+#define TIMER2_CLK_32 0x3 // 500 kHz
+
+static const unsigned int cs2_divider[] = {0, 1, 8, 32, 64, 128, 256, 1024};
+
+#define ENABLE_TIMER_INTERRUPT() TIMSK2 = (1 << TOIE2)
+#define DISABLE_TIMER_INTERRUPT() TIMSK2 = 0
+
+unsigned char buzzerInitialized = 0;
+volatile unsigned char buzzerFinished = 1; // flag: 0 while playing
+const char * volatile buzzerSequence = 0;
+
+// declaring these globals as static means they won't conflict
+// with globals in other .cpp files that share the same name
+static volatile unsigned int buzzerTimeout = 0; // tracks buzzer time limit
+static volatile char play_mode_setting = PLAY_AUTOMATIC;
+
+extern volatile unsigned char buzzerFinished; // flag: 0 while playing
+extern const char * volatile buzzerSequence;
+
+
+static volatile unsigned char use_program_space; // boolean: true if we should
+ // use program space
+
+// music settings and defaults
+static volatile unsigned char octave = 4; // the current octave
+static volatile unsigned int whole_note_duration = 2000; // the duration of a whole note
+static volatile unsigned int note_type = 4; // 4 for quarter, etc
+static volatile unsigned int duration = 500; // the duration of a note in ms
+static volatile unsigned int volume = 15; // the note volume
+static volatile unsigned char staccato = 0; // true if playing staccato
+
+// staccato handling
+static volatile unsigned char staccato_rest_duration; // duration of a staccato rest,
+ // or zero if it is time to play a note
+
+static void nextNote();
+
+PololuBuzzer::PololuBuzzer(PinName pin) : _pwm(pin), playing(0) {
+ _pwm.write(0.0); // after creating it have to be off
+}
+
+ /** stop the beep instantaneous
+ * usually not used
+ */
+void PololuBuzzer::nobeep() {
+ _pwm.write(0.0);
+ buzzerFinished = 1;
+ buzzerSequence = 0;
+ if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+ nextNote();
+ else
+ isPlaying2(0);
+}
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+
+bool PololuBuzzer::isPlaying2(int request){
+ if (request == 0) {
+ playing = false;
+ }
+ else if (request == 1){
+ playing = true;
+ }
+ return playing;
+}
+
+void PololuBuzzer::beep(float freq, float time, float volume) {
+ _pwm.period(1.0/freq);
+ _pwm.write(0.5); // 50% duty cycle - beep on
+ float newTime = (time/500);
+ toff.attach(callback(this, &PololuBuzzer::nobeep), newTime);
+}
+
+// Timer2 overflow interrupt
+// ISR (TIMER2_OVF_vect)
+// {
+// if (buzzerTimeout-- == 0)
+// {
+// DISABLE_TIMER_INTERRUPT();
+// sei(); // re-enable global interrupts (nextNote() is very slow)
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+// buzzerFinished = 1;
+// if (buzzerSequence && (play_mode_setting == PLAY_AUTOMATIC))
+// nextNote();
+// }
+// }
+
+
+// this is called by playFrequency()
+inline void PololuBuzzer::init()
+{
+ if (!buzzerInitialized)
+ {
+ buzzerInitialized = 1;
+ //init2();
+ }
+}
+
+// initializes timer4 (32U4) or timer2 (328P) for buzzer control
+// void PololuBuzzer::init2()
+// {
+// DISABLE_TIMER_INTERRUPT();
+
+// TCCR2A = 0x21; // bits 7 and 6 clear: normal port op., OC4A disconnected
+// // bit 5 set, 4 clear: clear OC2B on comp match when upcounting,
+// // set OC2B on comp match when downcounting
+// // bits 3 and 2: not used
+// // bit 1 clear, 0 set: combine with bit 3 of TCCR2B...
+
+// TCCR2B = 0x0B; // bit 7 clear: no force output compare for channel A
+// // bit 6 clear: no force output compare for channel B
+// // bits 5 and 4: not used
+// // bit 3 set: combine with bits 1 and 0 of TCCR2A to
+// // select waveform generation mode 5, phase-correct PWM,
+// // TOP = OCR2A, OCR2B set at TOP, TOV2 flag set at BOTTOM
+// // bit 2 clear, 1-0 set: timer clock = clkT2S/32
+
+ // This sets timer 2 to run in phase-correct PWM mode, where TOP = OCR2A,
+ // OCR2B is updated at TOP, TOV2 Flag is set on BOTTOM. OC2B is cleared
+ // on compare match when upcounting, set on compare match when downcounting;
+ // OC2A is disconnected.
+ // Note: if the PWM frequency and duty cycle are changed, the first
+ // cycle of the new frequency will be at the old duty cycle, since
+ // the duty cycle (OCR2B) is not updated until TOP.
+
+
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// BUZZER_DDR |= BUZZER; // buzzer pin set as an output
+// sei();
+// }
+
+
+// Set up the timer to play the desired frequency (in Hz or .1 Hz) for the
+// the desired duration (in ms). Allowed frequencies are 40 Hz to 10 kHz.
+// volume controls buzzer volume, with 15 being loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playFrequency(unsigned int freq, unsigned int dur,
+ unsigned char volume)
+{
+ init(); // initializes the buzzer if necessary
+ buzzerFinished = 0;
+
+ unsigned int timeout;
+ unsigned char multiplier = 1;
+
+ if (freq & DIV_BY_10) // if frequency's DIV_BY_10 bit is set
+ { // then the true frequency is freq/10
+ multiplier = 10; // (gives higher resolution for small freqs)
+ freq &= ~DIV_BY_10; // clear DIV_BY_10 bit
+ }
+
+ unsigned char min = 40 * multiplier;
+ if (freq < min) // min frequency allowed is 40 Hz
+ freq = min;
+ if (multiplier == 1 && freq > 10000)
+ freq = 10000; // max frequency allowed is 10kHz
+
+// unsigned int top;
+// unsigned char newCS2 = 2; // try prescaler divider of 8 first (minimum necessary for 10 kHz)
+// unsigned int divider = cs2_divider[newCS2];
+
+// // calculate necessary clock source and counter top value to get freq
+// top = (unsigned int)(((F_CPU/16 * multiplier) + (freq >> 1))/ freq);
+
+// while (top > 255)
+// {
+// divider = cs2_divider[++newCS2];
+// top = (unsigned int)(((F_CPU/2/divider * multiplier) + (freq >> 1))/ freq);
+// }
+
+ // set timeout (duration):
+ if (multiplier == 10)
+ freq = (freq + 5) / 10;
+
+ if (freq == 1000)
+ timeout = dur; // duration for silent notes is exact
+ else
+ timeout = (unsigned int)((long)dur * freq / 1000);
+
+ if (volume > 15)
+ volume = 15;
+
+ BuzzerValues buzz(freq,timeout,volume);
+ return buzz;
+
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts while writing to registers
+
+// TCCR2B = (TCCR2B & 0xF8) | newCS2; // select timer 2 clock prescaler
+// OCR2A = top; // set timer 2 pwm frequency
+// OCR2B = top >> (16 - volume); // set duty cycle (volume)
+// buzzerTimeout = timeout; // set buzzer duration
+
+// TIFR2 |= 0xFF; // clear any pending t2 overflow int.
+
+// ENABLE_TIMER_INTERRUPT();
+}
+
+
+
+// Determine the frequency for the specified note, then play that note
+// for the desired duration (in ms). This is done without using floats
+// and without having to loop. volume controls buzzer volume, with 15 being
+// loudest and 0 being quietest.
+// Note: frequency*duration/1000 must be less than 0xFFFF (65535). This
+// means that you can't use a max duration of 65535 ms for frequencies
+// greater than 1 kHz. For example, the max duration you can use for a
+// frequency of 10 kHz is 6553 ms. If you use a duration longer than this,
+// you will cause an integer overflow that produces unexpected behavior.
+BuzzerValues PololuBuzzer::playNote(unsigned char note, unsigned int dur,
+ unsigned char volume)
+{
+ // note = key + octave * 12, where 0 <= key < 12
+ // example: A4 = A + 4 * 12, where A = 9 (so A4 = 57)
+ // A note is converted to a frequency by the formula:
+ // Freq(n) = Freq(0) * a^n
+ // where
+ // Freq(0) is chosen as A4, which is 440 Hz
+ // and
+ // a = 2 ^ (1/12)
+ // n is the number of notes you are away from A4.
+ // One can see that the frequency will double every 12 notes.
+ // This function exploits this property by defining the frequencies of the
+ // 12 lowest notes allowed and then doubling the appropriate frequency
+ // the appropriate number of times to get the frequency for the specified
+ // note.
+
+ // if note = 16, freq = 41.2 Hz (E1 - lower limit as freq must be >40 Hz)
+ // if note = 57, freq = 440 Hz (A4 - central value of ET Scale)
+ // if note = 111, freq = 9.96 kHz (D#9 - upper limit, freq must be <10 kHz)
+ // if note = 255, freq = 1 kHz and buzzer is silent (silent note)
+
+ // The most significant bit of freq is the "divide by 10" bit. If set,
+ // the units for frequency are .1 Hz, not Hz, and freq must be divided
+ // by 10 to get the true frequency in Hz. This allows for an extra digit
+ // of resolution for low frequencies without the need for using floats.
+
+ unsigned int freq = 0;
+ unsigned char offset_note = note - 16;
+ BuzzerValues buzz;
+
+ if (note == SILENT_NOTE || volume == 0)
+ {
+ freq = 1000; // silent notes => use 1kHz freq (for cycle counter)
+ buzz.changeValue(freq,dur,volume);
+ // buzz = playFrequency(freq, dur, 0);
+ // return buzz;
+ }
+ else {
+ if (note <= 16)
+ offset_note = 0;
+ else if (offset_note > 95)
+ offset_note = 95;
+
+ unsigned char exponent = offset_note / 12;
+
+ // frequency table for the lowest 12 allowed notes
+ // frequencies are specified in tenths of a Hertz for added resolution
+ switch (offset_note - exponent * 12) // equivalent to (offset_note % 12)
+ {
+ case 0: // note E1 = 41.2 Hz
+ freq = 412;
+ break;
+ case 1: // note F1 = 43.7 Hz
+ freq = 437;
+ break;
+ case 2: // note F#1 = 46.3 Hz
+ freq = 463;
+ break;
+ case 3: // note G1 = 49.0 Hz
+ freq = 490;
+ break;
+ case 4: // note G#1 = 51.9 Hz
+ freq = 519;
+ break;
+ case 5: // note A1 = 55.0 Hz
+ freq = 550;
+ break;
+ case 6: // note A#1 = 58.3 Hz
+ freq = 583;
+ break;
+ case 7: // note B1 = 61.7 Hz
+ freq = 617;
+ break;
+ case 8: // note C2 = 65.4 Hz
+ freq = 654;
+ break;
+ case 9: // note C#2 = 69.3 Hz
+ freq = 693;
+ break;
+ case 10: // note D2 = 73.4 Hz
+ freq = 734;
+ break;
+ case 11: // note D#2 = 77.8 Hz
+ freq = 778;
+ break;
+ }
+ if (exponent < 7)
+ {
+ freq = freq << exponent; // frequency *= 2 ^ exponent
+ if (exponent > 1) // if the frequency is greater than 160 Hz
+ freq = (freq + 5) / 10; // we don't need the extra resolution
+ else
+ freq += DIV_BY_10; // else keep the added digit of resolution
+ }
+ else
+ freq = (freq * 64 + 2) / 5; // == freq * 2^7 / 10 without int overflow
+
+ if (volume > 15)
+ volume = 15;
+ buzz = playFrequency(freq, dur, volume); // set buzzer this freq/duration
+ }
+ return buzz;
+}
+
+
+
+// Returns 1 if the buzzer is currently playing, otherwise it returns 0
+unsigned char PololuBuzzer::isPlaying()
+{
+ return !buzzerFinished || buzzerSequence != 0;
+}
+
+
+// Plays the specified sequence of notes. If the play mode is
+// PLAY_AUTOMATIC, the sequence of notes will play with no further
+// action required by the user. If the play mode is PLAY_CHECK,
+// the user will need to call playCheck() in the main loop to initiate
+// the playing of each new note in the sequence. The play mode can
+// be changed while the sequence is playing.
+// This is modeled after the PLAY commands in GW-BASIC, with just a
+// few differences.
+//
+// The notes are specified by the characters C, D, E, F, G, A, and
+// B, and they are played by default as "quarter notes" with a
+// length of 500 ms. This corresponds to a tempo of 120
+// beats/min. Other durations can be specified by putting a number
+// immediately after the note. For example, C8 specifies C played as an
+// eighth note, with half the duration of a quarter note. The special
+// note R plays a rest (no sound).
+//
+// Various control characters alter the sound:
+// '>' plays the next note one octave higher
+//
+// '<' plays the next note one octave lower
+//
+// '+' or '#' after a note raises any note one half-step
+//
+// '-' after a note lowers any note one half-step
+//
+// '.' after a note "dots" it, increasing the length by
+// 50%. Each additional dot adds half as much as the
+// previous dot, so that "A.." is 1.75 times the length of
+// "A".
+//
+// 'O' followed by a number sets the octave (default: O4).
+//
+// 'T' followed by a number sets the tempo (default: T120).
+//
+// 'L' followed by a number sets the default note duration to
+// the type specified by the number: 4 for quarter notes, 8
+// for eighth notes, 16 for sixteenth notes, etc.
+// (default: L4)
+//
+// 'V' followed by a number from 0-15 sets the music volume.
+// (default: V15)
+//
+// 'MS' sets all subsequent notes to play staccato - each note is played
+// for 1/2 of its allotted time, followed by an equal period
+// of silence.
+//
+// 'ML' sets all subsequent notes to play legato - each note is played
+// for its full length. This is the default setting.
+//
+// '!' resets all persistent settings to their defaults.
+//
+// The following plays a c major scale up and back down:
+// play("L16 V8 cdefgab>cbagfedc");
+//
+// Here is an example from Bach:
+// play("T240 L8 a gafaeada c+adaeafa <aa<bac#ada c#adaeaf4");
+void PololuBuzzer::play(const char *notes)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes;
+ use_program_space = 0;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+void PololuBuzzer::playFromProgramSpace(const char *notes_p)
+{
+// DISABLE_TIMER_INTERRUPT(); // prevent this from being interrupted
+ buzzerSequence = notes_p;
+ use_program_space = 1;
+ staccato_rest_duration = 0;
+ nextNote(); // this re-enables the timer interrupt
+}
+
+
+// // stop all sound playback immediately
+// void PololuBuzzer::stopPlaying()
+// {
+// DISABLE_TIMER_INTERRUPT(); // disable interrupts
+
+
+// TCCR2B = (TCCR2B & 0xF8) | TIMER2_CLK_32; // select IO clock
+// OCR2A = (F_CPU/64) / 1000; // set TOP for freq = 1 kHz
+// OCR2B = 0; // 0% duty cycle
+
+// buzzerFinished = 1;
+// buzzerSequence = 0;
+// }
+
+// Gets the current character, converting to lower-case and skipping
+// spaces. For any spaces, this automatically increments sequence!
+static char currentCharacter()
+{
+ char c = 0;
+ do
+ {
+ // if(use_program_space)
+ // c = pgm_read_byte(buzzerSequence);
+ // else
+ c = *buzzerSequence;
+
+ if(c >= 'A' && c <= 'Z')
+ c += 'a'-'A';
+ } while(c == ' ' && (buzzerSequence ++));
+
+ return c;
+}
+
+// Returns the numerical argument specified at buzzerSequence[0] and
+// increments sequence to point to the character immediately after the
+// argument.
+static unsigned int getNumber()
+{
+ unsigned int arg = 0;
+
+ // read all digits, one at a time
+ char c = currentCharacter();
+ while(c >= '0' && c <= '9')
+ {
+ arg *= 10;
+ arg += c-'0';
+ buzzerSequence ++;
+ c = currentCharacter();
+ }
+
+ return arg;
+}
+
+static void nextNote()
+{
+ unsigned char note = 0;
+ unsigned char rest = 0;
+ unsigned char tmp_octave = octave; // the octave for this note
+ unsigned int tmp_duration; // the duration of this note
+ unsigned int dot_add;
+
+ char c; // temporary variable
+
+ // if we are playing staccato, after every note we play a rest
+ if(staccato && staccato_rest_duration)
+ {
+ PololuBuzzer::playNote(SILENT_NOTE, staccato_rest_duration, 0);
+ staccato_rest_duration = 0;
+ return;
+ }
+
+ parse_character:
+
+ // Get current character
+ c = currentCharacter();
+ buzzerSequence ++;
+
+ // Interpret the character.
+ switch(c)
+ {
+ case '>':
+ // shift the octave temporarily up
+ tmp_octave ++;
+ goto parse_character;
+ case '<':
+ // shift the octave temporarily down
+ tmp_octave --;
+ goto parse_character;
+ case 'a':
+ note = NOTE_A(0);
+ break;
+ case 'b':
+ note = NOTE_B(0);
+ break;
+ case 'c':
+ note = NOTE_C(0);
+ break;
+ case 'd':
+ note = NOTE_D(0);
+ break;
+ case 'e':
+ note = NOTE_E(0);
+ break;
+ case 'f':
+ note = NOTE_F(0);
+ break;
+ case 'g':
+ note = NOTE_G(0);
+ break;
+ case 'l':
+ // set the default note duration
+ note_type = getNumber();
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'm':
+ // set music staccato or legato
+ if(currentCharacter() == 'l')
+ staccato = false;
+ else
+ {
+ staccato = true;
+ staccato_rest_duration = 0;
+ }
+ buzzerSequence ++;
+ goto parse_character;
+ case 'o':
+ // set the octave permanently
+ octave = tmp_octave = getNumber();
+ goto parse_character;
+ case 'r':
+ // Rest - the note value doesn't matter.
+ rest = 1;
+ break;
+ case 't':
+ // set the tempo
+ whole_note_duration = 60*400/getNumber()*10;
+ duration = whole_note_duration/note_type;
+ goto parse_character;
+ case 'v':
+ // set the volume
+ volume = getNumber();
+ goto parse_character;
+ case '!':
+ // reset to defaults
+ octave = 4;
+ whole_note_duration = 2000;
+ note_type = 4;
+ duration = 500;
+ volume = 15;
+ staccato = 0;
+ // reset temp variables that depend on the defaults
+ tmp_octave = octave;
+ tmp_duration = duration;
+ goto parse_character;
+ default:
+ buzzerSequence = 0;
+ return;
+ }
+
+ note += tmp_octave*12;
+
+ // handle sharps and flats
+ c = currentCharacter();
+ while(c == '+' || c == '#')
+ {
+ buzzerSequence ++;
+ note ++;
+ c = currentCharacter();
+ }
+ while(c == '-')
+ {
+ buzzerSequence ++;
+ note --;
+ c = currentCharacter();
+ }
+
+ // set the duration of just this note
+ tmp_duration = duration;
+
+ // If the input is 'c16', make it a 16th note, etc.
+ if(c > '0' && c < '9')
+ tmp_duration = whole_note_duration/getNumber();
+
+ // Handle dotted notes - the first dot adds 50%, and each
+ // additional dot adds 50% of the previous dot.
+ dot_add = tmp_duration/2;
+ while(currentCharacter() == '.')
+ {
+ buzzerSequence ++;
+ tmp_duration += dot_add;
+ dot_add /= 2;
+ }
+
+ if(staccato)
+ {
+ staccato_rest_duration = tmp_duration / 2;
+ tmp_duration -= staccato_rest_duration;
+ }
+
+ // this will re-enable the timer overflow interrupt
+ //PololuBuzzer::playNote(rest ? SILENT_NOTE : note, tmp_duration, volume);
+ wait_us(tmp_duration*1000);
+}
+
+
+// This puts play() into a mode where instead of advancing to the
+// next note in the sequence automatically, it waits until the
+// function playCheck() is called. The idea is that you can
+// put playCheck() in your main loop and avoid potential
+// delays due to the note sequence being checked in the middle of
+// a time sensitive calculation. It is recommended that you use
+// this function if you are doing anything that can't tolerate
+// being interrupted for more than a few microseconds.
+// Note that the play mode can be changed while a sequence is being
+// played.
+//
+// Usage: playMode(PLAY_AUTOMATIC) makes it automatic (the
+// default), playMode(PLAY_CHECK) sets it to a mode where you have
+// to call playCheck().
+void PololuBuzzer::playMode(unsigned char mode)
+{
+ play_mode_setting = mode;
+
+ // We want to check to make sure that we didn't miss a note if we
+ // are going out of play-check mode.
+ if(mode == PLAY_AUTOMATIC)
+ playCheck();
+}
+
+
+// Checks whether it is time to start another note, and starts
+// it if so. If it is not yet time to start the next note, this method
+// returns without doing anything. Call this as often as possible
+// in your main loop to avoid delays between notes in the sequence.
+//
+// Returns true if it is still playing.
+unsigned char PololuBuzzer::playCheck()
+{
+ if(buzzerFinished && buzzerSequence != 0)
+ nextNote();
+ return buzzerSequence != 0;
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoBuzzerExample/PololuBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,126 @@
+#pragma once
+#include "mbed.h"
+
+#ifndef MBED_BEEP_H
+#define MBED_BEEP_H
+
+/*! \brief Specifies that the sequence of notes will play with no further action
+ * required by the user. */
+#define PLAY_AUTOMATIC 0
+
+
+/*! \brief Specified that the user will need to call `playCheck()` regularly. */
+#define PLAY_CHECK 1
+
+// n
+// Equal Tempered Scale is given by f = f * a
+// n o
+//
+// where f is chosen as A above middle C (A4) at f = 440 Hz
+// o o
+// and a is given by the twelfth root of 2 (~1.059463094359)
+
+/*! \anchor note_macros
+ *
+ * \name Note Macros
+ * \a x specifies the octave of the note
+ * @{
+ */
+#define NOTE_C(x) ( 0 + (x)*12)
+#define NOTE_C_SHARP(x) ( 1 + (x)*12)
+#define NOTE_D_FLAT(x) ( 1 + (x)*12)
+#define NOTE_D(x) ( 2 + (x)*12)
+#define NOTE_D_SHARP(x) ( 3 + (x)*12)
+#define NOTE_E_FLAT(x) ( 3 + (x)*12)
+#define NOTE_E(x) ( 4 + (x)*12)
+#define NOTE_F(x) ( 5 + (x)*12)
+#define NOTE_F_SHARP(x) ( 6 + (x)*12)
+#define NOTE_G_FLAT(x) ( 6 + (x)*12)
+#define NOTE_G(x) ( 7 + (x)*12)
+#define NOTE_G_SHARP(x) ( 8 + (x)*12)
+#define NOTE_A_FLAT(x) ( 8 + (x)*12)
+#define NOTE_A(x) ( 9 + (x)*12)
+#define NOTE_A_SHARP(x) (10 + (x)*12)
+#define NOTE_B_FLAT(x) (10 + (x)*12)
+#define NOTE_B(x) (11 + (x)*12)
+
+/*! \brief silences buzzer for the note duration */
+#define SILENT_NOTE 0xFF
+
+/*! \brief frequency bit that indicates Hz/10<br>
+ * e.g. \a frequency = `(445 | DIV_BY_10)` gives a frequency of 44.5 Hz
+ */
+#define DIV_BY_10 (1 << 15)
+/*! @} */
+
+namespace mbed {
+
+class BuzzerValues{
+ private:
+ float freq;
+ float dur;
+ float vol;
+ public:
+ BuzzerValues(float frequency, float duration, float volume);
+ BuzzerValues();
+ void changeValue(float frequency, float duration, float volume);
+ float returnFrequency();
+ float returnDuration();
+ float returnVolume();
+};
+
+/* Class: PololuBuzzer
+ * A class which uses pwm to controle a beeper to generate sounds.
+ */
+class PololuBuzzer {
+private:
+ PwmOut _pwm;
+ Timeout toff;
+ bool playing;
+
+public:
+
+/** Create a Beep object connected to the specified PwmOut pin
+ *
+ * @param pin PwmOut pin to connect to
+ */
+ PololuBuzzer (PinName pin);
+
+/** Beep with given frequency and duration.
+ *
+ * @param frequency - the frequency of the tone in Hz
+ * @param time - the duration of the tone in seconds
+ */
+ void beep (float frequency, float time, float volume);
+
+/** stop the beep instantaneous
+ * usually not used
+ */
+ void nobeep();
+
+ static BuzzerValues playFrequency(unsigned int freq, unsigned int duration,
+ unsigned char volume);
+
+ static BuzzerValues playNote(unsigned char note, unsigned int duration,
+ unsigned char volume);
+
+ static void play(const char *sequence);
+
+ static void playFromProgramSpace(const char *sequence);
+
+ static void playMode(unsigned char mode);
+
+ static unsigned char playCheck();
+
+ static unsigned char isPlaying();
+
+ bool isPlaying2(int request);
+
+ //static void stopPlaying();
+
+ // initializes timer for buzzer control
+// static void init2();
+ static void init();
+ };
+}
+#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoBuzzerExample/Pushbutton.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,151 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+#include "Pushbutton.h"
+#include "mbed.h"
+#include "millis.h"
+
+// \cond
+
+PushbuttonStateMachine::PushbuttonStateMachine()
+{
+ state = 0;
+}
+
+// state 0: The value is considered to be true.
+// state 1: The value was considered to be true, but there
+// was a recent false reading so it might be falling.
+// state 2: The value is considered to be false.
+// state 3: The value was considered to be false, but there
+// was a recent true reading so it might be rising.
+//
+// The transition from state 3 to state 0 is the point where we
+// have successfully detected a rising edge an we return true.
+//
+// The prevTimeMillis variable holds the last time that we
+// transitioned to states 1 or 3.
+bool PushbuttonStateMachine::getSingleDebouncedRisingEdge(bool value)
+{
+ uint16_t timeMillis = millis();
+
+ switch (state)
+ {
+ case 0:
+ // If value is false, proceed to the next state.
+ if (!value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 1;
+ }
+ break;
+
+ case 1:
+ if (value)
+ {
+ // The value is true or bouncing, so go back to previous (initial)
+ // state.
+ state = 0;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still false, so
+ // proceed to the next state.
+ state = 2;
+ }
+ break;
+
+ case 2:
+ // If the value is true, proceed to the next state.
+ if (value)
+ {
+ prevTimeMillis = timeMillis;
+ state = 3;
+ }
+ break;
+
+ case 3:
+ if (!value)
+ {
+ // The value is false or bouncing, so go back to previous state.
+ state = 2;
+ }
+ else if ((uint16_t)(timeMillis - prevTimeMillis) >= 15)
+ {
+ // It has been at least 15 ms and the value is still true, so
+ // go back to the initial state and report this rising edge.
+ state = 0;
+ return true;
+ }
+ break;
+ }
+
+ return false;
+}
+
+// \endcond
+
+void PushbuttonBase::waitForPress()
+{
+ do
+ {
+ while (!isPressed()); // wait for button to be pressed
+ wait_us(10000); // debounce the button press
+ }
+ while (!isPressed()); // if button isn't still pressed, loop
+}
+
+void PushbuttonBase::waitForRelease()
+{
+ do
+ {
+ while (isPressed()); // wait for button to be released
+ wait_us(10000); // debounce the button release
+ }
+ while (isPressed()); // if button isn't still released, loop
+}
+
+void PushbuttonBase::waitForButton()
+{
+ waitForPress();
+ waitForRelease();
+}
+
+bool PushbuttonBase::getSingleDebouncedPress()
+{
+ return pressState.getSingleDebouncedRisingEdge(isPressed());
+}
+
+bool PushbuttonBase::getSingleDebouncedRelease()
+{
+ return releaseState.getSingleDebouncedRisingEdge(!isPressed());
+}
+
+Pushbutton::Pushbutton(PinName pin, uint8_t pullUp, uint8_t defaultState)
+{
+ initialized = false;
+ _pin = pin;
+ _pullUp = pullUp;
+ _defaultState = defaultState;
+}
+
+void Pushbutton::init2()
+{
+ if (_pullUp == PULL_UP_ENABLED)
+ {
+ DigitalIn myPin(_pin, PullUp);
+ }
+ else
+ {
+ DigitalIn myPin(_pin);
+ //pinMode(_pin, INPUT); // high impedance
+ }
+
+ wait_us(5); // give pull-up time to stabilize
+}
+
+bool Pushbutton::isPressed()
+{
+ init(); // initialize if needed
+ DigitalIn myPin(_pin);
+
+ return myPin != _defaultState;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoBuzzerExample/Pushbutton.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,176 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file Pushbutton.h
+ *
+ * This is the main header file for the %Pushbutton library.
+ *
+ * For an overview of the library's features, see
+ * https://github.com/pololu/pushbutton-arduino. That is the main repository
+ * for the library, though copies of the library may exist in other
+ * repositories. */
+
+#pragma once
+
+#include "mbed.h"
+
+/*! Indicates the that pull-up resistor should be disabled. */
+#define PULL_UP_DISABLED 0
+
+/*! Indicates the that pull-up resistor should be enabled. */
+#define PULL_UP_ENABLED 1
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads low. */
+#define DEFAULT_STATE_LOW 0
+
+/*! Indicates that the default (released) state of the button is when the
+ * I/O line reads high. */
+#define DEFAULT_STATE_HIGH 1
+
+/*! The pin used for the button on the
+ * [Zumo Shield for Arduino](http://www.pololu.com/product/2508).
+ *
+ * This does not really belong here in this general pushbutton library and will
+ * probably be removed in the future. */
+#define ZUMO_BUTTON PTD3
+
+// \cond
+/** \brief A state machine that detects when a boolean value changes from false
+ * to true, with debouncing.
+ *
+ * This should be considered a private implementation detail of the library.
+ */
+class PushbuttonStateMachine
+{
+public:
+
+ PushbuttonStateMachine();
+
+ /** This should be called repeatedly in a loop. It will return true once after
+ * each time it detects the given value changing from false to true. */
+ bool getSingleDebouncedRisingEdge(bool value);
+
+private:
+
+ uint8_t state;
+ uint16_t prevTimeMillis;
+};
+// \endcond
+
+/*! \brief General pushbutton class that handles debouncing.
+ *
+ * This is an abstract class used for interfacing with pushbuttons. It knows
+ * about debouncing, but it knows nothing about how to read the current state of
+ * the button. The functions in this class get the current state of the button
+ * by calling isPressed(), a virtual function which must be implemented in a
+ * subclass of PushbuttonBase, such as Pushbutton.
+ *
+ * Most users of this library do not need to directly use PushbuttonBase or even
+ * know that it exists. They can use Pushbutton instead.*/
+class PushbuttonBase
+{
+public:
+
+ /*! \brief Waits until the button is pressed and takes care of debouncing.
+ *
+ * This function waits until the button is in the pressed state and then
+ * returns. Note that if the button is already pressed when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForPress();
+
+ /*! \brief Waits until the button is released and takes care of debouncing.
+ *
+ * This function waits until the button is in the released state and then
+ * returns. Note that if the button is already released when you call this
+ * function, it will return quickly (in 10 ms). */
+ void waitForRelease();
+
+ /*! \brief Waits until the button is pressed and then waits until the button
+ * is released, taking care of debouncing.
+ *
+ * This is equivalent to calling waitForPress() and then waitForRelease(). */
+ void waitForButton();
+
+ /*! \brief Uses a state machine to return true once after each time it detects
+ * the button moving from the released state to the pressed state.
+ *
+ * This is a non-blocking function that is meant to be called repeatedly in a
+ * loop. Each time it is called, it updates a state machine that monitors the
+ * state of the button. When it detects the button changing from the released
+ * state to the pressed state, with debouncing, it returns true. */
+ bool getSingleDebouncedPress();
+
+ /*! \brief Uses a state machine to return true once after each time it detects the button moving from the pressed state to the released state.
+ *
+ * This is just like getSingleDebouncedPress() except it has a separate state
+ * machine and it watches for when the button goes from the pressed state to
+ * the released state.
+ *
+ * There is no strict guarantee that every debounced button press event
+ * returned by getSingleDebouncedPress() will have a corresponding
+ * button release event returned by getSingleDebouncedRelease(); the two
+ * functions use independent state machines and sample the button at different
+ * times. */
+ bool getSingleDebouncedRelease();
+
+ /*! \brief indicates whether button is currently pressed without any debouncing.
+ *
+ * @return 1 if the button is pressed right now, 0 if it is not.
+ *
+ * This function must be implemented in a subclass of PushbuttonBase, such as
+ * Pushbutton. */
+ virtual bool isPressed() = 0;
+
+private:
+
+ PushbuttonStateMachine pressState;
+ PushbuttonStateMachine releaseState;
+};
+
+/** \brief Main class for interfacing with pushbuttons.
+ *
+ * This class can interface with any pushbutton whose state can be read with
+ * the `digitalRead` function, which is part of the Arduino core.
+ *
+ * See https://github.com/pololu/pushbutton-arduino for an overview
+ * of the different ways to use this class. */
+class Pushbutton : public PushbuttonBase
+{
+public:
+
+ /** Constructs a new instance of Pushbutton.
+ *
+ * @param pin The pin number of the pin. This is used as an argument to
+ * `pinMode` and `digitalRead`.
+ *
+ * @param pullUp Specifies whether the pin's internal pull-up resistor should
+ * be enabled. This should be either #PULL_UP_ENABLED (which is the default if
+ * the argument is omitted) or #PULL_UP_DISABLED.
+ *
+ * @param defaultState Specifies the voltage level that corresponds to the
+ * button's default (released) state. This should be either
+ * #DEFAULT_STATE_HIGH (which is the default if this argument is omitted) or
+ * #DEFAULT_STATE_LOW. */
+ Pushbutton(PinName pin, uint8_t pullUp = PULL_UP_ENABLED,
+ uint8_t defaultState = DEFAULT_STATE_HIGH);
+
+ virtual bool isPressed();
+
+private:
+
+ void init()
+ {
+ if (!initialized)
+ {
+ initialized = true;
+ init2();
+ }
+ }
+
+ void init2();
+
+ bool initialized;
+ PinName _pin;
+ bool _pullUp;
+ bool _defaultState;
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoBuzzerExample/ZumoBuzzer.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,29 @@
+// Copyright Pololu Corporation. For more information, see http://www.pololu.com/
+
+/*! \file ZumoBuzzer.h */
+
+#pragma once
+
+#include "PololuBuzzer.h"
+
+/*! \brief Plays beeps and music on the buzzer on the Zumo 32U4.
+ *
+ * This class uses Timer 4 and pin 6 (PD7/OC4D) to play beeps and melodies on
+ * the Zumo 32U4 buzzer.
+ *
+ * Note durations are timed using a timer overflow interrupt
+ * (`TIMER4_OVF`), which will briefly interrupt execution of your
+ * main program at the frequency of the sound being played. In most cases, the
+ * interrupt-handling routine is very short (several microseconds). However,
+ * when playing a sequence of notes in `PLAY_AUTOMATIC` mode (the default mode)
+ * with the `play()` command, this interrupt takes much longer than normal
+ * (perhaps several hundred microseconds) every time it starts a new note. It is
+ * important to take this into account when writing timing-critical code.
+ */
+class ZumoBuzzer : public PololuBuzzer
+{
+ // This is a trivial subclass of PololuBuzzer; it exists because
+ // we wanted the ZumoShield class names to be consistent and we
+ // didn't just use a typedef to define it because that would make
+ // the Doxygen documentation harder to understand.
+};
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoBuzzerExample/ZumoBuzzerExample.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,112 @@
+/*
+ * This example uses the ZumoBuzzer library to play a series of notes on
+ * the buzzer. It also uses Pushbutton library to allow the user to
+ * stop/reset the melody with the user pushbutton.
+ */
+
+//#include <Wire.h>
+#include "mbed.h"
+#include "ZumoShield.h"
+I2C i2c(I2C_SDA , I2C_SCL );
+
+#define MELODY_LENGTH 95
+
+// These arrays take up a total of 285 bytes of RAM (out of a limit of 1k (ATmega168), 2k (ATmega328), or 2.5k(ATmega32U4))
+unsigned char note[MELODY_LENGTH] =
+{
+ NOTE_E(5), SILENT_NOTE, NOTE_E(5), SILENT_NOTE, NOTE_E(5), SILENT_NOTE, NOTE_C(5), NOTE_E(5),
+ NOTE_G(5), SILENT_NOTE, NOTE_G(4), SILENT_NOTE,
+
+ NOTE_C(5), NOTE_G(4), SILENT_NOTE, NOTE_E(4), NOTE_A(4), NOTE_B(4), NOTE_B_FLAT(4), NOTE_A(4), NOTE_G(4),
+ NOTE_E(5), NOTE_G(5), NOTE_A(5), NOTE_F(5), NOTE_G(5), SILENT_NOTE, NOTE_E(5), NOTE_C(5), NOTE_D(5), NOTE_B(4),
+
+ NOTE_C(5), NOTE_G(4), SILENT_NOTE, NOTE_E(4), NOTE_A(4), NOTE_B(4), NOTE_B_FLAT(4), NOTE_A(4), NOTE_G(4),
+ NOTE_E(5), NOTE_G(5), NOTE_A(5), NOTE_F(5), NOTE_G(5), SILENT_NOTE, NOTE_E(5), NOTE_C(5), NOTE_D(5), NOTE_B(4),
+
+ SILENT_NOTE, NOTE_G(5), NOTE_F_SHARP(5), NOTE_F(5), NOTE_D_SHARP(5), NOTE_E(5), SILENT_NOTE,
+ NOTE_G_SHARP(4), NOTE_A(4), NOTE_C(5), SILENT_NOTE, NOTE_A(4), NOTE_C(5), NOTE_D(5),
+
+ SILENT_NOTE, NOTE_G(5), NOTE_F_SHARP(5), NOTE_F(5), NOTE_D_SHARP(5), NOTE_E(5), SILENT_NOTE,
+ NOTE_C(6), SILENT_NOTE, NOTE_C(6), SILENT_NOTE, NOTE_C(6),
+
+ SILENT_NOTE, NOTE_G(5), NOTE_F_SHARP(5), NOTE_F(5), NOTE_D_SHARP(5), NOTE_E(5), SILENT_NOTE,
+ NOTE_G_SHARP(4), NOTE_A(4), NOTE_C(5), SILENT_NOTE, NOTE_A(4), NOTE_C(5), NOTE_D(5),
+
+ SILENT_NOTE, NOTE_E_FLAT(5), SILENT_NOTE, NOTE_D(5), NOTE_C(5)
+};
+
+unsigned int duration[MELODY_LENGTH] =
+{
+ 100, 25, 125, 125, 125, 125, 125, 250, 250, 250, 250, 250,
+
+ 375, 125, 250, 375, 250, 250, 125, 250, 167, 167, 167, 250, 125, 125,
+ 125, 250, 125, 125, 375,
+
+ 375, 125, 250, 375, 250, 250, 125, 250, 167, 167, 167, 250, 125, 125,
+ 125, 250, 125, 125, 375,
+
+ 250, 125, 125, 125, 250, 125, 125, 125, 125, 125, 125, 125, 125, 125,
+
+ 250, 125, 125, 125, 250, 125, 125, 200, 50, 100, 25, 500,
+
+ 250, 125, 125, 125, 250, 125, 125, 125, 125, 125, 125, 125, 125, 125,
+
+ 250, 250, 125, 375, 500
+};
+
+PololuBuzzer buzzer(PTA1);
+Pushbutton button(ZUMO_BUTTON);
+BuzzerValues buzzing;
+unsigned char currentIdx;
+
+void setup() // run once, when the sketch starts
+{
+ currentIdx = 0;
+}
+
+void loop() // run over and over again
+{
+ // if we haven't finished playing the song and
+ // the buzzer is ready for the next note, play the next note
+ if (currentIdx < MELODY_LENGTH)
+ {
+ // play note at max volume
+ buzzing = buzzer.playNote(note[currentIdx], duration[currentIdx], 15);
+ if (buzzing.returnFrequency() == 1000){
+ wait_us(buzzing.returnDuration() * 1000);
+ }
+ else {
+ buzzer.beep(buzzing.returnFrequency(), buzzing.returnDuration(), buzzing.returnVolume());
+ buzzer.isPlaying2(1);
+ bool isItPlaying = true;
+ while (isItPlaying == true){
+ isItPlaying = buzzer.isPlaying2(2);
+ }
+ }
+ currentIdx++;
+ }
+
+ // Insert some other useful code here...
+ // the melody will play normally while the rest of your code executes
+ // as long as it executes quickly enough to keep from inserting delays
+ // between the notes.
+
+ // For example, let the user pushbutton function as a stop/reset melody button
+ if (button.isPressed())
+ {
+ // buzzer.stopPlaying(); // silence the buzzer
+ buzzer.nobeep();
+ if (currentIdx < MELODY_LENGTH)
+ currentIdx = MELODY_LENGTH; // terminate the melody
+ else
+ currentIdx = 0; // restart the melody
+ button.waitForRelease(); // wait here for the button to be released
+ }
+}
+
+int main() {
+ setup();
+ while(1){
+ loop();
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/ZumoBuzzerExample/ZumoShield.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,9 @@ +#pragma once + +// #include "LSM303.h" +// #include "L3G.h" +#include "Pushbutton.h" +// #include "QTRSensors.h" +#include "ZumoBuzzer.h" +// #include "ZumoMotors.h" +// #include "ZumoReflectanceSensorArray.h" \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoBuzzerExample/millis.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,38 @@
+#include "mbed.h"
+#include "millis.h"
+/*
+ millis.cpp
+ Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com>
+ All rights reserved.
+
+ This program is free software: you can redistribute it and/or modify
+ it under the terms of the GNU General Public License as published by
+ the Free Software Foundation, either version 3 of the License, or
+ (at your option) any later version.
+
+ This program is distributed in the hope that it will be useful,
+ but WITHOUT ANY WARRANTY; without even the implied warranty of
+ MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ GNU General Public License for more details.
+
+ You should have received a copy of the GNU General Public License
+ along with this program. If not, see <http://www.gnu.org/licenses/>.
+ */
+
+volatile unsigned long _millis;
+
+void millisStart(void) {
+ SysTick_Config(SystemCoreClock / 1000);
+}
+
+extern "C" {
+ void SysTicks_Handler(void) {
+ _millis++;
+ }
+}
+
+unsigned long millis(void) {
+ return _millis;
+}
+
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ZumoShield/ZumoBuzzerExample/millis.h Fri Oct 09 14:45:19 2020 +0000 @@ -0,0 +1,25 @@ +#ifndef MILLIS_H +#define MILLIS_H +/* + millis.h + Copyright (c) 2016 Zoltan Hudak <hudakz@inbox.com> + All rights reserved. + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see <http://www.gnu.org/licenses/>. + */ + +void millisStart(void); +unsigned long millis(void); + +#endif
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoMotorExample/ZumoMotorExample.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,106 @@
+/*
+ * This example uses the ZumoMotors library to drive each motor on the Zumo
+ * forward, then backward. The yellow user LED is on when a motor should be
+ * running forward and off when a motor should be running backward. If a
+ * motor on your Zumo has been flipped, you can correct its direction by
+ * uncommenting the call to flipLeftMotor() or flipRightMotor() in the setup()
+ * function.
+ */
+
+// #include <Wire.h>
+#include "ZumoMotors.h"
+#include "mbed.h"
+
+ZumoMotors motors(PTD0, PTC12, PTC4, PTC3);
+
+int main(){
+ while(1){
+
+ //turn right
+ for (float speed = 0; speed <= 400; speed++){
+ float correctspeed = (speed * 51 / 80)/255;
+ motors.motorL_fwd(correctspeed);
+ wait_us(2000);
+ // wait(1.0);
+ //motors.motorR_stop();
+ // motors.motorL_stop();
+ // wait(1.0);
+ }
+
+ for (float speed = 400; speed >= 0; speed--){
+ float correctspeed2 = (speed * 51 / 80)/255;
+ motors.motorL_fwd(correctspeed2);
+ wait_us(2000);
+ }
+
+ //go back
+ //motors.motorR_rev(0.4);
+ for (float speed = 0; speed <= 400; speed++){
+ float correctspeed3 = (speed * 51 / 80)/255;
+ motors.motorL_rev(correctspeed3);
+ wait_us(2000);
+ // wait(1.0);
+ // //motors.motorR_stop();
+ // motors.motorL_stop();
+ // wait(1.0);
+ }
+
+ for (float speed = 400; speed >= 0; speed--){
+ float correctspeed4 = (speed * 51 / 80)/255;
+ motors.motorL_rev(correctspeed4);
+ wait_us(2000);
+ }
+
+ //turn left
+ //motors.motorR_fwd(0.4);
+ for (float speed = 0; speed <= 400; speed++){
+ float correctspeed5 = (speed * 51 / 80)/255;
+ motors.motorR_fwd(correctspeed5);
+ wait_us(2000);
+ // wait(1.0);
+ // //motors.motorR_stop();
+ // motors.motorR_stop();
+ // wait(1.0);
+ }
+
+ for (float speed = 400; speed >= 0; speed--){
+ float correctspeed6 = (speed * 51 / 80)/255;
+ motors.motorR_fwd(correctspeed6);
+ wait_us(2000);
+ }
+
+ //go back
+ //motors.motorR_rev(0.4);
+ for (float speed = 0; speed <= 400; speed++){
+ float correctspeed7 = (speed * 51 / 80)/255;
+ motors.motorR_rev(correctspeed7);
+ wait_us(2000);
+ // wait(1.0);
+ // //motors.motorR_stop();
+ // motors.motorR_stop();
+ // wait(1.0);
+ }
+
+ for (float speed = 400; speed >= 0; speed--){
+ float correctspeed8 = (speed * 51 / 80)/255;
+ motors.motorR_rev(correctspeed8);
+ wait_us(2000);
+ }
+
+ //drive backward
+ // motors.motorR_fwd(0.8);
+ // motors.motorL_rev(0.8);
+ // wait(1.0);
+ // motors.motorR_stop();
+ // motors.motorL_stop();
+ // wait(1.0);
+
+ // //drive forward
+ // motors.motorR_rev(0.8);
+ // motors.motorL_fwd(0.8);
+ // wait(1.0);
+ // motors.motorR_stop();
+ // motors.motorL_stop();
+ // wait(1.0);
+ }
+}
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoMotorExample/ZumoMotors.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,84 @@
+/* File: DRV8835.cpp
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+
+#include "mbed.h"
+#include "ZumoMotors.h"
+
+
+ZumoMotors::ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin) :
+pwmL(pinPwmL),
+Lin(pinLin),
+pwmR(pinPwmR),
+Rin(pinRin)
+{
+ Lin = 0;
+ Rin = 0;
+ pwmL.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmL = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ pwmR.period(DRV8835_PWM_PERIOD_DEFAULT);
+ pwmR = DRV8835_PWM_PULSEWIDTH_DEFAULT;
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::stop()
+{
+ motorL_stop();
+ motorR_stop();
+}
+
+void ZumoMotors::motorL_stop(void)
+{
+ pwmL = 0;
+}
+
+void ZumoMotors::motorR_stop(void)
+{
+ pwmR = 0;
+}
+
+void ZumoMotors::setSpeeds(float Left,float Right)
+{
+ //Set Right Speed and Direction
+ if(Right<0)
+ {
+ motorR_rev(Right*-1);
+ } else {
+ motorR_fwd(Right);
+ }
+
+ //Set Left Speed and Direction
+ if(Left<0)
+ {
+ motorL_rev(Left*-1);
+ } else {
+ motorL_fwd(Left);
+ }
+}
+
+void ZumoMotors::motorL_fwd(float fPulsewidth)
+{
+ Lin = 0;
+ pwmL = fPulsewidth;
+}
+void ZumoMotors::motorL_rev(float fPulsewidth)
+{
+ Lin = 1;
+ pwmL = fPulsewidth;
+}
+
+void ZumoMotors::motorR_fwd(float fPulsewidth)
+{
+ Rin = 0;
+ pwmR = fPulsewidth;
+}
+void ZumoMotors::motorR_rev(float fPulsewidth)
+{
+ Rin = 1;
+ pwmR = fPulsewidth;
+}
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/ZumoMotorExample/ZumoMotors.h Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,38 @@
+/* File: DRV8835.h
+ * Adopted from work by Cameron Isbell
+ *
+ * Description: library for DRV8835 Motor Driver
+ * Assumptions: A is left and B is right
+ */
+#ifndef __DRV8835_H__
+#define __DRV8835_H__
+
+#include "mbed.h"
+
+#define DRV8835_PWM_PERIOD_DEFAULT (0.00005) // 2 ms period // 50KHz (0.00002)
+#define DRV8835_PWM_PULSEWIDTH_DEFAULT (0.50) // 50% duty cycle
+
+class ZumoMotors
+{
+public:
+ ZumoMotors( PinName pinPwmL, PinName pinLin,
+ PinName pinPwmR, PinName pinRin);
+
+ void motorL_stop(void);
+ void motorL_fwd(float fPulsewidth);
+ void motorL_rev(float fPulsewidth);
+ void motorR_stop(void);
+ void motorR_fwd(float fPulsewidth);
+ void motorR_rev(float fPulsewidth);
+ void setSpeeds(float Left,float Right);
+ void stop(void);
+
+private:
+ PwmOut pwmL;
+ DigitalOut Lin;
+ PwmOut pwmR;
+ DigitalOut Rin;
+};
+
+#endif /* __DRV8835_H__ */
+
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/ZumoShield/sdCard/main.cpp Fri Oct 09 14:45:19 2020 +0000
@@ -0,0 +1,86 @@
+/*
+Breadboard recorder program
+
+Copyright 2018 Mollusc Micros Ltd.
+
+Licensed under the Apache License, Version 2.0 (the "License");
+you may not use this file except in compliance with the License.
+You may obtain a copy of the License at
+
+ http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, software
+distributed under the License is distributed on an "AS IS" BASIS,
+WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+See the License for the specific language governing permissions and
+limitations under the License.
+
+*/
+
+#include "mbed.h"
+#include "SDBlockDevice.h"
+//#include "LittleFileSystem.h"
+#include "FATFileSystem.h"
+
+#define pin_mosi PTE3
+#define pin_miso PTE1
+#define pin_clk PTE2
+#define pin_cs PTE4
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+SDBlockDevice bd(pin_mosi, pin_miso, pin_clk, pin_cs);
+
+//LittleFileSystem fs("fs");
+FATFileSystem fs("fs");
+
+int main() {
+ fflush(stdout);
+ led1 = false;
+ led2 = true;
+ led3 = false;
+ led4 = false;
+
+ int err = fs.mount(&bd);
+ if (err) {
+ printf("Failed to mount filesystem (%d, %s), formatting...\n", err, strerror(-err));
+ fflush(stdout);
+ err = fs.reformat(&bd);
+ printf("err = %d, %s\n", err, strerror(-err));
+ fflush(stdout);
+ if (err > 0) {
+ return 1;
+ }
+ }
+ led1 = true;
+ printf("Mounted filesystem\n");
+ fflush(stdout);
+
+ FILE *f = fopen("/fs/test.txt", "w");
+ if (f) {
+ err = fprintf(f, "Hello, SD card!\n");
+ if (err > 0) {
+ led3 = true;
+ printf("Wrote %d bytes\n", err);
+ } else {
+ printf("Error writing = %d (%s)\n", err, strerror(-err));
+ fflush(stdout);
+ }
+ }
+ err = fclose(f);
+ printf("Error closing = %d (%s)\n", err, strerror(err));
+ fflush(stdout);
+
+ err = fs.unmount();
+ printf("Error unmounting = %d (%s)\n", err, strerror(err));
+ fflush(stdout);
+ led4 = true;
+
+ while (true) {
+ led1 = !led1;
+ wait_us(1000000);
+ }
+}
\ No newline at end of file