anusha tottempudi / Mbed 2 deprecated servo_sensor

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?

UserRevisionLine numberNew 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 }