Alexander Ferrara / Mbed 2 deprecated 4180_Final_project

bluetooth robot

Dependencies:   Motor ShiftBrite mbed-rtos mbed

main.cpp@0:18550647a842, 2016-12-12 (annotated)

Committer:
AlexFerrara
Date:
Mon Dec 12 04:24:28 2016 +0000
Revision:
0:18550647a842
final

Who changed what in which revision?

UserRevisionLine numberNew contents of line
AlexFerrara 0:18550647a842 1 #include "mbed.h"
AlexFerrara 0:18550647a842 2 #include "Motor.h"
AlexFerrara 0:18550647a842 3 #include "rtos.h"
AlexFerrara 0:18550647a842 4 #include "stdio.h"
AlexFerrara 0:18550647a842 5 #include "ShiftBrite.h"
AlexFerrara 0:18550647a842 6 #include "Speaker.h"
AlexFerrara 0:18550647a842 7
AlexFerrara 0:18550647a842 8 // I/O Configurations
AlexFerrara 0:18550647a842 9
AlexFerrara 0:18550647a842 10 // LEDS
AlexFerrara 0:18550647a842 11 DigitalOut led1(LED1);
AlexFerrara 0:18550647a842 12 DigitalOut led2(LED2);
AlexFerrara 0:18550647a842 13 DigitalOut led3(LED3);
AlexFerrara 0:18550647a842 14 DigitalOut led4(LED4);
AlexFerrara 0:18550647a842 15
AlexFerrara 0:18550647a842 16 // Motors
AlexFerrara 0:18550647a842 17 Motor m_l(p21, p22, p23); // pwm, fwd, rev
AlexFerrara 0:18550647a842 18 Motor m_r(p26, p30, p24); // pwm, fwd, rev
AlexFerrara 0:18550647a842 19
AlexFerrara 0:18550647a842 20 //Shiftbrites
AlexFerrara 0:18550647a842 21 SPI spi(p11, p12, p13);
AlexFerrara 0:18550647a842 22 ShiftBrite myBrite(p15,p16,spi); //latch, enable, spi
AlexFerrara 0:18550647a842 23
AlexFerrara 0:18550647a842 24 //Speaker
AlexFerrara 0:18550647a842 25 Speaker mySpeaker(p25);
AlexFerrara 0:18550647a842 26
AlexFerrara 0:18550647a842 27 // Serial ports
AlexFerrara 0:18550647a842 28 RawSerial pc(USBTX, USBRX);
AlexFerrara 0:18550647a842 29 RawSerial dev(p28, p27);
AlexFerrara 0:18550647a842 30 char X;
AlexFerrara 0:18550647a842 31 Mutex dev_mutex;
AlexFerrara 0:18550647a842 32 DigitalOut rst1(p8);
AlexFerrara 0:18550647a842 33
AlexFerrara 0:18550647a842 34 // IR
AlexFerrara 0:18550647a842 35 AnalogIn IR(p20);
AlexFerrara 0:18550647a842 36 float IR_distance;
AlexFerrara 0:18550647a842 37
AlexFerrara 0:18550647a842 38 // Serial reading variables
AlexFerrara 0:18550647a842 39 char building = '0';
AlexFerrara 0:18550647a842 40 int tVal = 0;
AlexFerrara 0:18550647a842 41 int l_speed = 32767;
AlexFerrara 0:18550647a842 42 int r_speed = 32767;
AlexFerrara 0:18550647a842 43 int c;
AlexFerrara 0:18550647a842 44
AlexFerrara 0:18550647a842 45 // Horn and Light flags
AlexFerrara 0:18550647a842 46 bool horn = false;
AlexFerrara 0:18550647a842 47 int lightMode = 0;
AlexFerrara 0:18550647a842 48 int colorMode = 0;
AlexFerrara 0:18550647a842 49
AlexFerrara 0:18550647a842 50 // Threads
AlexFerrara 0:18550647a842 51
AlexFerrara 0:18550647a842 52 // Motor Thread
AlexFerrara 0:18550647a842 53 // Uses thresholds for the Y-axis of the analog joysticks to set variable motor speed
AlexFerrara 0:18550647a842 54 void thread_Motor(void const *args){
AlexFerrara 0:18550647a842 55 while (true) {
AlexFerrara 0:18550647a842 56
AlexFerrara 0:18550647a842 57 m_r.speed(0);
AlexFerrara 0:18550647a842 58 m_l.speed(0);
AlexFerrara 0:18550647a842 59
AlexFerrara 0:18550647a842 60 // LEFT MOTOR CONTROL
AlexFerrara 0:18550647a842 61 if(l_speed < 30000){
AlexFerrara 0:18550647a842 62 if(l_speed < 8000)
AlexFerrara 0:18550647a842 63 m_l.speed(-1);
AlexFerrara 0:18550647a842 64 else if(l_speed < 16000)
AlexFerrara 0:18550647a842 65 m_l.speed(-0.8);
AlexFerrara 0:18550647a842 66 else if(l_speed < 24000)
AlexFerrara 0:18550647a842 67 m_l.speed(-0.6);
AlexFerrara 0:18550647a842 68 else
AlexFerrara 0:18550647a842 69 m_l.speed(-0.5);
AlexFerrara 0:18550647a842 70 }
AlexFerrara 0:18550647a842 71 else if(l_speed > 34000 & IR_distance < 0.9f){
AlexFerrara 0:18550647a842 72 if(l_speed > 56000)
AlexFerrara 0:18550647a842 73 m_l.speed(1);
AlexFerrara 0:18550647a842 74 else if(l_speed > 48000)
AlexFerrara 0:18550647a842 75 m_l.speed(0.8);
AlexFerrara 0:18550647a842 76 else if(l_speed > 40000)
AlexFerrara 0:18550647a842 77 m_l.speed(0.6);
AlexFerrara 0:18550647a842 78 else
AlexFerrara 0:18550647a842 79 m_l.speed(0.5);
AlexFerrara 0:18550647a842 80 }
AlexFerrara 0:18550647a842 81 else
AlexFerrara 0:18550647a842 82 m_l.speed(0);
AlexFerrara 0:18550647a842 83
AlexFerrara 0:18550647a842 84 // RIGHT MOTOR CONTROL
AlexFerrara 0:18550647a842 85 if(r_speed < 30000){
AlexFerrara 0:18550647a842 86 if(r_speed < 8000)
AlexFerrara 0:18550647a842 87 m_r.speed(-1);
AlexFerrara 0:18550647a842 88 else if(r_speed < 16000)
AlexFerrara 0:18550647a842 89 m_r.speed(-0.8);
AlexFerrara 0:18550647a842 90 else if(r_speed < 24000)
AlexFerrara 0:18550647a842 91 m_r.speed(-0.6);
AlexFerrara 0:18550647a842 92 else
AlexFerrara 0:18550647a842 93 m_r.speed(-0.5);
AlexFerrara 0:18550647a842 94 }
AlexFerrara 0:18550647a842 95 else if(r_speed > 34000 & IR_distance < 0.9f){
AlexFerrara 0:18550647a842 96 if(r_speed > 56000)
AlexFerrara 0:18550647a842 97 m_r.speed(1);
AlexFerrara 0:18550647a842 98 else if(r_speed > 48000)
AlexFerrara 0:18550647a842 99 m_r.speed(0.8);
AlexFerrara 0:18550647a842 100 else if(r_speed > 40000)
AlexFerrara 0:18550647a842 101 m_r.speed(0.6);
AlexFerrara 0:18550647a842 102 else
AlexFerrara 0:18550647a842 103 m_r.speed(0.5);
AlexFerrara 0:18550647a842 104 }
AlexFerrara 0:18550647a842 105 else
AlexFerrara 0:18550647a842 106 m_r.speed(0);
AlexFerrara 0:18550647a842 107 Thread::wait(10);
AlexFerrara 0:18550647a842 108 }
AlexFerrara 0:18550647a842 109 }
AlexFerrara 0:18550647a842 110
AlexFerrara 0:18550647a842 111
AlexFerrara 0:18550647a842 112 // Serial Port Reading Interrupt
AlexFerrara 0:18550647a842 113 void dev_recv(){
AlexFerrara 0:18550647a842 114 led1 = !led1;
AlexFerrara 0:18550647a842 115 while(dev.readable()) {
AlexFerrara 0:18550647a842 116 dev_mutex.lock();
AlexFerrara 0:18550647a842 117 X = dev.getc();
AlexFerrara 0:18550647a842 118 dev_mutex.unlock();
AlexFerrara 0:18550647a842 119 switch (X) {
AlexFerrara 0:18550647a842 120 case 'L': //number button 1
AlexFerrara 0:18550647a842 121 building = 'L';
AlexFerrara 0:18550647a842 122 tVal = 0;
AlexFerrara 0:18550647a842 123 break;
AlexFerrara 0:18550647a842 124 case 'R': //number button 2
AlexFerrara 0:18550647a842 125 building = 'R';
AlexFerrara 0:18550647a842 126 tVal = 0;
AlexFerrara 0:18550647a842 127 break;
AlexFerrara 0:18550647a842 128 case ']': //number button 3
AlexFerrara 0:18550647a842 129 if(building == 'L')
AlexFerrara 0:18550647a842 130 l_speed = tVal;
AlexFerrara 0:18550647a842 131 else
AlexFerrara 0:18550647a842 132 r_speed = tVal;
AlexFerrara 0:18550647a842 133 building = '0';
AlexFerrara 0:18550647a842 134 break;
AlexFerrara 0:18550647a842 135 case 'T': //number button 2
AlexFerrara 0:18550647a842 136 horn = true;
AlexFerrara 0:18550647a842 137 break;
AlexFerrara 0:18550647a842 138 case 't': //number button 2
AlexFerrara 0:18550647a842 139 horn = false;
AlexFerrara 0:18550647a842 140 break;
AlexFerrara 0:18550647a842 141 case 'S': //number button 2
AlexFerrara 0:18550647a842 142 lightMode++;
AlexFerrara 0:18550647a842 143 break;
AlexFerrara 0:18550647a842 144 case 'C': //number button 2
AlexFerrara 0:18550647a842 145 colorMode++;
AlexFerrara 0:18550647a842 146 break;
AlexFerrara 0:18550647a842 147 default:
AlexFerrara 0:18550647a842 148 break;
AlexFerrara 0:18550647a842 149 }
AlexFerrara 0:18550647a842 150 c = (int)X - 48;
AlexFerrara 0:18550647a842 151 if(building != '0' && c >= 0 && c <= 9)
AlexFerrara 0:18550647a842 152 tVal = tVal*10 + c;
AlexFerrara 0:18550647a842 153 }
AlexFerrara 0:18550647a842 154 }
AlexFerrara 0:18550647a842 155
AlexFerrara 0:18550647a842 156 // Shiftbrite Thread
AlexFerrara 0:18550647a842 157 void thread_Lights(void const *args){
AlexFerrara 0:18550647a842 158 int r,g,b;
AlexFerrara 0:18550647a842 159 r=g=b=0;
AlexFerrara 0:18550647a842 160 int br = 0;
AlexFerrara 0:18550647a842 161 int scale = 0;
AlexFerrara 0:18550647a842 162 //myBrite.Brightness(1023,1023,1023);
AlexFerrara 0:18550647a842 163 //myBrite.Write(0,0,0);
AlexFerrara 0:18550647a842 164 while(1) {
AlexFerrara 0:18550647a842 165 int mode = lightMode%3;
AlexFerrara 0:18550647a842 166 int color = colorMode % 4;
AlexFerrara 0:18550647a842 167 switch (color){
AlexFerrara 0:18550647a842 168 case 0:
AlexFerrara 0:18550647a842 169 r=g=b=1;
AlexFerrara 0:18550647a842 170 break;
AlexFerrara 0:18550647a842 171 case 1:
AlexFerrara 0:18550647a842 172 r = 1;
AlexFerrara 0:18550647a842 173 g=b=0;
AlexFerrara 0:18550647a842 174 break;
AlexFerrara 0:18550647a842 175 case 2:
AlexFerrara 0:18550647a842 176 g = 1;
AlexFerrara 0:18550647a842 177 r=b=0;
AlexFerrara 0:18550647a842 178 break;
AlexFerrara 0:18550647a842 179 case 3:
AlexFerrara 0:18550647a842 180 b = 1;
AlexFerrara 0:18550647a842 181 r=g=0;
AlexFerrara 0:18550647a842 182 break;
AlexFerrara 0:18550647a842 183 default:
AlexFerrara 0:18550647a842 184 break;
AlexFerrara 0:18550647a842 185 }
AlexFerrara 0:18550647a842 186
AlexFerrara 0:18550647a842 187 //pc.printf("%d,%d,%d\r\n",r,g,b);
AlexFerrara 0:18550647a842 188 switch (mode){
AlexFerrara 0:18550647a842 189 case 0:
AlexFerrara 0:18550647a842 190 scale = 0;
AlexFerrara 0:18550647a842 191 br = 0;
AlexFerrara 0:18550647a842 192 break;
AlexFerrara 0:18550647a842 193 case 1:
AlexFerrara 0:18550647a842 194 scale = 100;
AlexFerrara 0:18550647a842 195 br = 600;
AlexFerrara 0:18550647a842 196 break;
AlexFerrara 0:18550647a842 197 case 2:
AlexFerrara 0:18550647a842 198 scale= 255;
AlexFerrara 0:18550647a842 199 br = 1023;
AlexFerrara 0:18550647a842 200 break;
AlexFerrara 0:18550647a842 201 default:
AlexFerrara 0:18550647a842 202 break;
AlexFerrara 0:18550647a842 203 }
AlexFerrara 0:18550647a842 204 r = (int) (r * scale);
AlexFerrara 0:18550647a842 205 b = (int) (b * scale);
AlexFerrara 0:18550647a842 206 g = (int) (g * scale);
AlexFerrara 0:18550647a842 207 myBrite.Write(r,g,b);
AlexFerrara 0:18550647a842 208 myBrite.Brightness(br*r,br*g,br*b);
AlexFerrara 0:18550647a842 209
AlexFerrara 0:18550647a842 210 Thread::wait(50);
AlexFerrara 0:18550647a842 211 }
AlexFerrara 0:18550647a842 212 }
AlexFerrara 0:18550647a842 213
AlexFerrara 0:18550647a842 214 // Speaker Thread
AlexFerrara 0:18550647a842 215 void thread_Speaker(void const *args){
AlexFerrara 0:18550647a842 216 while(1){
AlexFerrara 0:18550647a842 217 if (horn){
AlexFerrara 0:18550647a842 218 mySpeaker.PlayNote(400.0,0.2,0.5);
AlexFerrara 0:18550647a842 219 }
AlexFerrara 0:18550647a842 220 Thread::wait(10);
AlexFerrara 0:18550647a842 221 }
AlexFerrara 0:18550647a842 222 }
AlexFerrara 0:18550647a842 223
AlexFerrara 0:18550647a842 224 // Main
AlexFerrara 0:18550647a842 225 int main(){
AlexFerrara 0:18550647a842 226 pc.baud(9600);
AlexFerrara 0:18550647a842 227
AlexFerrara 0:18550647a842 228 dev.attach(&dev_recv, Serial::RxIrq);
AlexFerrara 0:18550647a842 229
AlexFerrara 0:18550647a842 230 Thread t2(thread_Motor);
AlexFerrara 0:18550647a842 231 Thread t3(thread_Lights);
AlexFerrara 0:18550647a842 232 Thread t4(thread_Speaker);
AlexFerrara 0:18550647a842 233
AlexFerrara 0:18550647a842 234 while(1) {
AlexFerrara 0:18550647a842 235 if(IR > 0.9f){
AlexFerrara 0:18550647a842 236 led1 = 1;
AlexFerrara 0:18550647a842 237 led2 = 1;
AlexFerrara 0:18550647a842 238 led3 = 1;
AlexFerrara 0:18550647a842 239 led4 = 1;
AlexFerrara 0:18550647a842 240 }
AlexFerrara 0:18550647a842 241 else if(IR > 0.7f){
AlexFerrara 0:18550647a842 242 led1 = 1;
AlexFerrara 0:18550647a842 243 led2 = 1;
AlexFerrara 0:18550647a842 244 led3 = 1;
AlexFerrara 0:18550647a842 245 led4 = 0;
AlexFerrara 0:18550647a842 246 }
AlexFerrara 0:18550647a842 247 else if(IR > 0.5f){
AlexFerrara 0:18550647a842 248 led1 = 1;
AlexFerrara 0:18550647a842 249 led2 = 1;
AlexFerrara 0:18550647a842 250 led3 = 0;
AlexFerrara 0:18550647a842 251 led4 = 0;
AlexFerrara 0:18550647a842 252 }
AlexFerrara 0:18550647a842 253 else if(IR > 0.3f){
AlexFerrara 0:18550647a842 254 led1 = 1;
AlexFerrara 0:18550647a842 255 led2 = 0;
AlexFerrara 0:18550647a842 256 led3 = 0;
AlexFerrara 0:18550647a842 257 led4 = 0;
AlexFerrara 0:18550647a842 258 }
AlexFerrara 0:18550647a842 259 else if(IR > 0.1f){
AlexFerrara 0:18550647a842 260 led1 = 0;
AlexFerrara 0:18550647a842 261 led2 = 0;
AlexFerrara 0:18550647a842 262 led3 = 0;
AlexFerrara 0:18550647a842 263 led4 = 0;
AlexFerrara 0:18550647a842 264 }
AlexFerrara 0:18550647a842 265 IR_distance = IR;
AlexFerrara 0:18550647a842 266 Thread::wait(50); // wait 0.5s
AlexFerrara 0:18550647a842 267 }
AlexFerrara 0:18550647a842 268 }