main.cpp

00001 #include "mbed.h"
00002 
00003 Timer t1;
00004 Timer t2;
00005 
00006 DigitalOut myled(LED1);
00007 I2C i2c(p9, p10);
00008 AnalogIn light(p18);
00009 AnalogIn linel(p19);
00010 AnalogIn liner(p20);
00011 PwmOut eyes(p21);
00012 PwmOut motlspd(p22);
00013 PwmOut motrspd(p23);
00014 DigitalOut motldir(p24);
00015 DigitalOut motrdir(p25);
00016 DigitalOut speaker(p26);
00017 DigitalIn armr(p27);
00018 DigitalIn armrf(p28);
00019 DigitalIn arml(p29);
00020 DigitalIn armlf(p30);
00021 
00022 int HatLightAddr = 0x46;                    // PCA8575 16 bit IO
00023 char HatLightData[2];                       // 16 bit data
00024 float PWMFreq = 5000;                       // PWM Frequency in Hz
00025 float PWMPeriod = 1 / PWMFreq;              // On the mbed this is global for all PWM output pins
00026 float MotLeftAdj = 1;                       // Left motor speed adjust
00027 float MotRightAdj = 1;                      // Right motor speed adjust
00028 float vcc = 3.3;                            // mbed supply voltage
00029 float ForwardFull = 1;                      // motor forward full speed
00030 float ForwardNorm = ForwardFull * 0.5;      // forward normal speed
00031 float ForwardSlow = ForwardNorm * 0.5;      // forward half normal speed
00032 float ReverseNorm = 0-ForwardNorm;          // reverse normal speed
00033 float ReverseFull = 0-ForwardFull;          // motor reverse full speed
00034 float ReverseSlow = 0-ForwardSlow;          // reverse half normal speed
00035 float Stop = 0.0;                           // stop
00036 float SensorTimeOut1 = 10;                  // Nothing happening to Arms or Line Following in seconds (change to line following)
00037 float SensorTimeOut2 = 10;                  // Nothing happening to Light Following in seconds (stuck? so reverse turn)
00038 int LightChange = 0;                        // used to determine change in light following
00039 int LineFollow = 0;                         // used to determine continous run of line following
00040 int LineBright = 1;                         // voltage that determines line brightness
00041 
00042 // MotorLeft expects -1 to 1 float for speed and direction
00043 void MotorLeft(float sp) {
00044     if (sp > 0) {
00045         motldir = 1;
00046     }
00047     else {
00048         motldir = 0;
00049         sp = 0 - sp;
00050     }
00051     motlspd.pulsewidth(sp * PWMPeriod * MotLeftAdj);   // left motor a bit fast so slow it down
00052 }
00053 
00054 // MotorRight expects -1 to 1 float for speed and direction
00055 void MotorRight(float sp) {
00056     if (sp > 0) {
00057         motrdir = 1;
00058     }
00059     else {
00060         motrdir = 0;
00061         sp = 0 - sp;
00062     }
00063     motrspd.pulsewidth(sp * PWMPeriod * MotRightAdj);
00064 }
00065 
00066 // EyesBright expects 0 to 1 float for brrightness
00067 void EyesBright(float sp) {
00068     eyes.pulsewidth(sp * PWMPeriod);
00069 }
00070 
00071 void HatLights(int d) {
00072     d=d ^ 0xffff;
00073     HatLightData[0]=(d);
00074     HatLightData[1]=(d >>8);
00075     i2c.write( HatLightAddr, HatLightData, 2 );
00076 }
00077 
00078 unsigned int m_z=12434,m_w=33254;
00079  
00080 unsigned int rnd() {
00081     m_z = 36969 * (m_z & 65535) + (m_z >>16);
00082     m_w = 18000 * (m_w & 65535) + (m_w >>16);
00083     return ((m_z <<16) + m_w);
00084 }
00085 
00086 // Idea sets hat lights to a random pattern n times at period in seconds
00087 void Idea(int n, float p) {
00088     for (int i=0; i<n; i++) {
00089         HatLights(rnd()%0x8000);
00090         wait(p);
00091     }
00092     HatLights(0);
00093 }
00094 
00095 // Thinking rotates hat lights n times at period p in seconds
00096 void Thinking(int n, float p) {
00097     int r=1;
00098     for (int i=0; i<n; i++) {
00099         HatLights(r);
00100         wait(p);
00101         if (r == 0x8000) {
00102             r=1;
00103         }
00104         else {
00105         r=(r <<1);
00106         }
00107     }
00108     HatLights(0);
00109 }
00110 
00111 // Beep sounder at frequency f (cycles/sec) for period p in seconds
00112 void Beep(float f, float p) {
00113     float t = 0.5/f;
00114     for (int i=0; i<(f*p); i++) {
00115         speaker = 1;                        // speaker high
00116         wait(t);
00117         speaker = 0;                        // speaker low
00118         wait(t);
00119     } 
00120 }
00121 
00122 // FlashEyes flashes eyes n times at period p in seconds
00123 void FlashEyes(int n, float p) {
00124     for (int i=0; i<n; i++) {
00125         EyesBright(1);                      // eyes full on
00126         wait(p/2);
00127         EyesBright(0);                      // eyes off
00128         wait(p/2);
00129     }
00130     EyesBright(0.5);                        // back to half bright
00131 }
00132 
00133 void AvoidLeft() {
00134     MotorLeft(Stop);
00135     MotorRight(Stop);                       // stop
00136     Thinking(16, 0.02);                     // rotate hat lights
00137     MotorLeft(ReverseSlow);
00138     MotorRight(ReverseNorm);                // reverse turn back
00139     FlashEyes(3, 0.1);
00140     MotorLeft(ForwardNorm);
00141     MotorRight(ForwardNorm);                // forward
00142 }
00143 
00144 void AvoidRight() {
00145     MotorLeft(Stop);
00146     MotorRight(Stop);                       // stop
00147     Thinking(16, 0.02);                     // rotate hat lights
00148     MotorLeft(ReverseNorm);
00149     MotorRight(ReverseSlow);                // reverse turn back
00150     FlashEyes(3, 0.1);
00151     MotorLeft(ForwardNorm);
00152     MotorRight(ForwardNorm);                // forward
00153 }   
00154 
00155 void AvoidLeftFront() {
00156     MotorLeft(Stop);
00157     MotorRight(Stop);                       // stop
00158     Thinking(16, 0.02);                     // rotate hat lights
00159     MotorLeft(ReverseNorm);
00160     MotorRight(ReverseNorm);                // reverse
00161     FlashEyes(6, 0.1);                      // flash eyes
00162     MotorRight(Stop);
00163     FlashEyes(6, 0.1);                      // rotate
00164     MotorLeft(ForwardNorm);
00165     MotorRight(ForwardNorm);                // forward
00166 }     
00167 
00168 void AvoidRightFront() {
00169     MotorLeft(Stop);
00170     MotorRight(Stop);                       // stop
00171     Thinking(16, 0.02);                     // rotate hat lights
00172     MotorLeft(ReverseNorm);
00173     MotorRight(ReverseNorm);                // reverse
00174     FlashEyes(6, 0.1);                      // flash eyes
00175     MotorLeft(Stop);
00176     FlashEyes(6, 0.1);                      // rotate
00177     MotorLeft(ForwardNorm);
00178     MotorRight(ForwardNorm);                // forward
00179 }         
00180 
00181 void AvoidHeadOn() {
00182     MotorLeft(Stop);
00183     MotorRight(Stop);                       // stop
00184     Thinking(50, 0.02);                     // rotate hat lights
00185         Idea(20, 0.02);                     // flash random hat lights
00186     MotorLeft(ReverseNorm);
00187     MotorRight(ReverseNorm);                // reverse
00188     FlashEyes(8, 0.1);                      // flash eyes
00189     MotorLeft(ForwardNorm);
00190     FlashEyes(8, 0.1);                      // rotate clockwise
00191     MotorRight(ForwardNorm);                // carry on straight
00192 }
00193 
00194 void NudgeLeft() {
00195     MotorLeft(Stop);                        // stop left motor
00196     wait(0.01);                             // wait a bit
00197     MotorLeft(ForwardNorm);                 // carry on
00198 }     
00199 
00200 void NudgeRight() {
00201     MotorRight(Stop);                       // stop right motor
00202     wait(0.01);                             // wait a bit
00203     MotorRight(ForwardNorm);                // carry on
00204 }     
00205 
00206 int main() {
00207     motlspd.period(PWMPeriod);
00208     motrspd.period(PWMPeriod);
00209     eyes.period(PWMPeriod);
00210     EyesBright(0.5);                        // eyes half brightness
00211     MotorLeft(ForwardNorm);                 // motor left forward
00212     MotorRight(ForwardNorm);                // motor right forward
00213     t1.start();
00214     while(1) {
00215         if (arml == 0 && armr != 0) {
00216             t1.reset();
00217             AvoidLeft();
00218         }
00219         if (arml != 0 && armr == 0) {
00220             t1.reset();
00221             AvoidRight();
00222         }
00223         if (arml == 0 && armr == 0) {
00224             t1.reset();
00225             AvoidHeadOn();    
00226         }
00227         if (armlf == 0 && armrf == 0) {
00228             t1.reset();
00229             AvoidHeadOn();
00230         }
00231         if (armlf == 0 && armrf != 0) {
00232             t1.reset();
00233             AvoidLeftFront();
00234         }
00235         if (armlf != 0 && armrf == 0) {
00236             t1.reset();
00237             AvoidRightFront();
00238         }
00239         if ((linel > (LineBright/vcc)) && (liner < (LineBright/vcc))) {
00240             NudgeLeft();
00241         }
00242         if ((linel < (LineBright/vcc)) && (liner > (LineBright/vcc))) {
00243             NudgeRight();
00244         }
00245         if (((linel < (LineBright/vcc)) && (liner < (LineBright/vcc))) || ((linel > (LineBright/vcc)) && (liner > (LineBright/vcc)))) {
00246             LineFollow = 0;
00247         }
00248         else {
00249             LineFollow++;
00250             if (LineFollow > 10) {
00251                 t1.reset();                 // only reset activity timer if consistant line following
00252             }
00253         }
00254         if (t1.read() > SensorTimeOut1) {   // if nothing else is happening then follow light
00255             t2.start();
00256             if (light > 0.7) {              // more light to the left
00257                 if (LightChange == 0) {
00258                     LightChange = 1;
00259                     t2.reset();
00260                     Beep(4000,0.1);
00261                 }
00262                 NudgeLeft();
00263             }
00264             if (light < 0.3) {              // more light to the right
00265                 if (LightChange == 1) {
00266                     LightChange = 0;
00267                     t2.reset();
00268                     Beep(1000,0.1);
00269                 }
00270                 NudgeRight();
00271             }
00272         }
00273         else {
00274             t2.reset();
00275             t2.stop();
00276         }
00277         if (t2.read() > SensorTimeOut2) {   // probably stuck
00278             AvoidHeadOn();
00279             t1.reset();
00280             t2.reset();
00281             t2.stop();
00282         }
00283     }
00284 }