File content as of revision 0:3c55e8a81dfb:

#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();
        }
    }
}