Diff: main.cpp

Revision:

0:8fe745012832

diff -r 000000000000 -r 8fe745012832 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Dec 01 21:34:07 2010 +0000
@@ -0,0 +1,137 @@
+#include <mbed.h>
+#include <motordriver.h>
+#include <Servo.h>
+#include <GP2xx.h>
+//pc interface
+Serial pc(USBTX, USBRX); // tx, rx
+//servos
+Servo LeftServo(p24);
+Servo RightServo(p23);
+//motors, left and right side
+Motor leftM(p22, p6, p5, 1); // pwm, fwd, rev, can break
+Motor rightM(p21, p7, p8, 1); // pwm, fwd, rev, can break
+//range finders, left and right side, and left and right front
+IRRangeFinder LS(p18,1);
+IRRangeFinder LF(p17,1);
+IRRangeFinder RF(p16,1);
+IRRangeFinder RS(p15,1);
+//debug leds.
+DigitalOut led1 (LED1);
+DigitalOut led2 (LED2);
+DigitalOut led3 (LED3);
+DigitalOut led4 (LED4);
+DigitalOut ledleft  (p14);
+DigitalOut ledright (p13);
+DigitalOut ledfront (p12);
+/** simple object avoidance using a logo based set of functions. it works and provides some intresting ways
+* to control behaviour.
+*
+*/
+
+
+/** function to spin the robot on the spot. technicaly redundant, as turn can do the same, but clearer when coding
+* @param time, how long to spin for. 
+* @param speed how fast to run the motors.
+*
+*/
+float spin (float time, float speed);//-1 to 1
+/** function to make the robot go fowards in a straight line
+* @param time, how long to go straight for. 
+* @param speed how fast to run the motors. -1 is reverse, 1 is foward, 0 is technicaly possible but means the robot coasts. 
+*
+*/
+float straight (float time, float speed);//0 to 1
+/** function to make the robot turn
+* @param time, how long to turn for. 
+* @param speed how fast to run the motors. -1 is right, 1 is left, 0 is technicaly possible but means the robot coasts. 
+* @param diffrence, from 1 to -1. how much slower the opposite side is -1 is same speed and direction, 1 is same speed opposite direction
+*/
+float turn (float time, float speed, float diffrence);
+/** function to make the robot stop
+* @param time, how long to stop for 
+* @param duty how hard to stop the motors.  1 is full on, 0 is technicaly possible but means the robot coasts. 
+*
+*/
+float stop (float time, float duty);
+
+int main() {
+    float tempLF = 0, tempRF = 0;
+    while (1) {
+        tempLF = LS.read();//cuts down function calls, and therefore clock cycles used
+        tempRF = RS.read();
+
+        if (tempLF < 12) {
+            if (tempLF > tempRF) {
+                turn(0.1,-0.8,0.5);
+            } else if (tempLF < tempRF) {
+                turn(0.1,1,0.5);
+            } else {
+                spin(0.1,1);
+            }
+        } else if (tempRF < 12) {
+            if (tempLF < tempRF) {
+                turn(0.1,-1,0.5);
+            } else if (tempLF > tempRF) {
+                turn(0.1,1,0.5);
+            } else {
+               spin(0.1,1);
+            }
+        } else {
+            straight(0.1, 0.7);
+        }
+    }
+}
+
+float spin (float time, float speed) {
+    float temp1 = 0, temp2 = 0;
+    temp1 = leftM.speed(speed);
+    temp2 = rightM.speed(-speed);
+    wait(time);
+    leftM.coast():
+    rightM.coast();
+    return temp1+temp2;//should return 0.
+}
+
+float straight (float time, float speed) {
+    float temp1 = 0, temp2 = 0;
+    temp1 = leftM.speed(speed);
+    temp2 = rightM.speed(speed);
+    wait(time);
+    leftM.coast():
+    rightM.coast();
+    return temp1-temp2;//should return 0.
+}
+
+float stop (float time, float duty) {
+    float temp1 = 0, temp2 = 0;
+    temp1 = leftM.stop(duty);
+    temp2 = rightM.stop(duty);
+    wait(time);
+    leftM.coast():
+    rightM.coast();
+    return temp1-temp2;//should return 0.
+}
+
+float turn (float time, float speed, float diffrence) {
+    float temp1 = -speed*diffrence;
+    if (speed<0) {
+        leftM.speed(speed);
+        rightM.speed(temp1);
+    } else if (speed>0) {
+        leftM.speed(temp1);
+        rightM.speed(speed);
+    } else if (speed == 0) {
+        leftM.coast();
+        rightM.coast();
+    } else {
+        return -5;
+    }
+    wait(time);
+    leftM.coast():
+    rightM.coast();
+    return temp1;//should return -speed*diffrence.
+}
+
+
+
+