File content as of revision 0:541dc12fcbf0:

#include "mbed.h"
#include "m3pi.h"
#include "rgbLed.h"
#include "color.h"

m3pi m3pi;
Timer timerA;
Serial Wixel(p28, p27); // tx, rx
AnalogIn analog (p20); // Randomness generator
AnalogIn resistorAin (p16); // Voltage divider reads

//Setup RGB led using PWM pins and class
RGBLed myRGBled(p23,p22,p21); //RGB PWM pins

//setup some color objects in flash using const's
const LEDColor red   (255, 0, 0);
const LEDColor green (0, 255, 0);
const LEDColor blue  (0, 0, 255);
const LEDColor yellow(255 ,255 ,0);
const LEDColor white (255 ,255, 255);

DigitalOut PIN15 (p15);
DigitalOut PIN17 (p17);
DigitalOut PIN18 (p18);
DigitalOut PIN19 (p19);
DigitalOut PIN20 (p20);

int behaviour; // Behaviour switcher
float voltage;
int voltageCounter = 0; // To elmimate ADC spike noise
float timerThreshold;


unsigned int random_generator (void){
    unsigned int x = 0;
    unsigned int iRandom = 0;
 
    for (x = 0; x <= 32; x += 2)
    {
        iRandom += ((analog.read_u16() % 3) << x);
        wait_us (10);
    }
    
    return iRandom;
}

void forward(float amount) {
    float speed = amount/5.0 + .075;
    m3pi.left_motor(-speed);
    m3pi.right_motor(-speed);
    timerThreshold = 1;
}

void backward(float amount) {
    float speed = amount/5.0 + .075;
    m3pi.left_motor(speed);
    m3pi.right_motor(speed);
    timerThreshold = 1;
}

void turnLeft(float amount) {
    // try to anchor it so that 1 means 90 degree turn
    float right_speed = amount/5.0 + .075;
    m3pi.left_motor(right_speed);
    
    m3pi.right_motor(-right_speed);
    timerThreshold=.5;
}

void turnRight(float amount) {
    float left_speed = amount/5.0 + .075;
    m3pi.right_motor(left_speed);
    m3pi.left_motor(-left_speed);
    timerThreshold=.5;
}

/*void shakeHead() {
    m3pi.left_motor(.15);
    m3pi.right_motor(-.15);
    wait(.3);
    for(int i = 0; i < 3; i++) {
        if(i % 2 == 0) {
            m3pi.left_motor(-.15);
            m3pi.right_motor(.15);
        } else {
            m3pi.left_motor(.15);
            m3pi.right_motor(-.15);
        }
        timerThreshold = .6);
    }
    m3pi.left_motor(.15);
    m3pi.right_motor(-.15);
    wait(.3);
}*/

void actLikeYoureInLove() {
    myRGBled = red;
    m3pi.left_motor(0.0);
    m3pi.right_motor(0.0);
    wait(1);
    m3pi.left_motor(-0.1);
    m3pi.right_motor(-0.1);
    wait(.5);
    while(1) {
        m3pi.left_motor(.2);
        m3pi.right_motor(-.2);

    }
}

void init()
{
    Wixel.baud(9600);
    timerA.start();
    myRGBled = green;
    srand (random_generator()); // Using ADC value to generate random seed
}

int main() 
{
     
    init();

    while(1)  
    {
        voltage = resistorAin;
        Wixel.printf("Voltage %f \n",voltage);
        
        if (( voltage > 0.45) & ( voltage < 0.7))
        {
            voltageCounter ++;
            if (voltageCounter == 2){
                actLikeYoureInLove();
            }
        }

        //Wixel.printf("%f seconds\n", timerA.read());

        if( timerA.read() >= timerThreshold)
        {
            behaviour = rand() % 5 + 1;  // Generate num between 1 and 5
            //Wixel.printf("Behavior &d\n", behaviour);
            float speed = rand()/double(RAND_MAX);

            switch (behaviour) 
            {
                case 1:
                    forward(1);
                    break;
                case 2:
                    turnLeft(speed);
                    break;
                case 3:
                    turnRight(speed);
                    break;
                case 4:
                    backward(1);
                    break;
                case 5:
                    //Do noting
                    break;
            }
            timerA.reset();
        }
    }
}