Stephen Piper / Mbed 2 deprecated ECE4180_PROJECT_ROBOT

Initial Release

Dependencies:   MCP23S17 Motor SDFileSystem mbed-rtos mbed wave_player

main.cpp@0:5be0a1cce3f6, 2017-12-06 (annotated)

Committer:
EngineerGuy1GT
Date:
Wed Dec 06 16:34:32 2017 +0000
Revision:
0:5be0a1cce3f6
Initial Release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
EngineerGuy1GT 0:5be0a1cce3f6 1 #include "mbed.h"
EngineerGuy1GT 0:5be0a1cce3f6 2 #include "rtos.h"
EngineerGuy1GT 0:5be0a1cce3f6 3 #include "Motor.h"
EngineerGuy1GT 0:5be0a1cce3f6 4 #include "SDFileSystem.h"
EngineerGuy1GT 0:5be0a1cce3f6 5 #include "wave_player.h"
EngineerGuy1GT 0:5be0a1cce3f6 6 #include "SongPlayer.h"
EngineerGuy1GT 0:5be0a1cce3f6 7 #include "Speaker.h"
EngineerGuy1GT 0:5be0a1cce3f6 8 #include "MCP23S17.h"
EngineerGuy1GT 0:5be0a1cce3f6 9 #include "mbed.h"
EngineerGuy1GT 0:5be0a1cce3f6 10
EngineerGuy1GT 0:5be0a1cce3f6 11 Serial pc(USBTX, USBRX);
EngineerGuy1GT 0:5be0a1cce3f6 12
EngineerGuy1GT 0:5be0a1cce3f6 13 //ZIGBEE (XBEE)
EngineerGuy1GT 0:5be0a1cce3f6 14 Serial xbee(p9, p10);
EngineerGuy1GT 0:5be0a1cce3f6 15 DigitalOut xbee_rst(p8);
EngineerGuy1GT 0:5be0a1cce3f6 16
EngineerGuy1GT 0:5be0a1cce3f6 17 //DC MOTORS
EngineerGuy1GT 0:5be0a1cce3f6 18 Motor motor_left(p26, p30, p29); // pwm, fwd, rev
EngineerGuy1GT 0:5be0a1cce3f6 19 Motor motor_right(p23, p25, p24); // pwm, fwd, rev
EngineerGuy1GT 0:5be0a1cce3f6 20
EngineerGuy1GT 0:5be0a1cce3f6 21 //SD CARD
EngineerGuy1GT 0:5be0a1cce3f6 22 SDFileSystem sd(p11, p12, p13, p14, "sd"); // (MOSI,MISO,SCLK,CS,NAME)
EngineerGuy1GT 0:5be0a1cce3f6 23
EngineerGuy1GT 0:5be0a1cce3f6 24 // I/O EXPANDER
EngineerGuy1GT 0:5be0a1cce3f6 25 SPI IO_spi(p5, p6, p7); //MOSI(SI), MISO(SO), SCLK(SCK)
EngineerGuy1GT 0:5be0a1cce3f6 26 DigitalOut IO_rst(p15);
EngineerGuy1GT 0:5be0a1cce3f6 27
EngineerGuy1GT 0:5be0a1cce3f6 28 //Speaker
EngineerGuy1GT 0:5be0a1cce3f6 29 AnalogOut DACout(p18);
EngineerGuy1GT 0:5be0a1cce3f6 30 wave_player waver(&DACout);
EngineerGuy1GT 0:5be0a1cce3f6 31 FILE *wave_file;
EngineerGuy1GT 0:5be0a1cce3f6 32
EngineerGuy1GT 0:5be0a1cce3f6 33 //LED Signals
EngineerGuy1GT 0:5be0a1cce3f6 34 DigitalOut brake_sig(p20);
EngineerGuy1GT 0:5be0a1cce3f6 35 DigitalOut left_turn_sig(p22);
EngineerGuy1GT 0:5be0a1cce3f6 36 DigitalOut right_turn_sig(p21);
EngineerGuy1GT 0:5be0a1cce3f6 37
EngineerGuy1GT 0:5be0a1cce3f6 38 //Mutex Lock
EngineerGuy1GT 0:5be0a1cce3f6 39 Mutex mutex;
EngineerGuy1GT 0:5be0a1cce3f6 40
EngineerGuy1GT 0:5be0a1cce3f6 41 //Threads
EngineerGuy1GT 0:5be0a1cce3f6 42 Thread motor_thread;
EngineerGuy1GT 0:5be0a1cce3f6 43 Thread speaker_thread;
EngineerGuy1GT 0:5be0a1cce3f6 44 Thread RGB_LED_bar_thread;
EngineerGuy1GT 0:5be0a1cce3f6 45
EngineerGuy1GT 0:5be0a1cce3f6 46 volatile float left_speed;
EngineerGuy1GT 0:5be0a1cce3f6 47 volatile float right_speed;
EngineerGuy1GT 0:5be0a1cce3f6 48 volatile float old_left;
EngineerGuy1GT 0:5be0a1cce3f6 49 volatile float old_right;
EngineerGuy1GT 0:5be0a1cce3f6 50 volatile char RX_char;
EngineerGuy1GT 0:5be0a1cce3f6 51
EngineerGuy1GT 0:5be0a1cce3f6 52 float get_left_speed(char input)
EngineerGuy1GT 0:5be0a1cce3f6 53 {
EngineerGuy1GT 0:5be0a1cce3f6 54 switch(input)
EngineerGuy1GT 0:5be0a1cce3f6 55 {
EngineerGuy1GT 0:5be0a1cce3f6 56 case 'f':
EngineerGuy1GT 0:5be0a1cce3f6 57 return 1.0;
EngineerGuy1GT 0:5be0a1cce3f6 58 case 'm':
EngineerGuy1GT 0:5be0a1cce3f6 59 return 0.66;
EngineerGuy1GT 0:5be0a1cce3f6 60 case 's':
EngineerGuy1GT 0:5be0a1cce3f6 61 return 0.33;
EngineerGuy1GT 0:5be0a1cce3f6 62 case 'n':
EngineerGuy1GT 0:5be0a1cce3f6 63 return 0;
EngineerGuy1GT 0:5be0a1cce3f6 64 case 'r':
EngineerGuy1GT 0:5be0a1cce3f6 65 return -0.33;
EngineerGuy1GT 0:5be0a1cce3f6 66 case 'l':
EngineerGuy1GT 0:5be0a1cce3f6 67 return -0.66;
EngineerGuy1GT 0:5be0a1cce3f6 68 case 'e':
EngineerGuy1GT 0:5be0a1cce3f6 69 return -1.0;
EngineerGuy1GT 0:5be0a1cce3f6 70 default:
EngineerGuy1GT 0:5be0a1cce3f6 71 return old_left;
EngineerGuy1GT 0:5be0a1cce3f6 72 }
EngineerGuy1GT 0:5be0a1cce3f6 73 }
EngineerGuy1GT 0:5be0a1cce3f6 74
EngineerGuy1GT 0:5be0a1cce3f6 75 float get_right_speed(char input)
EngineerGuy1GT 0:5be0a1cce3f6 76 {
EngineerGuy1GT 0:5be0a1cce3f6 77 switch(input)
EngineerGuy1GT 0:5be0a1cce3f6 78 {
EngineerGuy1GT 0:5be0a1cce3f6 79 case 'F':
EngineerGuy1GT 0:5be0a1cce3f6 80 return 1.0;
EngineerGuy1GT 0:5be0a1cce3f6 81 case 'M':
EngineerGuy1GT 0:5be0a1cce3f6 82 return 0.66;
EngineerGuy1GT 0:5be0a1cce3f6 83 case 'S':
EngineerGuy1GT 0:5be0a1cce3f6 84 return 0.33;
EngineerGuy1GT 0:5be0a1cce3f6 85 case 'N':
EngineerGuy1GT 0:5be0a1cce3f6 86 return 0;
EngineerGuy1GT 0:5be0a1cce3f6 87 case 'R':
EngineerGuy1GT 0:5be0a1cce3f6 88 return -0.33;
EngineerGuy1GT 0:5be0a1cce3f6 89 case 'L':
EngineerGuy1GT 0:5be0a1cce3f6 90 return -0.66;
EngineerGuy1GT 0:5be0a1cce3f6 91 case 'E':
EngineerGuy1GT 0:5be0a1cce3f6 92 return -1.0;
EngineerGuy1GT 0:5be0a1cce3f6 93 default:
EngineerGuy1GT 0:5be0a1cce3f6 94 return old_right;
EngineerGuy1GT 0:5be0a1cce3f6 95 }
EngineerGuy1GT 0:5be0a1cce3f6 96 }
EngineerGuy1GT 0:5be0a1cce3f6 97
EngineerGuy1GT 0:5be0a1cce3f6 98 void control_motors()
EngineerGuy1GT 0:5be0a1cce3f6 99 {
EngineerGuy1GT 0:5be0a1cce3f6 100 // reset the Zigbee (at least 200ns)
EngineerGuy1GT 0:5be0a1cce3f6 101 xbee_rst = 0;
EngineerGuy1GT 0:5be0a1cce3f6 102 wait_ms(1);
EngineerGuy1GT 0:5be0a1cce3f6 103 xbee_rst = 1;
EngineerGuy1GT 0:5be0a1cce3f6 104 wait_ms(1);
EngineerGuy1GT 0:5be0a1cce3f6 105 pc.printf("ZIGBEE COMMUNICATION SETUP COMPLETE\n\r");
EngineerGuy1GT 0:5be0a1cce3f6 106
EngineerGuy1GT 0:5be0a1cce3f6 107 while(1)
EngineerGuy1GT 0:5be0a1cce3f6 108 {
EngineerGuy1GT 0:5be0a1cce3f6 109 if(xbee.readable())
EngineerGuy1GT 0:5be0a1cce3f6 110 {
EngineerGuy1GT 0:5be0a1cce3f6 111 RX_char = xbee.getc();
EngineerGuy1GT 0:5be0a1cce3f6 112 //pc.printf("receiving... %c\n\r", RX_char);
EngineerGuy1GT 0:5be0a1cce3f6 113 pc.putc(RX_char);
EngineerGuy1GT 0:5be0a1cce3f6 114
EngineerGuy1GT 0:5be0a1cce3f6 115 left_speed = get_left_speed(RX_char);
EngineerGuy1GT 0:5be0a1cce3f6 116 old_left = left_speed;
EngineerGuy1GT 0:5be0a1cce3f6 117 right_speed = get_right_speed(RX_char);
EngineerGuy1GT 0:5be0a1cce3f6 118 old_right = right_speed;
EngineerGuy1GT 0:5be0a1cce3f6 119
EngineerGuy1GT 0:5be0a1cce3f6 120 //Signal and Brake Control
EngineerGuy1GT 0:5be0a1cce3f6 121 if((left_speed == 0) && (right_speed == 0))
EngineerGuy1GT 0:5be0a1cce3f6 122 {
EngineerGuy1GT 0:5be0a1cce3f6 123 brake_sig = 1;
EngineerGuy1GT 0:5be0a1cce3f6 124 left_turn_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 125 right_turn_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 126 }
EngineerGuy1GT 0:5be0a1cce3f6 127
EngineerGuy1GT 0:5be0a1cce3f6 128 else if(left_speed > right_speed)
EngineerGuy1GT 0:5be0a1cce3f6 129
EngineerGuy1GT 0:5be0a1cce3f6 130 {
EngineerGuy1GT 0:5be0a1cce3f6 131 brake_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 132 left_turn_sig = ! left_turn_sig;
EngineerGuy1GT 0:5be0a1cce3f6 133 right_turn_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 134 }
EngineerGuy1GT 0:5be0a1cce3f6 135
EngineerGuy1GT 0:5be0a1cce3f6 136 else if(left_speed < right_speed)
EngineerGuy1GT 0:5be0a1cce3f6 137 {
EngineerGuy1GT 0:5be0a1cce3f6 138 brake_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 139 right_turn_sig = ! right_turn_sig;
EngineerGuy1GT 0:5be0a1cce3f6 140 left_turn_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 141 }
EngineerGuy1GT 0:5be0a1cce3f6 142
EngineerGuy1GT 0:5be0a1cce3f6 143 else
EngineerGuy1GT 0:5be0a1cce3f6 144 {
EngineerGuy1GT 0:5be0a1cce3f6 145 brake_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 146 left_turn_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 147 right_turn_sig = 0;
EngineerGuy1GT 0:5be0a1cce3f6 148 }
EngineerGuy1GT 0:5be0a1cce3f6 149
EngineerGuy1GT 0:5be0a1cce3f6 150 motor_left.speed(left_speed);
EngineerGuy1GT 0:5be0a1cce3f6 151 motor_right.speed(right_speed);
EngineerGuy1GT 0:5be0a1cce3f6 152 }
EngineerGuy1GT 0:5be0a1cce3f6 153 }
EngineerGuy1GT 0:5be0a1cce3f6 154 }
EngineerGuy1GT 0:5be0a1cce3f6 155
EngineerGuy1GT 0:5be0a1cce3f6 156 void speaker()
EngineerGuy1GT 0:5be0a1cce3f6 157 {
EngineerGuy1GT 0:5be0a1cce3f6 158 //Mount the filesystem
EngineerGuy1GT 0:5be0a1cce3f6 159 //sd.mount();
EngineerGuy1GT 0:5be0a1cce3f6 160 //wave_file=fopen("/sd/wavfiles/police_siren.wav","r");
EngineerGuy1GT 0:5be0a1cce3f6 161 //waver.play(wave_file);
EngineerGuy1GT 0:5be0a1cce3f6 162 //Thread::wait(31000); //wait 31 seconds
EngineerGuy1GT 0:5be0a1cce3f6 163 while(1)
EngineerGuy1GT 0:5be0a1cce3f6 164 {
EngineerGuy1GT 0:5be0a1cce3f6 165 if (RX_char == 'w')
EngineerGuy1GT 0:5be0a1cce3f6 166 {
EngineerGuy1GT 0:5be0a1cce3f6 167 wave_file=fopen("/sd/wavfiles/police_siren.wav","r");
EngineerGuy1GT 0:5be0a1cce3f6 168 waver.play(wave_file);
EngineerGuy1GT 0:5be0a1cce3f6 169 fclose(wave_file);
EngineerGuy1GT 0:5be0a1cce3f6 170 Thread::wait(31000); //wait 31 seconds
EngineerGuy1GT 0:5be0a1cce3f6 171 }
EngineerGuy1GT 0:5be0a1cce3f6 172 Thread::wait(500); //wait 500ms
EngineerGuy1GT 0:5be0a1cce3f6 173 }
EngineerGuy1GT 0:5be0a1cce3f6 174 }
EngineerGuy1GT 0:5be0a1cce3f6 175
EngineerGuy1GT 0:5be0a1cce3f6 176
EngineerGuy1GT 0:5be0a1cce3f6 177 void RGB_LED_bar()
EngineerGuy1GT 0:5be0a1cce3f6 178 {
EngineerGuy1GT 0:5be0a1cce3f6 179 //Device Opcode Defined:
EngineerGuy1GT 0:5be0a1cce3f6 180 // 0 1 0 0 A2 A1 A0 0
EngineerGuy1GT 0:5be0a1cce3f6 181
EngineerGuy1GT 0:5be0a1cce3f6 182 //chipR --> Red
EngineerGuy1GT 0:5be0a1cce3f6 183 //chipG --> Green
EngineerGuy1GT 0:5be0a1cce3f6 184 //chipB --> Blue
EngineerGuy1GT 0:5be0a1cce3f6 185
EngineerGuy1GT 0:5be0a1cce3f6 186 //MCP23S17 ChipR
EngineerGuy1GT 0:5be0a1cce3f6 187 // A0, A1, A2 of MCP23S17 chip0 are tied to ground on the breadboard, so the 8-bit address for writes is 0x40
EngineerGuy1GT 0:5be0a1cce3f6 188 // This is referred to as the opcode in the device datasheet
EngineerGuy1GT 0:5be0a1cce3f6 189 //A2=0 A1=0 A0=0
EngineerGuy1GT 0:5be0a1cce3f6 190 char OpcodeR = 0x40;
EngineerGuy1GT 0:5be0a1cce3f6 191 // Next create a MCP23S17
EngineerGuy1GT 0:5be0a1cce3f6 192 // mbed p16 is connected to ~chipSelect on the MCP23S17
EngineerGuy1GT 0:5be0a1cce3f6 193 MCP23S17 chipR = MCP23S17(IO_spi, p16, OpcodeR);
EngineerGuy1GT 0:5be0a1cce3f6 194
EngineerGuy1GT 0:5be0a1cce3f6 195 //MCP23S17 ChipG
EngineerGuy1GT 0:5be0a1cce3f6 196 //A2=0 A1=0 A0=1
EngineerGuy1GT 0:5be0a1cce3f6 197 char OpcodeG = 0x42;
EngineerGuy1GT 0:5be0a1cce3f6 198 // Next create a MCP23S17
EngineerGuy1GT 0:5be0a1cce3f6 199 // mbed p17 is connected to ~chipSelect on the MCP23S17
EngineerGuy1GT 0:5be0a1cce3f6 200 MCP23S17 chipG = MCP23S17(IO_spi, p17, OpcodeG);
EngineerGuy1GT 0:5be0a1cce3f6 201
EngineerGuy1GT 0:5be0a1cce3f6 202 //MCP23S17 ChipB
EngineerGuy1GT 0:5be0a1cce3f6 203 //A2=0 A1=1 A0=0
EngineerGuy1GT 0:5be0a1cce3f6 204 char OpcodeB = 0x44;
EngineerGuy1GT 0:5be0a1cce3f6 205 // Next create a MCP23S17
EngineerGuy1GT 0:5be0a1cce3f6 206 // mbed p19 is connected to ~chipSelect on the MCP23S17
EngineerGuy1GT 0:5be0a1cce3f6 207 MCP23S17 chipB = MCP23S17(IO_spi, p19, OpcodeB);
EngineerGuy1GT 0:5be0a1cce3f6 208
EngineerGuy1GT 0:5be0a1cce3f6 209 IO_rst = 0;
EngineerGuy1GT 0:5be0a1cce3f6 210 wait_us(10);
EngineerGuy1GT 0:5be0a1cce3f6 211 IO_rst = 1;
EngineerGuy1GT 0:5be0a1cce3f6 212
EngineerGuy1GT 0:5be0a1cce3f6 213 //0x00 = 'input' 0xFF = 'output'
EngineerGuy1GT 0:5be0a1cce3f6 214 //Set all 8 Port A bits to output direction
EngineerGuy1GT 0:5be0a1cce3f6 215 chipR.direction(PORT_A, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 216 chipG.direction(PORT_A, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 217 chipB.direction(PORT_A, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 218 //Set all 8 Port B bits to output direction
EngineerGuy1GT 0:5be0a1cce3f6 219 chipR.direction(PORT_B, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 220 chipG.direction(PORT_B, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 221 chipB.direction(PORT_B, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 222
EngineerGuy1GT 0:5be0a1cce3f6 223 int init_wait = 500;
EngineerGuy1GT 0:5be0a1cce3f6 224 mutex.lock();
EngineerGuy1GT 0:5be0a1cce3f6 225 chipR.write(PORT_A, 0xFF);
EngineerGuy1GT 0:5be0a1cce3f6 226 chipR.write(PORT_B, 0xFF);
EngineerGuy1GT 0:5be0a1cce3f6 227 mutex.unlock();
EngineerGuy1GT 0:5be0a1cce3f6 228 Thread::wait(init_wait);
EngineerGuy1GT 0:5be0a1cce3f6 229 mutex.lock();
EngineerGuy1GT 0:5be0a1cce3f6 230 chipG.write(PORT_A, 0xFF);
EngineerGuy1GT 0:5be0a1cce3f6 231 chipG.write(PORT_B, 0xFF);
EngineerGuy1GT 0:5be0a1cce3f6 232 mutex.unlock();
EngineerGuy1GT 0:5be0a1cce3f6 233 Thread::wait(init_wait);
EngineerGuy1GT 0:5be0a1cce3f6 234 mutex.lock();
EngineerGuy1GT 0:5be0a1cce3f6 235 chipB.write(PORT_A, 0xFF);
EngineerGuy1GT 0:5be0a1cce3f6 236 chipB.write(PORT_B, 0xFF);
EngineerGuy1GT 0:5be0a1cce3f6 237 mutex.unlock();
EngineerGuy1GT 0:5be0a1cce3f6 238 Thread::wait(init_wait);
EngineerGuy1GT 0:5be0a1cce3f6 239 mutex.lock();
EngineerGuy1GT 0:5be0a1cce3f6 240 chipR.write(PORT_A, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 241 chipR.write(PORT_B, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 242 mutex.unlock();
EngineerGuy1GT 0:5be0a1cce3f6 243 Thread::wait(init_wait);
EngineerGuy1GT 0:5be0a1cce3f6 244 mutex.lock();
EngineerGuy1GT 0:5be0a1cce3f6 245 chipG.write(PORT_A, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 246 chipG.write(PORT_B, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 247 mutex.unlock();
EngineerGuy1GT 0:5be0a1cce3f6 248 Thread::wait(init_wait);
EngineerGuy1GT 0:5be0a1cce3f6 249 mutex.lock();
EngineerGuy1GT 0:5be0a1cce3f6 250 chipB.write(PORT_A, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 251 chipB.write(PORT_B, 0x00);
EngineerGuy1GT 0:5be0a1cce3f6 252 mutex.unlock();
EngineerGuy1GT 0:5be0a1cce3f6 253 Thread::wait(init_wait);
EngineerGuy1GT 0:5be0a1cce3f6 254
EngineerGuy1GT 0:5be0a1cce3f6 255 bool flag = 0;
EngineerGuy1GT 0:5be0a1cce3f6 256 volatile int redA_i = 0b10000000;
EngineerGuy1GT 0:5be0a1cce3f6 257 volatile int greenA_i = 0b10000000;
EngineerGuy1GT 0:5be0a1cce3f6 258 volatile int redB_i = 0b00000001;
EngineerGuy1GT 0:5be0a1cce3f6 259 volatile int greenB_i = 0b00000001;
EngineerGuy1GT 0:5be0a1cce3f6 260 volatile int redA;
EngineerGuy1GT 0:5be0a1cce3f6 261 volatile int redB;
EngineerGuy1GT 0:5be0a1cce3f6 262 volatile int greenA;
EngineerGuy1GT 0:5be0a1cce3f6 263 volatile int greenB;
EngineerGuy1GT 0:5be0a1cce3f6 264 volatile int blueA = 0b11111111;
EngineerGuy1GT 0:5be0a1cce3f6 265 volatile int blueB = 0b11111111;
EngineerGuy1GT 0:5be0a1cce3f6 266
EngineerGuy1GT 0:5be0a1cce3f6 267
EngineerGuy1GT 0:5be0a1cce3f6 268 while(1)
EngineerGuy1GT 0:5be0a1cce3f6 269 {
EngineerGuy1GT 0:5be0a1cce3f6 270 //2-PART CYCLON SWEEP (LED LIGHTING EFFECT)
EngineerGuy1GT 0:5be0a1cce3f6 271 mutex.lock();
EngineerGuy1GT 0:5be0a1cce3f6 272 if (flag == 0)
EngineerGuy1GT 0:5be0a1cce3f6 273 {
EngineerGuy1GT 0:5be0a1cce3f6 274 redA_i = redA_i>>1;
EngineerGuy1GT 0:5be0a1cce3f6 275 greenA_i = greenA_i>>1;
EngineerGuy1GT 0:5be0a1cce3f6 276 redB_i = redB_i<<1;
EngineerGuy1GT 0:5be0a1cce3f6 277 greenB_i = greenB_i<<1;
EngineerGuy1GT 0:5be0a1cce3f6 278 if (redA_i == 0b00000001)
EngineerGuy1GT 0:5be0a1cce3f6 279 {
EngineerGuy1GT 0:5be0a1cce3f6 280 flag = 1;
EngineerGuy1GT 0:5be0a1cce3f6 281 }
EngineerGuy1GT 0:5be0a1cce3f6 282 }
EngineerGuy1GT 0:5be0a1cce3f6 283 else if (flag == 1)
EngineerGuy1GT 0:5be0a1cce3f6 284 {
EngineerGuy1GT 0:5be0a1cce3f6 285 redA_i = redA_i<<1;
EngineerGuy1GT 0:5be0a1cce3f6 286 greenA_i = greenA_i<<1;
EngineerGuy1GT 0:5be0a1cce3f6 287 redB_i = redB_i>>1;
EngineerGuy1GT 0:5be0a1cce3f6 288 greenB_i = greenB_i>>1;
EngineerGuy1GT 0:5be0a1cce3f6 289 if (redA_i == 0b10000000)
EngineerGuy1GT 0:5be0a1cce3f6 290 {
EngineerGuy1GT 0:5be0a1cce3f6 291 flag = 0;
EngineerGuy1GT 0:5be0a1cce3f6 292 }
EngineerGuy1GT 0:5be0a1cce3f6 293 }
EngineerGuy1GT 0:5be0a1cce3f6 294 redA = redA_i ^ 0b11111111; //XOR
EngineerGuy1GT 0:5be0a1cce3f6 295 greenA = greenA_i ^ 0b11111111; //XOR
EngineerGuy1GT 0:5be0a1cce3f6 296 redB = redB_i ^ 0b11111111; //XOR
EngineerGuy1GT 0:5be0a1cce3f6 297 greenB = greenB_i ^ 0b11111111; //XOR
EngineerGuy1GT 0:5be0a1cce3f6 298 chipR.write(PORT_A, redA);
EngineerGuy1GT 0:5be0a1cce3f6 299 chipR.write(PORT_B, redB);
EngineerGuy1GT 0:5be0a1cce3f6 300 chipG.write(PORT_A, greenA);
EngineerGuy1GT 0:5be0a1cce3f6 301 chipG.write(PORT_B, greenB);
EngineerGuy1GT 0:5be0a1cce3f6 302 chipB.write(PORT_A, blueA);
EngineerGuy1GT 0:5be0a1cce3f6 303 chipB.write(PORT_B, blueB);
EngineerGuy1GT 0:5be0a1cce3f6 304 mutex.unlock();
EngineerGuy1GT 0:5be0a1cce3f6 305 Thread::wait(100);
EngineerGuy1GT 0:5be0a1cce3f6 306 }
EngineerGuy1GT 0:5be0a1cce3f6 307 }
EngineerGuy1GT 0:5be0a1cce3f6 308
EngineerGuy1GT 0:5be0a1cce3f6 309
EngineerGuy1GT 0:5be0a1cce3f6 310 int main()
EngineerGuy1GT 0:5be0a1cce3f6 311 {
EngineerGuy1GT 0:5be0a1cce3f6 312
EngineerGuy1GT 0:5be0a1cce3f6 313 // Start Threads
EngineerGuy1GT 0:5be0a1cce3f6 314 motor_thread.start(control_motors);
EngineerGuy1GT 0:5be0a1cce3f6 315 speaker_thread.start(speaker);
EngineerGuy1GT 0:5be0a1cce3f6 316 RGB_LED_bar_thread.start(RGB_LED_bar);
EngineerGuy1GT 0:5be0a1cce3f6 317
EngineerGuy1GT 0:5be0a1cce3f6 318 }