Diff: main.cpp

Revision:

0:21a2de59d16f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Oct 02 06:41:36 2014 +0000
@@ -0,0 +1,183 @@
+#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++;
+        
+        
+    }
+}