main.cpp

00001 #include "mbed.h"
00002 #include "m3pi.h"
00003 #include "rgbLed.h"
00004 #include "color.h"
00005 
00006 m3pi m3pi;
00007 Timer timerA;
00008 Serial Wixel(p28, p27); // tx, rx
00009 AnalogIn analog (p20); // Randomness generator
00010 AnalogIn resistorAin (p16); // Voltage divider reads
00011 
00012 //Setup RGB led using PWM pins and class
00013 RGBLed myRGBled(p23,p22,p21); //RGB PWM pins
00014 
00015 //setup some color objects in flash using const's
00016 const LEDColor red   (255, 0, 0);
00017 const LEDColor green (0, 255, 0);
00018 const LEDColor blue  (0, 0, 255);
00019 const LEDColor yellow(255 ,255 ,0);
00020 const LEDColor white (255 ,255, 255);
00021 
00022 DigitalOut PIN15 (p15);
00023 DigitalOut PIN17 (p17);
00024 DigitalOut PIN18 (p18);
00025 DigitalOut PIN19 (p19);
00026 DigitalOut PIN20 (p20);
00027 
00028 int behaviour; // Behaviour switcher
00029 float voltage;
00030 int voltageCounter = 0; // To elmimate ADC spike noise
00031 float timerThreshold;
00032 
00033 
00034 unsigned int random_generator (void){
00035     unsigned int x = 0;
00036     unsigned int iRandom = 0;
00037  
00038     for (x = 0; x <= 32; x += 2)
00039     {
00040         iRandom += ((analog.read_u16() % 3) << x);
00041         wait_us (10);
00042     }
00043     
00044     return iRandom;
00045 }
00046 
00047 void forward(float amount) {
00048     float speed = amount/5.0 + .075;
00049     m3pi.left_motor(-speed);
00050     m3pi.right_motor(-speed);
00051     timerThreshold = 1;
00052 }
00053 
00054 void backward(float amount) {
00055     float speed = amount/5.0 + .075;
00056     m3pi.left_motor(speed);
00057     m3pi.right_motor(speed);
00058     timerThreshold = 1;
00059 }
00060 
00061 void turnLeft(float amount) {
00062     // try to anchor it so that 1 means 90 degree turn
00063     float right_speed = amount/5.0 + .075;
00064     m3pi.left_motor(right_speed);
00065     
00066     m3pi.right_motor(-right_speed);
00067     timerThreshold=.5;
00068 }
00069 
00070 void turnRight(float amount) {
00071     float left_speed = amount/5.0 + .075;
00072     m3pi.right_motor(left_speed);
00073     m3pi.left_motor(-left_speed);
00074     timerThreshold=.5;
00075 }
00076 
00077 /*void shakeHead() {
00078     m3pi.left_motor(.15);
00079     m3pi.right_motor(-.15);
00080     wait(.3);
00081     for(int i = 0; i < 3; i++) {
00082         if(i % 2 == 0) {
00083             m3pi.left_motor(-.15);
00084             m3pi.right_motor(.15);
00085         } else {
00086             m3pi.left_motor(.15);
00087             m3pi.right_motor(-.15);
00088         }
00089         timerThreshold = .6);
00090     }
00091     m3pi.left_motor(.15);
00092     m3pi.right_motor(-.15);
00093     wait(.3);
00094 }*/
00095 
00096 void actLikeYoureInLove() {
00097     myRGBled = red;
00098     m3pi.left_motor(0.0);
00099     m3pi.right_motor(0.0);
00100     wait(1);
00101     m3pi.left_motor(-0.1);
00102     m3pi.right_motor(-0.1);
00103     wait(.5);
00104     while(1) {
00105         m3pi.left_motor(.2);
00106         m3pi.right_motor(-.2);
00107 
00108     }
00109 }
00110 
00111 void init()
00112 {
00113     Wixel.baud(9600);
00114     timerA.start();
00115     myRGBled = green;
00116     srand (random_generator()); // Using ADC value to generate random seed
00117 }
00118 
00119 int main() 
00120 {
00121      
00122     init();
00123 
00124     while(1)  
00125     {
00126         voltage = resistorAin;
00127         Wixel.printf("Voltage %f \n",voltage);
00128         
00129         if (( voltage > 0.45) & ( voltage < 0.7))
00130         {
00131             voltageCounter ++;
00132             if (voltageCounter == 2){
00133                 actLikeYoureInLove();
00134             }
00135         }
00136 
00137         //Wixel.printf("%f seconds\n", timerA.read());
00138 
00139         if( timerA.read() >= timerThreshold)
00140         {
00141             behaviour = rand() % 5 + 1;  // Generate num between 1 and 5
00142             //Wixel.printf("Behavior &d\n", behaviour);
00143             float speed = rand()/double(RAND_MAX);
00144 
00145             switch (behaviour) 
00146             {
00147                 case 1:
00148                     forward(1);
00149                     break;
00150                 case 2:
00151                     turnLeft(speed);
00152                     break;
00153                 case 3:
00154                     turnRight(speed);
00155                     break;
00156                 case 4:
00157                     backward(1);
00158                     break;
00159                 case 5:
00160                     //Do noting
00161                     break;
00162             }
00163             timerA.reset();
00164         }
00165     }
00166 }