obstacle avoidance with sensors and servo installed
Dependencies: Adafruit_GFX GPS QEI Servo mbed
main.cpp
- Committer:
- anushachowdary16
- Date:
- 2014-10-02
- Revision:
- 0:21a2de59d16f
File content as of revision 0:21a2de59d16f:
#include "mbed.h" #include "Adafruit_SSD1306.h" #include "LSM303D.h" #include "QEI.h" #include "MSCFileSystem.h" #include "SDHCFileSystem.h" #include "PololuQik2.h" #include "GPS.h" #include "Servo.h" // an SPI sub-class that provides a constructed default format and frequency class SPI2 : public SPI { public: SPI2(PinName mosi, PinName miso, PinName clk) : SPI(mosi,miso,clk) { format(8,3); frequency(2000000); }; }; //Serial pc(USBTX,USBRX); SPI2 gSpi(p5,p6,p7); Adafruit_SSD1306 gOled(gSpi,p8,p11,p12); LSM303D comp(gSpi,p15); QEI Left(p30,p29,NC,48,QEI::X4_ENCODING); // encoder object for Left wheel QEI Right(p17,p16,NC,48,QEI::X4_ENCODING); // encoder object for Left wheel SDFileSystem sd(p5, p6, p7, p26, "sd"); // mosi, miso, sclk, cs PololuQik2 motors(p13,NC); Serial computer(p28, p27); // tx, rx read from bluetooth module GPS gpsAda(NC,p14,9600); AnalogIn IR(p18); Timer t; int main() { int pulses[2], check, mag[3], acc[3], i=0; float hdg; int distance; //Variable to store the distance calculated from the sensor int triggerDistance = 30; //The distance we want the robot to look for a new path int fDistance; //Variable to store the distance in front of the robot int lDistance; //Variable to store the distance on the left side of the robot int rDistance; //Variable to store the distance on the right side of the robot computer.baud(115200); motors.begin(); //display CoyleBot Logo gOled.display(); wait(1.0); //Initialization returns 1 if talking to right device check=comp.initialize(); //Set scales and offset for compass, could spin at start-up to get comp.setOffset(0, 0, 0); // example calibration comp.setScale(1.00, 1.00, 1.00); // example calibration t.reset(); t.start(); while(1) { //Compass, Accelerometer Data mag[0]=comp.magnitometer(1000); mag[1]=comp.magnitometer(1000); mag[2]=comp.magnitometer(1000); acc[0]=comp.accelerometer(1000); acc[1]=comp.accelerometer(1000); acc[2]=comp.accelerometer(1000); hdg=comp.heading(); //Get Wheel Encoder Pulses pulses[0]=Left.getPulses(); pulses[1]=Right.getPulses(); //Clear screen information so it can be written to gOled.clearDisplay(); //Go to top of screen gOled.setCursor(20,20); void moveForward(); { //Print RC Commands to SCREEN gOled.printf("Forward :"); 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); gOled.display(); //sets the speed of the motor 0, must be a value between -255 and 255 motors.setMotor0Speed(100); //sets the speed of the motor 0, must be a value between -255 and 255 motors.setMotor1Speed(100); } void moveBackward(); { //Print RC Commands to SCREEN gOled.printf("Backward :"); 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); gOled.display(); //sets the speed of the motor 0, must be a value between -255 and 255 motors.setMotor0Speed(-100); //sets the speed of the motor 0, must be a value between -255 and 255 motors.setMotor1Speed(-100); } int scan(); //Get the distance retrieved fDistance = distance; //Set that distance to the front distance if(fDistance < triggerDistance){ //If there is something closer than 30cm in front of us moveBackward(); //Move Backward for a second wait(1000); void moveRight(); //Turn Right for half a second wait(500); void moveStop(); //Stop scan(); //Take a reading rDistance = distance; //Store that to the distance on the right side void moveLeft(); wait(1000); //Turn left for a second moveStop(); //Stop scan(); //Take a reading lDistance = distance; //Store that to the distance on the left side if(lDistance < rDistance){ //If the distance on the left is smaller than that of the right moveRight(); //Move right for a second wait(1000); moveForward(); //Then move forward } else { moveForward(); //If the left side is larger than the right side move forward } } else { moveForward(); //If there is nothing infront of the robot move forward } } int scan() { time = sonar.ping(); //Send out a ping and store the time it took for it to come back in the time variable distance = time / US_ROUNDTRIP_CM; //Convert that time into a distance if(distance == 0){ //If no ping was recieved distance = 100; //Set the distance to max } delay(10); void moveBackward() { rightServo.write(180); leftServo.write(0); } void moveForward() { rightServo.write(0); leftServo.write(180); } void moveRight() { rightServo.write(0); leftServo.write(0); } void moveLeft() { rightServo.write(180); leftServo.write(180); } void moveStop() { rightServo.write(90); leftServo.write(95); } i++; } }