main.cpp

00001 #include <mbed.h>
00002 #include <motordriver.h>
00003 #include <Servo.h>
00004 #include <GP2xx.h>
00005 //pc interface
00006 Serial pc(USBTX, USBRX); // tx, rx
00007 //servos
00008 Servo LeftServo(p24);
00009 Servo RightServo(p23);
00010 //motors, left and right side
00011 Motor leftM(p22, p6, p5, 1); // pwm, fwd, rev, can break
00012 Motor rightM(p21, p7, p8, 1); // pwm, fwd, rev, can break
00013 //range finders, left and right side, and left and right front
00014 IRRangeFinder LS(p18,1);
00015 IRRangeFinder LF(p17,1);
00016 IRRangeFinder RF(p16,1);
00017 IRRangeFinder RS(p15,1);
00018 //debug leds.
00019 DigitalOut led1 (LED1);
00020 DigitalOut led2 (LED2);
00021 DigitalOut led3 (LED3);
00022 DigitalOut led4 (LED4);
00023 DigitalOut ledleft  (p14);
00024 DigitalOut ledright (p13);
00025 DigitalOut ledfront (p12);
00026 /** simple object avoidance using a logo based set of functions. it works and provides some intresting ways
00027 * to control behaviour.
00028 *
00029 */
00030 
00031 
00032 /** function to spin the robot on the spot. technicaly redundant, as turn can do the same, but clearer when coding
00033 * @param time, how long to spin for. 
00034 * @param speed how fast to run the motors.
00035 *
00036 */
00037 float spin (float time, float speed);//-1 to 1
00038 /** function to make the robot go fowards in a straight line
00039 * @param time, how long to go straight for. 
00040 * @param speed how fast to run the motors. -1 is reverse, 1 is foward, 0 is technicaly possible but means the robot coasts. 
00041 *
00042 */
00043 float straight (float time, float speed);//0 to 1
00044 /** function to make the robot turn
00045 * @param time, how long to turn for. 
00046 * @param speed how fast to run the motors. -1 is right, 1 is left, 0 is technicaly possible but means the robot coasts. 
00047 * @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
00048 */
00049 float turn (float time, float speed, float diffrence);
00050 /** function to make the robot stop
00051 * @param time, how long to stop for 
00052 * @param duty how hard to stop the motors.  1 is full on, 0 is technicaly possible but means the robot coasts. 
00053 *
00054 */
00055 float stop (float time, float duty);
00056 
00057 int main() {
00058     float tempLF = 0, tempRF = 0;
00059     while (1) {
00060         tempLF = LS.read();//cuts down function calls, and therefore clock cycles used
00061         tempRF = RS.read();
00062 
00063         if (tempLF < 12) {
00064             if (tempLF > tempRF) {
00065                 turn(0.1,-0.8,0.5);
00066             } else if (tempLF < tempRF) {
00067                 turn(0.1,1,0.5);
00068             } else {
00069                 spin(0.1,1);
00070             }
00071         } else if (tempRF < 12) {
00072             if (tempLF < tempRF) {
00073                 turn(0.1,-1,0.5);
00074             } else if (tempLF > tempRF) {
00075                 turn(0.1,1,0.5);
00076             } else {
00077                spin(0.1,1);
00078             }
00079         } else {
00080             straight(0.1, 0.7);
00081         }
00082     }
00083 }
00084 
00085 float spin (float time, float speed) {
00086     float temp1 = 0, temp2 = 0;
00087     temp1 = leftM.speed(speed);
00088     temp2 = rightM.speed(-speed);
00089     wait(time);
00090     leftM.coast():
00091     rightM.coast();
00092     return temp1+temp2;//should return 0.
00093 }
00094 
00095 float straight (float time, float speed) {
00096     float temp1 = 0, temp2 = 0;
00097     temp1 = leftM.speed(speed);
00098     temp2 = rightM.speed(speed);
00099     wait(time);
00100     leftM.coast():
00101     rightM.coast();
00102     return temp1-temp2;//should return 0.
00103 }
00104 
00105 float stop (float time, float duty) {
00106     float temp1 = 0, temp2 = 0;
00107     temp1 = leftM.stop(duty);
00108     temp2 = rightM.stop(duty);
00109     wait(time);
00110     leftM.coast():
00111     rightM.coast();
00112     return temp1-temp2;//should return 0.
00113 }
00114 
00115 float turn (float time, float speed, float diffrence) {
00116     float temp1 = -speed*diffrence;
00117     if (speed<0) {
00118         leftM.speed(speed);
00119         rightM.speed(temp1);
00120     } else if (speed>0) {
00121         leftM.speed(temp1);
00122         rightM.speed(speed);
00123     } else if (speed == 0) {
00124         leftM.coast();
00125         rightM.coast();
00126     } else {
00127         return -5;
00128     }
00129     wait(time);
00130     leftM.coast():
00131     rightM.coast();
00132     return temp1;//should return -speed*diffrence.
00133 }
00134 
00135 
00136 
00137