Diff: main.cpp

Revision:

0:b62ee1c95bd3

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Apr 01 20:29:35 2011 +0000
@@ -0,0 +1,97 @@
+#include "mbed.h"
+#include "SRF08.h"
+#include "TextLCD.h"
+
+//This program uses the Magnevation Dual Driver board that I originally
+//had for the OOPIC. I have the board driving HN-GH12-2217Y geared motors
+//type HSIANG NENG.
+
+SRF08 srf08(p9, p10, 0xE0);  // Define SDA, SCL pin and I2C address
+
+PwmOut MotorRightWheel(p23); // pwm
+PwmOut MotorLeftWheel(p25);  // pwm
+
+DigitalOut DirectionR=p27;//Sets the direction pin on the Magnevation
+DigitalOut DirectionL=p29;
+DigitalOut BrakeR=p28;//Releases the Brake pin on the Magnevation
+DigitalOut BrakeL=p30;
+
+TextLCD lcd(p15, p16, p17, p18, p19, p20);
+
+float RangeReading() {
+    float value;
+    value = srf08.read();
+    return (value);
+}
+
+float DriveForwardFullSpeed() {
+    float s;
+    DirectionR=0;//Set the direction Right motor
+    DirectionL=1;//Set the direction Left motor
+    BrakeR = BrakeL = 1;//Take Brake OFF
+    for (s= 0.0; s < 1.0 ; s += 0.01) {
+        MotorRightWheel=s;
+        MotorLeftWheel=s;
+        wait(0.2);
+    }
+    return (s);
+}
+
+float DriveReverse() {
+    float s =0;
+    DirectionR=1;//Set the direction Right motor
+    DirectionL=0;//Set the direction Left motor
+    BrakeR = BrakeL = 1; //Take Brake OFF
+    for (float s= 0.0; s < 1.0 ; s += 0.01) {
+        MotorRightWheel=s;
+        MotorLeftWheel=s;
+        wait(0.2);
+    }
+    wait(0.2);// Slow down and stop
+    for (float s = 1.0; s > 0.0; s -= 0.01) {
+        MotorRightWheel=s;
+        MotorLeftWheel=s;
+        wait(0.2);
+    }
+    BrakeR = BrakeL = 0; //Put Brake ON
+    return (s);
+}
+
+float ZeroSpeedForward() {
+    float s =0;
+    DirectionR=0;//Set the direction Right motor
+    DirectionL=1;//Set the direction Left motor
+    for (float s = 1.0; s > 0.0; s -= 0.01) {
+        MotorRightWheel=s;
+        MotorLeftWheel=s;
+        wait(0.2);
+    }
+    BrakeR = BrakeL = 0; //Put Brake ON
+    return (s);
+}
+
+int main() {
+    float distance, speed = 0;
+    while (1) {
+        lcd.cls();
+        distance = RangeReading();
+        lcd.printf("Range: %2.f cm\n",distance);
+        wait(0.2);
+        if (distance >100  && speed <0.1) {
+            lcd.printf("Forward \n");
+            speed = DriveForwardFullSpeed();
+        }
+        distance = RangeReading();
+        lcd.printf("Range: %2.f cm\n",distance);
+        if  (distance < 100 && speed >0.5) {
+            speed = ZeroSpeedForward();
+        }
+        distance = RangeReading();
+        if (distance > 0.0 && speed == 0.0) {
+            lcd.printf("Reverse \n");
+            speed = DriveReverse();
+        }
+        wait (0.5);
+    }
+}
+