George Wang / Mbed 2 deprecated Robot_Love

Mobile Robot Project

Dependencies:   RGB_LED m3pi mbed

main.cpp@0:541dc12fcbf0, 2015-10-29 (annotated)

Committer:
wqz9822
Date:
Thu Oct 29 16:19:19 2015 +0000
Revision:
0:541dc12fcbf0
Mobile Robot Project

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wqz9822 0:541dc12fcbf0 1 #include "mbed.h"
wqz9822 0:541dc12fcbf0 2 #include "m3pi.h"
wqz9822 0:541dc12fcbf0 3 #include "rgbLed.h"
wqz9822 0:541dc12fcbf0 4 #include "color.h"
wqz9822 0:541dc12fcbf0 5
wqz9822 0:541dc12fcbf0 6 m3pi m3pi;
wqz9822 0:541dc12fcbf0 7 Timer timerA;
wqz9822 0:541dc12fcbf0 8 Serial Wixel(p28, p27); // tx, rx
wqz9822 0:541dc12fcbf0 9 AnalogIn analog (p20); // Randomness generator
wqz9822 0:541dc12fcbf0 10 AnalogIn resistorAin (p16); // Voltage divider reads
wqz9822 0:541dc12fcbf0 11
wqz9822 0:541dc12fcbf0 12 //Setup RGB led using PWM pins and class
wqz9822 0:541dc12fcbf0 13 RGBLed myRGBled(p23,p22,p21); //RGB PWM pins
wqz9822 0:541dc12fcbf0 14
wqz9822 0:541dc12fcbf0 15 //setup some color objects in flash using const's
wqz9822 0:541dc12fcbf0 16 const LEDColor red (255, 0, 0);
wqz9822 0:541dc12fcbf0 17 const LEDColor green (0, 255, 0);
wqz9822 0:541dc12fcbf0 18 const LEDColor blue (0, 0, 255);
wqz9822 0:541dc12fcbf0 19 const LEDColor yellow(255 ,255 ,0);
wqz9822 0:541dc12fcbf0 20 const LEDColor white (255 ,255, 255);
wqz9822 0:541dc12fcbf0 21
wqz9822 0:541dc12fcbf0 22 DigitalOut PIN15 (p15);
wqz9822 0:541dc12fcbf0 23 DigitalOut PIN17 (p17);
wqz9822 0:541dc12fcbf0 24 DigitalOut PIN18 (p18);
wqz9822 0:541dc12fcbf0 25 DigitalOut PIN19 (p19);
wqz9822 0:541dc12fcbf0 26 DigitalOut PIN20 (p20);
wqz9822 0:541dc12fcbf0 27
wqz9822 0:541dc12fcbf0 28 int behaviour; // Behaviour switcher
wqz9822 0:541dc12fcbf0 29 float voltage;
wqz9822 0:541dc12fcbf0 30 int voltageCounter = 0; // To elmimate ADC spike noise
wqz9822 0:541dc12fcbf0 31 float timerThreshold;
wqz9822 0:541dc12fcbf0 32
wqz9822 0:541dc12fcbf0 33
wqz9822 0:541dc12fcbf0 34 unsigned int random_generator (void){
wqz9822 0:541dc12fcbf0 35 unsigned int x = 0;
wqz9822 0:541dc12fcbf0 36 unsigned int iRandom = 0;
wqz9822 0:541dc12fcbf0 37
wqz9822 0:541dc12fcbf0 38 for (x = 0; x <= 32; x += 2)
wqz9822 0:541dc12fcbf0 39 {
wqz9822 0:541dc12fcbf0 40 iRandom += ((analog.read_u16() % 3) << x);
wqz9822 0:541dc12fcbf0 41 wait_us (10);
wqz9822 0:541dc12fcbf0 42 }
wqz9822 0:541dc12fcbf0 43
wqz9822 0:541dc12fcbf0 44 return iRandom;
wqz9822 0:541dc12fcbf0 45 }
wqz9822 0:541dc12fcbf0 46
wqz9822 0:541dc12fcbf0 47 void forward(float amount) {
wqz9822 0:541dc12fcbf0 48 float speed = amount/5.0 + .075;
wqz9822 0:541dc12fcbf0 49 m3pi.left_motor(-speed);
wqz9822 0:541dc12fcbf0 50 m3pi.right_motor(-speed);
wqz9822 0:541dc12fcbf0 51 timerThreshold = 1;
wqz9822 0:541dc12fcbf0 52 }
wqz9822 0:541dc12fcbf0 53
wqz9822 0:541dc12fcbf0 54 void backward(float amount) {
wqz9822 0:541dc12fcbf0 55 float speed = amount/5.0 + .075;
wqz9822 0:541dc12fcbf0 56 m3pi.left_motor(speed);
wqz9822 0:541dc12fcbf0 57 m3pi.right_motor(speed);
wqz9822 0:541dc12fcbf0 58 timerThreshold = 1;
wqz9822 0:541dc12fcbf0 59 }
wqz9822 0:541dc12fcbf0 60
wqz9822 0:541dc12fcbf0 61 void turnLeft(float amount) {
wqz9822 0:541dc12fcbf0 62 // try to anchor it so that 1 means 90 degree turn
wqz9822 0:541dc12fcbf0 63 float right_speed = amount/5.0 + .075;
wqz9822 0:541dc12fcbf0 64 m3pi.left_motor(right_speed);
wqz9822 0:541dc12fcbf0 65
wqz9822 0:541dc12fcbf0 66 m3pi.right_motor(-right_speed);
wqz9822 0:541dc12fcbf0 67 timerThreshold=.5;
wqz9822 0:541dc12fcbf0 68 }
wqz9822 0:541dc12fcbf0 69
wqz9822 0:541dc12fcbf0 70 void turnRight(float amount) {
wqz9822 0:541dc12fcbf0 71 float left_speed = amount/5.0 + .075;
wqz9822 0:541dc12fcbf0 72 m3pi.right_motor(left_speed);
wqz9822 0:541dc12fcbf0 73 m3pi.left_motor(-left_speed);
wqz9822 0:541dc12fcbf0 74 timerThreshold=.5;
wqz9822 0:541dc12fcbf0 75 }
wqz9822 0:541dc12fcbf0 76
wqz9822 0:541dc12fcbf0 77 /*void shakeHead() {
wqz9822 0:541dc12fcbf0 78 m3pi.left_motor(.15);
wqz9822 0:541dc12fcbf0 79 m3pi.right_motor(-.15);
wqz9822 0:541dc12fcbf0 80 wait(.3);
wqz9822 0:541dc12fcbf0 81 for(int i = 0; i < 3; i++) {
wqz9822 0:541dc12fcbf0 82 if(i % 2 == 0) {
wqz9822 0:541dc12fcbf0 83 m3pi.left_motor(-.15);
wqz9822 0:541dc12fcbf0 84 m3pi.right_motor(.15);
wqz9822 0:541dc12fcbf0 85 } else {
wqz9822 0:541dc12fcbf0 86 m3pi.left_motor(.15);
wqz9822 0:541dc12fcbf0 87 m3pi.right_motor(-.15);
wqz9822 0:541dc12fcbf0 88 }
wqz9822 0:541dc12fcbf0 89 timerThreshold = .6);
wqz9822 0:541dc12fcbf0 90 }
wqz9822 0:541dc12fcbf0 91 m3pi.left_motor(.15);
wqz9822 0:541dc12fcbf0 92 m3pi.right_motor(-.15);
wqz9822 0:541dc12fcbf0 93 wait(.3);
wqz9822 0:541dc12fcbf0 94 }*/
wqz9822 0:541dc12fcbf0 95
wqz9822 0:541dc12fcbf0 96 void actLikeYoureInLove() {
wqz9822 0:541dc12fcbf0 97 myRGBled = red;
wqz9822 0:541dc12fcbf0 98 m3pi.left_motor(0.0);
wqz9822 0:541dc12fcbf0 99 m3pi.right_motor(0.0);
wqz9822 0:541dc12fcbf0 100 wait(1);
wqz9822 0:541dc12fcbf0 101 m3pi.left_motor(-0.1);
wqz9822 0:541dc12fcbf0 102 m3pi.right_motor(-0.1);
wqz9822 0:541dc12fcbf0 103 wait(.5);
wqz9822 0:541dc12fcbf0 104 while(1) {
wqz9822 0:541dc12fcbf0 105 m3pi.left_motor(.2);
wqz9822 0:541dc12fcbf0 106 m3pi.right_motor(-.2);
wqz9822 0:541dc12fcbf0 107
wqz9822 0:541dc12fcbf0 108 }
wqz9822 0:541dc12fcbf0 109 }
wqz9822 0:541dc12fcbf0 110
wqz9822 0:541dc12fcbf0 111 void init()
wqz9822 0:541dc12fcbf0 112 {
wqz9822 0:541dc12fcbf0 113 Wixel.baud(9600);
wqz9822 0:541dc12fcbf0 114 timerA.start();
wqz9822 0:541dc12fcbf0 115 myRGBled = green;
wqz9822 0:541dc12fcbf0 116 srand (random_generator()); // Using ADC value to generate random seed
wqz9822 0:541dc12fcbf0 117 }
wqz9822 0:541dc12fcbf0 118
wqz9822 0:541dc12fcbf0 119 int main()
wqz9822 0:541dc12fcbf0 120 {
wqz9822 0:541dc12fcbf0 121
wqz9822 0:541dc12fcbf0 122 init();
wqz9822 0:541dc12fcbf0 123
wqz9822 0:541dc12fcbf0 124 while(1)
wqz9822 0:541dc12fcbf0 125 {
wqz9822 0:541dc12fcbf0 126 voltage = resistorAin;
wqz9822 0:541dc12fcbf0 127 Wixel.printf("Voltage %f \n",voltage);
wqz9822 0:541dc12fcbf0 128
wqz9822 0:541dc12fcbf0 129 if (( voltage > 0.45) & ( voltage < 0.7))
wqz9822 0:541dc12fcbf0 130 {
wqz9822 0:541dc12fcbf0 131 voltageCounter ++;
wqz9822 0:541dc12fcbf0 132 if (voltageCounter == 2){
wqz9822 0:541dc12fcbf0 133 actLikeYoureInLove();
wqz9822 0:541dc12fcbf0 134 }
wqz9822 0:541dc12fcbf0 135 }
wqz9822 0:541dc12fcbf0 136
wqz9822 0:541dc12fcbf0 137 //Wixel.printf("%f seconds\n", timerA.read());
wqz9822 0:541dc12fcbf0 138
wqz9822 0:541dc12fcbf0 139 if( timerA.read() >= timerThreshold)
wqz9822 0:541dc12fcbf0 140 {
wqz9822 0:541dc12fcbf0 141 behaviour = rand() % 5 + 1; // Generate num between 1 and 5
wqz9822 0:541dc12fcbf0 142 //Wixel.printf("Behavior &d\n", behaviour);
wqz9822 0:541dc12fcbf0 143 float speed = rand()/double(RAND_MAX);
wqz9822 0:541dc12fcbf0 144
wqz9822 0:541dc12fcbf0 145 switch (behaviour)
wqz9822 0:541dc12fcbf0 146 {
wqz9822 0:541dc12fcbf0 147 case 1:
wqz9822 0:541dc12fcbf0 148 forward(1);
wqz9822 0:541dc12fcbf0 149 break;
wqz9822 0:541dc12fcbf0 150 case 2:
wqz9822 0:541dc12fcbf0 151 turnLeft(speed);
wqz9822 0:541dc12fcbf0 152 break;
wqz9822 0:541dc12fcbf0 153 case 3:
wqz9822 0:541dc12fcbf0 154 turnRight(speed);
wqz9822 0:541dc12fcbf0 155 break;
wqz9822 0:541dc12fcbf0 156 case 4:
wqz9822 0:541dc12fcbf0 157 backward(1);
wqz9822 0:541dc12fcbf0 158 break;
wqz9822 0:541dc12fcbf0 159 case 5:
wqz9822 0:541dc12fcbf0 160 //Do noting
wqz9822 0:541dc12fcbf0 161 break;
wqz9822 0:541dc12fcbf0 162 }
wqz9822 0:541dc12fcbf0 163 timerA.reset();
wqz9822 0:541dc12fcbf0 164 }
wqz9822 0:541dc12fcbf0 165 }
wqz9822 0:541dc12fcbf0 166 }