obstacle avoidance with sensors and servo installed
Dependencies: Adafruit_GFX GPS QEI Servo mbed
main.cpp@0:21a2de59d16f, 2014-10-02 (annotated)
- Committer:
- anushachowdary16
- Date:
- Thu Oct 02 06:41:36 2014 +0000
- Revision:
- 0:21a2de59d16f
need to revise
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
anushachowdary16 | 0:21a2de59d16f | 1 | #include "mbed.h" |
anushachowdary16 | 0:21a2de59d16f | 2 | #include "Adafruit_SSD1306.h" |
anushachowdary16 | 0:21a2de59d16f | 3 | #include "LSM303D.h" |
anushachowdary16 | 0:21a2de59d16f | 4 | #include "QEI.h" |
anushachowdary16 | 0:21a2de59d16f | 5 | #include "MSCFileSystem.h" |
anushachowdary16 | 0:21a2de59d16f | 6 | #include "SDHCFileSystem.h" |
anushachowdary16 | 0:21a2de59d16f | 7 | #include "PololuQik2.h" |
anushachowdary16 | 0:21a2de59d16f | 8 | #include "GPS.h" |
anushachowdary16 | 0:21a2de59d16f | 9 | #include "Servo.h" |
anushachowdary16 | 0:21a2de59d16f | 10 | |
anushachowdary16 | 0:21a2de59d16f | 11 | // an SPI sub-class that provides a constructed default format and frequency |
anushachowdary16 | 0:21a2de59d16f | 12 | class SPI2 : public SPI |
anushachowdary16 | 0:21a2de59d16f | 13 | { |
anushachowdary16 | 0:21a2de59d16f | 14 | public: |
anushachowdary16 | 0:21a2de59d16f | 15 | SPI2(PinName mosi, PinName miso, PinName clk) : SPI(mosi,miso,clk) |
anushachowdary16 | 0:21a2de59d16f | 16 | { |
anushachowdary16 | 0:21a2de59d16f | 17 | format(8,3); |
anushachowdary16 | 0:21a2de59d16f | 18 | frequency(2000000); |
anushachowdary16 | 0:21a2de59d16f | 19 | }; |
anushachowdary16 | 0:21a2de59d16f | 20 | }; |
anushachowdary16 | 0:21a2de59d16f | 21 | |
anushachowdary16 | 0:21a2de59d16f | 22 | //Serial pc(USBTX,USBRX); |
anushachowdary16 | 0:21a2de59d16f | 23 | SPI2 gSpi(p5,p6,p7); |
anushachowdary16 | 0:21a2de59d16f | 24 | Adafruit_SSD1306 gOled(gSpi,p8,p11,p12); |
anushachowdary16 | 0:21a2de59d16f | 25 | LSM303D comp(gSpi,p15); |
anushachowdary16 | 0:21a2de59d16f | 26 | QEI Left(p30,p29,NC,48,QEI::X4_ENCODING); // encoder object for Left wheel |
anushachowdary16 | 0:21a2de59d16f | 27 | QEI Right(p17,p16,NC,48,QEI::X4_ENCODING); // encoder object for Left wheel |
anushachowdary16 | 0:21a2de59d16f | 28 | SDFileSystem sd(p5, p6, p7, p26, "sd"); // mosi, miso, sclk, cs |
anushachowdary16 | 0:21a2de59d16f | 29 | PololuQik2 motors(p13,NC); |
anushachowdary16 | 0:21a2de59d16f | 30 | Serial computer(p28, p27); // tx, rx read from bluetooth module |
anushachowdary16 | 0:21a2de59d16f | 31 | GPS gpsAda(NC,p14,9600); |
anushachowdary16 | 0:21a2de59d16f | 32 | AnalogIn IR(p18); |
anushachowdary16 | 0:21a2de59d16f | 33 | |
anushachowdary16 | 0:21a2de59d16f | 34 | Timer t; |
anushachowdary16 | 0:21a2de59d16f | 35 | |
anushachowdary16 | 0:21a2de59d16f | 36 | int main() |
anushachowdary16 | 0:21a2de59d16f | 37 | { |
anushachowdary16 | 0:21a2de59d16f | 38 | int pulses[2], check, mag[3], acc[3], i=0; |
anushachowdary16 | 0:21a2de59d16f | 39 | float hdg; |
anushachowdary16 | 0:21a2de59d16f | 40 | int distance; //Variable to store the distance calculated from the sensor |
anushachowdary16 | 0:21a2de59d16f | 41 | int triggerDistance = 30; //The distance we want the robot to look for a new path |
anushachowdary16 | 0:21a2de59d16f | 42 | int fDistance; //Variable to store the distance in front of the robot |
anushachowdary16 | 0:21a2de59d16f | 43 | int lDistance; //Variable to store the distance on the left side of the robot |
anushachowdary16 | 0:21a2de59d16f | 44 | int rDistance; //Variable to store the distance on the right side of the robot |
anushachowdary16 | 0:21a2de59d16f | 45 | |
anushachowdary16 | 0:21a2de59d16f | 46 | computer.baud(115200); |
anushachowdary16 | 0:21a2de59d16f | 47 | motors.begin(); |
anushachowdary16 | 0:21a2de59d16f | 48 | |
anushachowdary16 | 0:21a2de59d16f | 49 | //display CoyleBot Logo |
anushachowdary16 | 0:21a2de59d16f | 50 | gOled.display(); |
anushachowdary16 | 0:21a2de59d16f | 51 | wait(1.0); |
anushachowdary16 | 0:21a2de59d16f | 52 | |
anushachowdary16 | 0:21a2de59d16f | 53 | //Initialization returns 1 if talking to right device |
anushachowdary16 | 0:21a2de59d16f | 54 | check=comp.initialize(); |
anushachowdary16 | 0:21a2de59d16f | 55 | |
anushachowdary16 | 0:21a2de59d16f | 56 | //Set scales and offset for compass, could spin at start-up to get |
anushachowdary16 | 0:21a2de59d16f | 57 | comp.setOffset(0, 0, 0); // example calibration |
anushachowdary16 | 0:21a2de59d16f | 58 | comp.setScale(1.00, 1.00, 1.00); // example calibration |
anushachowdary16 | 0:21a2de59d16f | 59 | |
anushachowdary16 | 0:21a2de59d16f | 60 | t.reset(); |
anushachowdary16 | 0:21a2de59d16f | 61 | t.start(); |
anushachowdary16 | 0:21a2de59d16f | 62 | |
anushachowdary16 | 0:21a2de59d16f | 63 | while(1) |
anushachowdary16 | 0:21a2de59d16f | 64 | { |
anushachowdary16 | 0:21a2de59d16f | 65 | //Compass, Accelerometer Data |
anushachowdary16 | 0:21a2de59d16f | 66 | mag[0]=comp.magnitometer(1000); |
anushachowdary16 | 0:21a2de59d16f | 67 | mag[1]=comp.magnitometer(1000); |
anushachowdary16 | 0:21a2de59d16f | 68 | mag[2]=comp.magnitometer(1000); |
anushachowdary16 | 0:21a2de59d16f | 69 | acc[0]=comp.accelerometer(1000); |
anushachowdary16 | 0:21a2de59d16f | 70 | acc[1]=comp.accelerometer(1000); |
anushachowdary16 | 0:21a2de59d16f | 71 | acc[2]=comp.accelerometer(1000); |
anushachowdary16 | 0:21a2de59d16f | 72 | hdg=comp.heading(); |
anushachowdary16 | 0:21a2de59d16f | 73 | |
anushachowdary16 | 0:21a2de59d16f | 74 | //Get Wheel Encoder Pulses |
anushachowdary16 | 0:21a2de59d16f | 75 | pulses[0]=Left.getPulses(); |
anushachowdary16 | 0:21a2de59d16f | 76 | pulses[1]=Right.getPulses(); |
anushachowdary16 | 0:21a2de59d16f | 77 | |
anushachowdary16 | 0:21a2de59d16f | 78 | //Clear screen information so it can be written to |
anushachowdary16 | 0:21a2de59d16f | 79 | gOled.clearDisplay(); |
anushachowdary16 | 0:21a2de59d16f | 80 | |
anushachowdary16 | 0:21a2de59d16f | 81 | //Go to top of screen |
anushachowdary16 | 0:21a2de59d16f | 82 | gOled.setCursor(20,20); |
anushachowdary16 | 0:21a2de59d16f | 83 | void moveForward(); |
anushachowdary16 | 0:21a2de59d16f | 84 | { |
anushachowdary16 | 0:21a2de59d16f | 85 | //Print RC Commands to SCREEN |
anushachowdary16 | 0:21a2de59d16f | 86 | gOled.printf("Forward :"); |
anushachowdary16 | 0:21a2de59d16f | 87 | gOled.printf("%d %d %d %d %d %d %d %d %d %d %f %f \r\n", i, t.read_ms(),mag[0],mag[1],mag[2],acc[0],acc[1],acc[2],pulses[0],pulses[1],gpsAda.latitude,gpsAda.longitude); |
anushachowdary16 | 0:21a2de59d16f | 88 | gOled.display(); |
anushachowdary16 | 0:21a2de59d16f | 89 | //sets the speed of the motor 0, must be a value between -255 and 255 |
anushachowdary16 | 0:21a2de59d16f | 90 | motors.setMotor0Speed(100); |
anushachowdary16 | 0:21a2de59d16f | 91 | //sets the speed of the motor 0, must be a value between -255 and 255 |
anushachowdary16 | 0:21a2de59d16f | 92 | motors.setMotor1Speed(100); |
anushachowdary16 | 0:21a2de59d16f | 93 | } |
anushachowdary16 | 0:21a2de59d16f | 94 | void moveBackward(); |
anushachowdary16 | 0:21a2de59d16f | 95 | { |
anushachowdary16 | 0:21a2de59d16f | 96 | //Print RC Commands to SCREEN |
anushachowdary16 | 0:21a2de59d16f | 97 | gOled.printf("Backward :"); |
anushachowdary16 | 0:21a2de59d16f | 98 | gOled.printf("%d %d %d %d %d %d %d %d %d %d %f %f \r\n", i, t.read_ms(),mag[0],mag[1],mag[2],acc[0],acc[1],acc[2],pulses[0],pulses[1],gpsAda.latitude,gpsAda.longitude); |
anushachowdary16 | 0:21a2de59d16f | 99 | gOled.display(); |
anushachowdary16 | 0:21a2de59d16f | 100 | //sets the speed of the motor 0, must be a value between -255 and 255 |
anushachowdary16 | 0:21a2de59d16f | 101 | motors.setMotor0Speed(-100); |
anushachowdary16 | 0:21a2de59d16f | 102 | //sets the speed of the motor 0, must be a value between -255 and 255 |
anushachowdary16 | 0:21a2de59d16f | 103 | motors.setMotor1Speed(-100); |
anushachowdary16 | 0:21a2de59d16f | 104 | } |
anushachowdary16 | 0:21a2de59d16f | 105 | |
anushachowdary16 | 0:21a2de59d16f | 106 | int scan(); //Get the distance retrieved |
anushachowdary16 | 0:21a2de59d16f | 107 | fDistance = distance; //Set that distance to the front distance |
anushachowdary16 | 0:21a2de59d16f | 108 | if(fDistance < triggerDistance){ //If there is something closer than 30cm in front of us |
anushachowdary16 | 0:21a2de59d16f | 109 | moveBackward(); //Move Backward for a second |
anushachowdary16 | 0:21a2de59d16f | 110 | wait(1000); |
anushachowdary16 | 0:21a2de59d16f | 111 | void moveRight(); //Turn Right for half a second |
anushachowdary16 | 0:21a2de59d16f | 112 | wait(500); |
anushachowdary16 | 0:21a2de59d16f | 113 | void moveStop(); //Stop |
anushachowdary16 | 0:21a2de59d16f | 114 | scan(); //Take a reading |
anushachowdary16 | 0:21a2de59d16f | 115 | rDistance = distance; //Store that to the distance on the right side |
anushachowdary16 | 0:21a2de59d16f | 116 | void moveLeft(); |
anushachowdary16 | 0:21a2de59d16f | 117 | wait(1000); //Turn left for a second |
anushachowdary16 | 0:21a2de59d16f | 118 | moveStop(); //Stop |
anushachowdary16 | 0:21a2de59d16f | 119 | scan(); //Take a reading |
anushachowdary16 | 0:21a2de59d16f | 120 | lDistance = distance; //Store that to the distance on the left side |
anushachowdary16 | 0:21a2de59d16f | 121 | if(lDistance < rDistance){ //If the distance on the left is smaller than that of the right |
anushachowdary16 | 0:21a2de59d16f | 122 | moveRight(); //Move right for a second |
anushachowdary16 | 0:21a2de59d16f | 123 | wait(1000); |
anushachowdary16 | 0:21a2de59d16f | 124 | moveForward(); //Then move forward |
anushachowdary16 | 0:21a2de59d16f | 125 | } |
anushachowdary16 | 0:21a2de59d16f | 126 | else |
anushachowdary16 | 0:21a2de59d16f | 127 | { |
anushachowdary16 | 0:21a2de59d16f | 128 | moveForward(); //If the left side is larger than the right side move forward |
anushachowdary16 | 0:21a2de59d16f | 129 | } |
anushachowdary16 | 0:21a2de59d16f | 130 | } |
anushachowdary16 | 0:21a2de59d16f | 131 | else |
anushachowdary16 | 0:21a2de59d16f | 132 | { |
anushachowdary16 | 0:21a2de59d16f | 133 | moveForward(); //If there is nothing infront of the robot move forward |
anushachowdary16 | 0:21a2de59d16f | 134 | } |
anushachowdary16 | 0:21a2de59d16f | 135 | } |
anushachowdary16 | 0:21a2de59d16f | 136 | |
anushachowdary16 | 0:21a2de59d16f | 137 | int scan() |
anushachowdary16 | 0:21a2de59d16f | 138 | { |
anushachowdary16 | 0:21a2de59d16f | 139 | time = sonar.ping(); //Send out a ping and store the time it took for it to come back in the time variable |
anushachowdary16 | 0:21a2de59d16f | 140 | distance = time / US_ROUNDTRIP_CM; //Convert that time into a distance |
anushachowdary16 | 0:21a2de59d16f | 141 | if(distance == 0){ //If no ping was recieved |
anushachowdary16 | 0:21a2de59d16f | 142 | distance = 100; //Set the distance to max |
anushachowdary16 | 0:21a2de59d16f | 143 | } |
anushachowdary16 | 0:21a2de59d16f | 144 | delay(10); |
anushachowdary16 | 0:21a2de59d16f | 145 | |
anushachowdary16 | 0:21a2de59d16f | 146 | |
anushachowdary16 | 0:21a2de59d16f | 147 | void moveBackward() |
anushachowdary16 | 0:21a2de59d16f | 148 | { |
anushachowdary16 | 0:21a2de59d16f | 149 | rightServo.write(180); |
anushachowdary16 | 0:21a2de59d16f | 150 | leftServo.write(0); |
anushachowdary16 | 0:21a2de59d16f | 151 | } |
anushachowdary16 | 0:21a2de59d16f | 152 | |
anushachowdary16 | 0:21a2de59d16f | 153 | void moveForward() |
anushachowdary16 | 0:21a2de59d16f | 154 | { |
anushachowdary16 | 0:21a2de59d16f | 155 | rightServo.write(0); |
anushachowdary16 | 0:21a2de59d16f | 156 | leftServo.write(180); |
anushachowdary16 | 0:21a2de59d16f | 157 | } |
anushachowdary16 | 0:21a2de59d16f | 158 | |
anushachowdary16 | 0:21a2de59d16f | 159 | void moveRight() |
anushachowdary16 | 0:21a2de59d16f | 160 | { |
anushachowdary16 | 0:21a2de59d16f | 161 | rightServo.write(0); |
anushachowdary16 | 0:21a2de59d16f | 162 | leftServo.write(0); |
anushachowdary16 | 0:21a2de59d16f | 163 | } |
anushachowdary16 | 0:21a2de59d16f | 164 | |
anushachowdary16 | 0:21a2de59d16f | 165 | void moveLeft() |
anushachowdary16 | 0:21a2de59d16f | 166 | { |
anushachowdary16 | 0:21a2de59d16f | 167 | rightServo.write(180); |
anushachowdary16 | 0:21a2de59d16f | 168 | leftServo.write(180); |
anushachowdary16 | 0:21a2de59d16f | 169 | } |
anushachowdary16 | 0:21a2de59d16f | 170 | |
anushachowdary16 | 0:21a2de59d16f | 171 | void moveStop() |
anushachowdary16 | 0:21a2de59d16f | 172 | { |
anushachowdary16 | 0:21a2de59d16f | 173 | rightServo.write(90); |
anushachowdary16 | 0:21a2de59d16f | 174 | leftServo.write(95); |
anushachowdary16 | 0:21a2de59d16f | 175 | } |
anushachowdary16 | 0:21a2de59d16f | 176 | |
anushachowdary16 | 0:21a2de59d16f | 177 | |
anushachowdary16 | 0:21a2de59d16f | 178 | |
anushachowdary16 | 0:21a2de59d16f | 179 | i++; |
anushachowdary16 | 0:21a2de59d16f | 180 | |
anushachowdary16 | 0:21a2de59d16f | 181 | |
anushachowdary16 | 0:21a2de59d16f | 182 | } |
anushachowdary16 | 0:21a2de59d16f | 183 | } |