Revision 0:1601e157879c, committed 2016-02-02

Comitter:

diegorivera42

Date:

Tue Feb 02 12:22:07 2016 +0000

Commit message:

Marvino with mbed module demo program

Changed in this revision

diff -r 000000000000 -r 1601e157879c main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Feb 02 12:22:07 2016 +0000
@@ -0,0 +1,284 @@
+#include "mbed.h"
+
+Timer t1;
+Timer t2;
+
+DigitalOut myled(LED1);
+I2C i2c(p9, p10);
+AnalogIn light(p18);
+AnalogIn linel(p19);
+AnalogIn liner(p20);
+PwmOut eyes(p21);
+PwmOut motlspd(p22);
+PwmOut motrspd(p23);
+DigitalOut motldir(p24);
+DigitalOut motrdir(p25);
+DigitalOut speaker(p26);
+DigitalIn armr(p27);
+DigitalIn armrf(p28);
+DigitalIn arml(p29);
+DigitalIn armlf(p30);
+
+int HatLightAddr = 0x46;                    // PCA8575 16 bit IO
+char HatLightData[2];                       // 16 bit data
+float PWMFreq = 5000;                       // PWM Frequency in Hz
+float PWMPeriod = 1 / PWMFreq;              // On the mbed this is global for all PWM output pins
+float MotLeftAdj = 1;                       // Left motor speed adjust
+float MotRightAdj = 1;                      // Right motor speed adjust
+float vcc = 3.3;                            // mbed supply voltage
+float ForwardFull = 1;                      // motor forward full speed
+float ForwardNorm = ForwardFull * 0.5;      // forward normal speed
+float ForwardSlow = ForwardNorm * 0.5;      // forward half normal speed
+float ReverseNorm = 0-ForwardNorm;          // reverse normal speed
+float ReverseFull = 0-ForwardFull;          // motor reverse full speed
+float ReverseSlow = 0-ForwardSlow;          // reverse half normal speed
+float Stop = 0.0;                           // stop
+float SensorTimeOut1 = 10;                  // Nothing happening to Arms or Line Following in seconds (change to line following)
+float SensorTimeOut2 = 10;                  // Nothing happening to Light Following in seconds (stuck? so reverse turn)
+int LightChange = 0;                        // used to determine change in light following
+int LineFollow = 0;                         // used to determine continous run of line following
+int LineBright = 1;                         // voltage that determines line brightness
+
+// MotorLeft expects -1 to 1 float for speed and direction
+void MotorLeft(float sp) {
+    if (sp > 0) {
+        motldir = 1;
+    }
+    else {
+        motldir = 0;
+        sp = 0 - sp;
+    }
+    motlspd.pulsewidth(sp * PWMPeriod * MotLeftAdj);   // left motor a bit fast so slow it down
+}
+
+// MotorRight expects -1 to 1 float for speed and direction
+void MotorRight(float sp) {
+    if (sp > 0) {
+        motrdir = 1;
+    }
+    else {
+        motrdir = 0;
+        sp = 0 - sp;
+    }
+    motrspd.pulsewidth(sp * PWMPeriod * MotRightAdj);
+}
+
+// EyesBright expects 0 to 1 float for brrightness
+void EyesBright(float sp) {
+    eyes.pulsewidth(sp * PWMPeriod);
+}
+
+void HatLights(int d) {
+    d=d ^ 0xffff;
+    HatLightData[0]=(d);
+    HatLightData[1]=(d >>8);
+    i2c.write( HatLightAddr, HatLightData, 2 );
+}
+
+unsigned int m_z=12434,m_w=33254;
+ 
+unsigned int rnd() {
+    m_z = 36969 * (m_z & 65535) + (m_z >>16);
+    m_w = 18000 * (m_w & 65535) + (m_w >>16);
+    return ((m_z <<16) + m_w);
+}
+
+// Idea sets hat lights to a random pattern n times at period in seconds
+void Idea(int n, float p) {
+    for (int i=0; i<n; i++) {
+        HatLights(rnd()%0x8000);
+        wait(p);
+    }
+    HatLights(0);
+}
+
+// Thinking rotates hat lights n times at period p in seconds
+void Thinking(int n, float p) {
+    int r=1;
+    for (int i=0; i<n; i++) {
+        HatLights(r);
+        wait(p);
+        if (r == 0x8000) {
+            r=1;
+        }
+        else {
+        r=(r <<1);
+        }
+    }
+    HatLights(0);
+}
+
+// Beep sounder at frequency f (cycles/sec) for period p in seconds
+void Beep(float f, float p) {
+    float t = 0.5/f;
+    for (int i=0; i<(f*p); i++) {
+        speaker = 1;                        // speaker high
+        wait(t);
+        speaker = 0;                        // speaker low
+        wait(t);
+    } 
+}
+
+// FlashEyes flashes eyes n times at period p in seconds
+void FlashEyes(int n, float p) {
+    for (int i=0; i<n; i++) {
+        EyesBright(1);                      // eyes full on
+        wait(p/2);
+        EyesBright(0);                      // eyes off
+        wait(p/2);
+    }
+    EyesBright(0.5);                        // back to half bright
+}
+
+void AvoidLeft() {
+    MotorLeft(Stop);
+    MotorRight(Stop);                       // stop
+    Thinking(16, 0.02);                     // rotate hat lights
+    MotorLeft(ReverseSlow);
+    MotorRight(ReverseNorm);                // reverse turn back
+    FlashEyes(3, 0.1);
+    MotorLeft(ForwardNorm);
+    MotorRight(ForwardNorm);                // forward
+}
+
+void AvoidRight() {
+    MotorLeft(Stop);
+    MotorRight(Stop);                       // stop
+    Thinking(16, 0.02);                     // rotate hat lights
+    MotorLeft(ReverseNorm);
+    MotorRight(ReverseSlow);                // reverse turn back
+    FlashEyes(3, 0.1);
+    MotorLeft(ForwardNorm);
+    MotorRight(ForwardNorm);                // forward
+}   
+
+void AvoidLeftFront() {
+    MotorLeft(Stop);
+    MotorRight(Stop);                       // stop
+    Thinking(16, 0.02);                     // rotate hat lights
+    MotorLeft(ReverseNorm);
+    MotorRight(ReverseNorm);                // reverse
+    FlashEyes(6, 0.1);                      // flash eyes
+    MotorRight(Stop);
+    FlashEyes(6, 0.1);                      // rotate
+    MotorLeft(ForwardNorm);
+    MotorRight(ForwardNorm);                // forward
+}     
+
+void AvoidRightFront() {
+    MotorLeft(Stop);
+    MotorRight(Stop);                       // stop
+    Thinking(16, 0.02);                     // rotate hat lights
+    MotorLeft(ReverseNorm);
+    MotorRight(ReverseNorm);                // reverse
+    FlashEyes(6, 0.1);                      // flash eyes
+    MotorLeft(Stop);
+    FlashEyes(6, 0.1);                      // rotate
+    MotorLeft(ForwardNorm);
+    MotorRight(ForwardNorm);                // forward
+}         
+
+void AvoidHeadOn() {
+    MotorLeft(Stop);
+    MotorRight(Stop);                       // stop
+    Thinking(50, 0.02);                     // rotate hat lights
+        Idea(20, 0.02);                     // flash random hat lights
+    MotorLeft(ReverseNorm);
+    MotorRight(ReverseNorm);                // reverse
+    FlashEyes(8, 0.1);                      // flash eyes
+    MotorLeft(ForwardNorm);
+    FlashEyes(8, 0.1);                      // rotate clockwise
+    MotorRight(ForwardNorm);                // carry on straight
+}
+
+void NudgeLeft() {
+    MotorLeft(Stop);                        // stop left motor
+    wait(0.01);                             // wait a bit
+    MotorLeft(ForwardNorm);                 // carry on
+}     
+
+void NudgeRight() {
+    MotorRight(Stop);                       // stop right motor
+    wait(0.01);                             // wait a bit
+    MotorRight(ForwardNorm);                // carry on
+}     
+
+int main() {
+    motlspd.period(PWMPeriod);
+    motrspd.period(PWMPeriod);
+    eyes.period(PWMPeriod);
+    EyesBright(0.5);                        // eyes half brightness
+    MotorLeft(ForwardNorm);                 // motor left forward
+    MotorRight(ForwardNorm);                // motor right forward
+    t1.start();
+    while(1) {
+        if (arml == 0 && armr != 0) {
+            t1.reset();
+            AvoidLeft();
+        }
+        if (arml != 0 && armr == 0) {
+            t1.reset();
+            AvoidRight();
+        }
+        if (arml == 0 && armr == 0) {
+            t1.reset();
+            AvoidHeadOn();    
+        }
+        if (armlf == 0 && armrf == 0) {
+            t1.reset();
+            AvoidHeadOn();
+        }
+        if (armlf == 0 && armrf != 0) {
+            t1.reset();
+            AvoidLeftFront();
+        }
+        if (armlf != 0 && armrf == 0) {
+            t1.reset();
+            AvoidRightFront();
+        }
+        if ((linel > (LineBright/vcc)) && (liner < (LineBright/vcc))) {
+            NudgeLeft();
+        }
+        if ((linel < (LineBright/vcc)) && (liner > (LineBright/vcc))) {
+            NudgeRight();
+        }
+        if (((linel < (LineBright/vcc)) && (liner < (LineBright/vcc))) || ((linel > (LineBright/vcc)) && (liner > (LineBright/vcc)))) {
+            LineFollow = 0;
+        }
+        else {
+            LineFollow++;
+            if (LineFollow > 10) {
+                t1.reset();                 // only reset activity timer if consistant line following
+            }
+        }
+        if (t1.read() > SensorTimeOut1) {   // if nothing else is happening then follow light
+            t2.start();
+            if (light > 0.7) {              // more light to the left
+                if (LightChange == 0) {
+                    LightChange = 1;
+                    t2.reset();
+                    Beep(4000,0.1);
+                }
+                NudgeLeft();
+            }
+            if (light < 0.3) {              // more light to the right
+                if (LightChange == 1) {
+                    LightChange = 0;
+                    t2.reset();
+                    Beep(1000,0.1);
+                }
+                NudgeRight();
+            }
+        }
+        else {
+            t2.reset();
+            t2.stop();
+        }
+        if (t2.read() > SensorTimeOut2) {   // probably stuck
+            AvoidHeadOn();
+            t1.reset();
+            t2.reset();
+            t2.stop();
+        }
+    }
+}

diff -r 000000000000 -r 1601e157879c mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Feb 02 12:22:07 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/6f327212ef96
\ No newline at end of file