Updated
Dependencies: Adafruit_GFX GPS QEI mbed
main.cpp@0:d200f455dbd4, 2014-11-18 (annotated)
- Committer:
- canterol
- Date:
- Tue Nov 18 03:04:58 2014 +0000
- Revision:
- 0:d200f455dbd4
Updated
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
canterol | 0:d200f455dbd4 | 1 | /* |
canterol | 0:d200f455dbd4 | 2 | ME503 Unmanned Autonomous Vehicle Systems |
canterol | 0:d200f455dbd4 | 3 | Dr. E. Coyle |
canterol | 0:d200f455dbd4 | 4 | Wall Following Assignment |
canterol | 0:d200f455dbd4 | 5 | L. Cantero, B. Wallace |
canterol | 0:d200f455dbd4 | 6 | */ |
canterol | 0:d200f455dbd4 | 7 | |
canterol | 0:d200f455dbd4 | 8 | #include "mbed.h" |
canterol | 0:d200f455dbd4 | 9 | #include "Adafruit_SSD1306.h" |
canterol | 0:d200f455dbd4 | 10 | #include "LSM303D.h" |
canterol | 0:d200f455dbd4 | 11 | #include "QEI.h" |
canterol | 0:d200f455dbd4 | 12 | #include "MSCFileSystem.h" |
canterol | 0:d200f455dbd4 | 13 | #include "SDHCFileSystem.h" |
canterol | 0:d200f455dbd4 | 14 | #include "GPS.h" |
canterol | 0:d200f455dbd4 | 15 | #include "PololuQik2.h" |
canterol | 0:d200f455dbd4 | 16 | #include <math.h> |
canterol | 0:d200f455dbd4 | 17 | |
canterol | 0:d200f455dbd4 | 18 | // an SPI sub-class that provides a constructed default format and frequency |
canterol | 0:d200f455dbd4 | 19 | class SPI2 : public SPI |
canterol | 0:d200f455dbd4 | 20 | { |
canterol | 0:d200f455dbd4 | 21 | public: |
canterol | 0:d200f455dbd4 | 22 | SPI2(PinName mosi, PinName miso, PinName clk) : SPI(mosi,miso,clk) |
canterol | 0:d200f455dbd4 | 23 | { |
canterol | 0:d200f455dbd4 | 24 | format(8,3); |
canterol | 0:d200f455dbd4 | 25 | frequency(2000000); |
canterol | 0:d200f455dbd4 | 26 | }; |
canterol | 0:d200f455dbd4 | 27 | }; |
canterol | 0:d200f455dbd4 | 28 | |
canterol | 0:d200f455dbd4 | 29 | Serial pc(USBTX,USBRX); |
canterol | 0:d200f455dbd4 | 30 | SPI2 gSpi(p5,p6,p7); |
canterol | 0:d200f455dbd4 | 31 | Adafruit_SSD1306 gOled(gSpi,p8,p11,p12); |
canterol | 0:d200f455dbd4 | 32 | LSM303D comp(gSpi,p15); |
canterol | 0:d200f455dbd4 | 33 | QEI Left(p30,p29,NC,48,QEI::X4_ENCODING); // encoder object for Left wheel |
canterol | 0:d200f455dbd4 | 34 | QEI Right(p17,p16,NC,48,QEI::X4_ENCODING); // encoder object for Left wheel |
canterol | 0:d200f455dbd4 | 35 | SDFileSystem sd(p5, p6, p7, p26, "sd"); // mosi, miso, sclk, cs |
canterol | 0:d200f455dbd4 | 36 | PololuQik2 motors(p13,NC); |
canterol | 0:d200f455dbd4 | 37 | Serial computer(p28, p27); // tx, rx read from bluetooth module |
canterol | 0:d200f455dbd4 | 38 | //GPS gpsAda(NC,p14,9600); |
canterol | 0:d200f455dbd4 | 39 | |
canterol | 0:d200f455dbd4 | 40 | // Function prototypes |
canterol | 0:d200f455dbd4 | 41 | void splash(); |
canterol | 0:d200f455dbd4 | 42 | void move(int speed0, int speed1); |
canterol | 0:d200f455dbd4 | 43 | |
canterol | 0:d200f455dbd4 | 44 | int main() |
canterol | 0:d200f455dbd4 | 45 | { |
canterol | 0:d200f455dbd4 | 46 | computer.baud(230400); |
canterol | 0:d200f455dbd4 | 47 | int i=0, speed0 = 60, speed1 = 60, read; |
canterol | 0:d200f455dbd4 | 48 | double percent0 = 1.00, percent1 = 1.00; |
canterol | 0:d200f455dbd4 | 49 | |
canterol | 0:d200f455dbd4 | 50 | motors.begin(); // Initiate motor control |
canterol | 0:d200f455dbd4 | 51 | splash(); // Display splash screen |
canterol | 0:d200f455dbd4 | 52 | |
canterol | 0:d200f455dbd4 | 53 | move(speed0, speed1); |
canterol | 0:d200f455dbd4 | 54 | |
canterol | 0:d200f455dbd4 | 55 | while(1) |
canterol | 0:d200f455dbd4 | 56 | { |
canterol | 0:d200f455dbd4 | 57 | if(computer.readable()) |
canterol | 0:d200f455dbd4 | 58 | { |
canterol | 0:d200f455dbd4 | 59 | read=computer.getc(); |
canterol | 0:d200f455dbd4 | 60 | |
canterol | 0:d200f455dbd4 | 61 | if(read != 255) |
canterol | 0:d200f455dbd4 | 62 | { |
canterol | 0:d200f455dbd4 | 63 | |
canterol | 0:d200f455dbd4 | 64 | percent0 = (read / 100.00); |
canterol | 0:d200f455dbd4 | 65 | percent1 = (2 - percent0); |
canterol | 0:d200f455dbd4 | 66 | |
canterol | 0:d200f455dbd4 | 67 | move(floor(percent0*speed0), floor(percent1*speed1)); |
canterol | 0:d200f455dbd4 | 68 | } |
canterol | 0:d200f455dbd4 | 69 | |
canterol | 0:d200f455dbd4 | 70 | else |
canterol | 0:d200f455dbd4 | 71 | { |
canterol | 0:d200f455dbd4 | 72 | move(0,0); |
canterol | 0:d200f455dbd4 | 73 | } |
canterol | 0:d200f455dbd4 | 74 | } |
canterol | 0:d200f455dbd4 | 75 | |
canterol | 0:d200f455dbd4 | 76 | gOled.clearDisplay(); |
canterol | 0:d200f455dbd4 | 77 | gOled.setCursor(0,0); |
canterol | 0:d200f455dbd4 | 78 | gOled.printf("Left Motor: %.2f%%\nRight Motor: %.2f%%\nTime: %d",percent0*100, percent1*100,i); |
canterol | 0:d200f455dbd4 | 79 | gOled.display(); |
canterol | 0:d200f455dbd4 | 80 | i++; |
canterol | 0:d200f455dbd4 | 81 | } |
canterol | 0:d200f455dbd4 | 82 | } |
canterol | 0:d200f455dbd4 | 83 | |
canterol | 0:d200f455dbd4 | 84 | |
canterol | 0:d200f455dbd4 | 85 | // FUNCTIONS______________________________________________________ |
canterol | 0:d200f455dbd4 | 86 | |
canterol | 0:d200f455dbd4 | 87 | void splash() // Displays splash screen for 3 seconds, then clears buffer and resets cursor |
canterol | 0:d200f455dbd4 | 88 | { |
canterol | 0:d200f455dbd4 | 89 | gOled.display(); |
canterol | 0:d200f455dbd4 | 90 | wait(3.0); |
canterol | 0:d200f455dbd4 | 91 | gOled.clearDisplay(); |
canterol | 0:d200f455dbd4 | 92 | gOled.setCursor(0,0); // Splash screen stays until next display call |
canterol | 0:d200f455dbd4 | 93 | } |
canterol | 0:d200f455dbd4 | 94 | |
canterol | 0:d200f455dbd4 | 95 | void move(int speed0, int speed1) // Move robot using defined wheel speeds (corrects for wiring-polarity issue and mirroring of right motor) |
canterol | 0:d200f455dbd4 | 96 | { |
canterol | 0:d200f455dbd4 | 97 | motors.setMotor0Speed(speed0); |
canterol | 0:d200f455dbd4 | 98 | motors.setMotor1Speed(-speed1); |
canterol | 0:d200f455dbd4 | 99 | } |