FIRST Robotics Purdue Polytechnic / Mbed 2 deprecated Bluetooth_Controlle_Robot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed mbed-rtos Motor PowerControl

main.cpp@0:500360bf256b, 2016-03-14 (annotated)

Committer:
nbharwani3
Date:
Mon Mar 14 20:22:02 2016 +0000
Revision:
0:500360bf256b
Child:
1:b07198832d0d
Lab 4 Code - Nashwin Bharwani, Vasundhara Rawat

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nbharwani3 0:500360bf256b 1 #include "mbed.h"
nbharwani3 0:500360bf256b 2 #include "Motor.h"
nbharwani3 0:500360bf256b 3 #include "rtos.h"
nbharwani3 0:500360bf256b 4 #include "wave_player.h"
nbharwani3 0:500360bf256b 5 #include "SDFileSystem.h"
nbharwani3 0:500360bf256b 6 #include "PowerControl/PowerControl.h"
nbharwani3 0:500360bf256b 7 #include "PowerControl/EthernetPowerControl.h"
nbharwani3 0:500360bf256b 8
nbharwani3 0:500360bf256b 9 Serial blue(p13,p14);
nbharwani3 0:500360bf256b 10 Motor m1(p25, p26, p27); // pwm, fwd, rev
nbharwani3 0:500360bf256b 11 Motor m2(p24, p30, p29); // pwm, fwd, rev
nbharwani3 0:500360bf256b 12 AnalogOut DACout(p18);
nbharwani3 0:500360bf256b 13 wave_player waver(&DACout);
nbharwani3 0:500360bf256b 14 AnalogIn distance(p20);
nbharwani3 0:500360bf256b 15 SDFileSystem sd(p5, p6, p7, p8, "sd");
nbharwani3 0:500360bf256b 16 DigitalOut myled1(LED1);
nbharwani3 0:500360bf256b 17 DigitalOut myled2(LED2);
nbharwani3 0:500360bf256b 18 DigitalOut myled3(LED3);
nbharwani3 0:500360bf256b 19 DigitalOut myled4(LED4);
nbharwani3 0:500360bf256b 20
nbharwani3 0:500360bf256b 21
nbharwani3 0:500360bf256b 22 float past_values[3];
nbharwani3 0:500360bf256b 23 int past_index = 0;
nbharwani3 0:500360bf256b 24
nbharwani3 0:500360bf256b 25 int warningCounter = 0;
nbharwani3 0:500360bf256b 26 int clearCounter = 0;
nbharwani3 0:500360bf256b 27 int warning_flag = 0;
nbharwani3 0:500360bf256b 28
nbharwani3 0:500360bf256b 29
nbharwani3 0:500360bf256b 30 #define USR_POWERDOWN (0x104)
nbharwani3 0:500360bf256b 31 int semihost_powerdown()
nbharwani3 0:500360bf256b 32 {
nbharwani3 0:500360bf256b 33 uint32_t arg;
nbharwani3 0:500360bf256b 34 return __semihost(USR_POWERDOWN, &arg);
nbharwani3 0:500360bf256b 35 }
nbharwani3 0:500360bf256b 36
nbharwani3 0:500360bf256b 37 void bluetooth_thread(void const *args)
nbharwani3 0:500360bf256b 38 {
nbharwani3 0:500360bf256b 39 while(1) {
nbharwani3 0:500360bf256b 40 char bnum=0;
nbharwani3 0:500360bf256b 41 char bhit=0;
nbharwani3 0:500360bf256b 42 if (blue.getc()=='!') {
nbharwani3 0:500360bf256b 43 if (blue.getc()=='B') { //button data packet
nbharwani3 0:500360bf256b 44 bnum = blue.getc(); //button number
nbharwani3 0:500360bf256b 45 bhit = blue.getc(); //1=hit, 0=release
nbharwani3 0:500360bf256b 46 if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
nbharwani3 0:500360bf256b 47 switch (bnum) {
nbharwani3 0:500360bf256b 48 case '1': //number button 1
nbharwani3 0:500360bf256b 49 if (bhit=='1') {
nbharwani3 0:500360bf256b 50 //add hit code here
nbharwani3 0:500360bf256b 51 } else {
nbharwani3 0:500360bf256b 52 //add release code here
nbharwani3 0:500360bf256b 53 }
nbharwani3 0:500360bf256b 54 break;
nbharwani3 0:500360bf256b 55 case '2': //number button 2
nbharwani3 0:500360bf256b 56 if (bhit=='1') {
nbharwani3 0:500360bf256b 57 //add hit code here
nbharwani3 0:500360bf256b 58 } else {
nbharwani3 0:500360bf256b 59 //add release code here
nbharwani3 0:500360bf256b 60 }
nbharwani3 0:500360bf256b 61 break;
nbharwani3 0:500360bf256b 62 case '3': //number button 3
nbharwani3 0:500360bf256b 63 if (bhit=='1') {
nbharwani3 0:500360bf256b 64 //add hit code here
nbharwani3 0:500360bf256b 65 } else {
nbharwani3 0:500360bf256b 66 //add release code here
nbharwani3 0:500360bf256b 67 }
nbharwani3 0:500360bf256b 68 break;
nbharwani3 0:500360bf256b 69 case '4': //number button 4
nbharwani3 0:500360bf256b 70 if (bhit=='1') {
nbharwani3 0:500360bf256b 71 //add hit code here
nbharwani3 0:500360bf256b 72 } else {
nbharwani3 0:500360bf256b 73 //add release code here
nbharwani3 0:500360bf256b 74 }
nbharwani3 0:500360bf256b 75 break;
nbharwani3 0:500360bf256b 76 case '5': //button 5 up arrow
nbharwani3 0:500360bf256b 77 if (bhit=='1') {
nbharwani3 0:500360bf256b 78 if (warning_flag != 1){
nbharwani3 0:500360bf256b 79 m1.speed(0.5);
nbharwani3 0:500360bf256b 80 m2.speed(0.55);
nbharwani3 0:500360bf256b 81 } else {
nbharwani3 0:500360bf256b 82 m1.speed(0.0);
nbharwani3 0:500360bf256b 83 m2.speed(0.0);
nbharwani3 0:500360bf256b 84 }
nbharwani3 0:500360bf256b 85
nbharwani3 0:500360bf256b 86 } else {
nbharwani3 0:500360bf256b 87 m1.speed(0.0);
nbharwani3 0:500360bf256b 88 m2.speed(0.0);
nbharwani3 0:500360bf256b 89 //add release code here
nbharwani3 0:500360bf256b 90 }
nbharwani3 0:500360bf256b 91 break;
nbharwani3 0:500360bf256b 92 case '6': //button 6 down arrow
nbharwani3 0:500360bf256b 93 if (bhit=='1') {
nbharwani3 0:500360bf256b 94 //add hit code here
nbharwani3 0:500360bf256b 95 m1.speed(-0.5);
nbharwani3 0:500360bf256b 96 m2.speed(-0.54);
nbharwani3 0:500360bf256b 97 } else {
nbharwani3 0:500360bf256b 98 m1.speed(0.0);
nbharwani3 0:500360bf256b 99 m2.speed(0.0);
nbharwani3 0:500360bf256b 100 //add release code here
nbharwani3 0:500360bf256b 101 }
nbharwani3 0:500360bf256b 102 break;
nbharwani3 0:500360bf256b 103 case '7': //button 7 left arrow
nbharwani3 0:500360bf256b 104 if (bhit=='1') {
nbharwani3 0:500360bf256b 105 m1.speed(0.5);
nbharwani3 0:500360bf256b 106 m2.speed(-0.54);
nbharwani3 0:500360bf256b 107 //add hit code here
nbharwani3 0:500360bf256b 108 } else {
nbharwani3 0:500360bf256b 109 m1.speed(0.0);
nbharwani3 0:500360bf256b 110 m2.speed(0.0);
nbharwani3 0:500360bf256b 111 //add release code here
nbharwani3 0:500360bf256b 112 }
nbharwani3 0:500360bf256b 113 break;
nbharwani3 0:500360bf256b 114 case '8': //button 8 right arrow
nbharwani3 0:500360bf256b 115 if (bhit=='1') {
nbharwani3 0:500360bf256b 116 //add hit code here
nbharwani3 0:500360bf256b 117 m1.speed(-0.5);
nbharwani3 0:500360bf256b 118 m2.speed(0.5);
nbharwani3 0:500360bf256b 119 } else {
nbharwani3 0:500360bf256b 120 m1.speed(0.0);
nbharwani3 0:500360bf256b 121 m2.speed(0.0);
nbharwani3 0:500360bf256b 122 //add release code here
nbharwani3 0:500360bf256b 123 }
nbharwani3 0:500360bf256b 124 break;
nbharwani3 0:500360bf256b 125 default:
nbharwani3 0:500360bf256b 126 break;
nbharwani3 0:500360bf256b 127 }
nbharwani3 0:500360bf256b 128 }
nbharwani3 0:500360bf256b 129 }
nbharwani3 0:500360bf256b 130 }
nbharwani3 0:500360bf256b 131 Thread::wait(200);
nbharwani3 0:500360bf256b 132 }
nbharwani3 0:500360bf256b 133 }
nbharwani3 0:500360bf256b 134
nbharwani3 0:500360bf256b 135
nbharwani3 0:500360bf256b 136
nbharwani3 0:500360bf256b 137
nbharwani3 0:500360bf256b 138
nbharwani3 0:500360bf256b 139 void audio_thread(void const *args)
nbharwani3 0:500360bf256b 140 {
nbharwani3 0:500360bf256b 141
nbharwani3 0:500360bf256b 142
nbharwani3 0:500360bf256b 143 while(1) {
nbharwani3 0:500360bf256b 144
nbharwani3 0:500360bf256b 145
nbharwani3 0:500360bf256b 146 // if (distance > 0.8f && warningFlag == 0) {
nbharwani3 0:500360bf256b 147 // warningCounter += 1;
nbharwani3 0:500360bf256b 148 // } else if (warningFlag == 1 && distance < 0.8f) {
nbharwani3 0:500360bf256b 149 // clearCounter +=1;
nbharwani3 0:500360bf256b 150 // }
nbharwani3 0:500360bf256b 151 //
nbharwani3 0:500360bf256b 152 // if (warningCounter >= 3) {
nbharwani3 0:500360bf256b 153 // warningFlag = 1;
nbharwani3 0:500360bf256b 154 // warningCounter = 0;
nbharwani3 0:500360bf256b 155 // } else if (clearCounter >= 3) {
nbharwani3 0:500360bf256b 156 // clearCounter = 0;
nbharwani3 0:500360bf256b 157 // warningFlag = 0;
nbharwani3 0:500360bf256b 158 // }
nbharwani3 0:500360bf256b 159 //
nbharwani3 0:500360bf256b 160 if (warning_flag == 1) {
nbharwani3 0:500360bf256b 161 myled2 = 1;
nbharwani3 0:500360bf256b 162 FILE *wave_file;
nbharwani3 0:500360bf256b 163
nbharwani3 0:500360bf256b 164 wave_file=fopen("/sd/Warning_Beep.wav","r");
nbharwani3 0:500360bf256b 165 wait(0.001);
nbharwani3 0:500360bf256b 166
nbharwani3 0:500360bf256b 167 if (wave_file == NULL) {
nbharwani3 0:500360bf256b 168 myled1 = 1;
nbharwani3 0:500360bf256b 169 } else {
nbharwani3 0:500360bf256b 170 myled1 = 0;
nbharwani3 0:500360bf256b 171 waver.play(wave_file);
nbharwani3 0:500360bf256b 172 fclose(wave_file);
nbharwani3 0:500360bf256b 173 }
nbharwani3 0:500360bf256b 174 } else {
nbharwani3 0:500360bf256b 175 myled2 = 0;
nbharwani3 0:500360bf256b 176 }
nbharwani3 0:500360bf256b 177 Thread::wait(10);
nbharwani3 0:500360bf256b 178
nbharwani3 0:500360bf256b 179 }
nbharwani3 0:500360bf256b 180
nbharwani3 0:500360bf256b 181 }
nbharwani3 0:500360bf256b 182
nbharwani3 0:500360bf256b 183
nbharwani3 0:500360bf256b 184 int main()
nbharwani3 0:500360bf256b 185 {
nbharwani3 0:500360bf256b 186
nbharwani3 0:500360bf256b 187
nbharwani3 0:500360bf256b 188 PHY_PowerDown();
nbharwani3 0:500360bf256b 189 int result = semihost_powerdown();
nbharwani3 0:500360bf256b 190 Thread thread1(bluetooth_thread);
nbharwani3 0:500360bf256b 191 Thread thread2(audio_thread);
nbharwani3 0:500360bf256b 192
nbharwani3 0:500360bf256b 193 //thread2.set_priority(osPriorityRealtime);
nbharwani3 0:500360bf256b 194 //thread1.set_priority(osPriorityNormal);
nbharwani3 0:500360bf256b 195
nbharwani3 0:500360bf256b 196
nbharwani3 0:500360bf256b 197 past_values[0] = 0;
nbharwani3 0:500360bf256b 198 past_values[1] = 0;
nbharwani3 0:500360bf256b 199 past_values[2] = 0;
nbharwani3 0:500360bf256b 200
nbharwani3 0:500360bf256b 201 warning_flag = 0;
nbharwani3 0:500360bf256b 202
nbharwani3 0:500360bf256b 203 while(1) {
nbharwani3 0:500360bf256b 204 past_values[past_index] = distance;
nbharwani3 0:500360bf256b 205 past_index = (past_index + 1) % 3;
nbharwani3 0:500360bf256b 206
nbharwani3 0:500360bf256b 207 //Running average
nbharwani3 0:500360bf256b 208 float running_sum = 0.0;
nbharwani3 0:500360bf256b 209 for (int i = 0; i < 3; i++){
nbharwani3 0:500360bf256b 210 running_sum+=past_values[i];
nbharwani3 0:500360bf256b 211 }
nbharwani3 0:500360bf256b 212 float average = running_sum/3.0;
nbharwani3 0:500360bf256b 213
nbharwani3 0:500360bf256b 214 //Hysteresis
nbharwani3 0:500360bf256b 215 if (average >= 0.6f && warning_flag == 0){
nbharwani3 0:500360bf256b 216 warning_flag = 1;
nbharwani3 0:500360bf256b 217 //myled2 = 1;
nbharwani3 0:500360bf256b 218
nbharwani3 0:500360bf256b 219 } else if (average <= 0.4f && warning_flag == 1){
nbharwani3 0:500360bf256b 220 warning_flag = 0;
nbharwani3 0:500360bf256b 221 //myled2 = 0;
nbharwani3 0:500360bf256b 222 }
nbharwani3 0:500360bf256b 223
nbharwani3 0:500360bf256b 224 // //Set the warning state
nbharwani3 0:500360bf256b 225 // if (warning_flag == 1){
nbharwani3 0:500360bf256b 226 // myled2 = 1;
nbharwani3 0:500360bf256b 227 // } else {
nbharwani3 0:500360bf256b 228 // myled2 = 0;
nbharwani3 0:500360bf256b 229 // }
nbharwani3 0:500360bf256b 230
nbharwani3 0:500360bf256b 231 if (distance > 0.6f && average > 0.6f){
nbharwani3 0:500360bf256b 232 myled4 = 1;
nbharwani3 0:500360bf256b 233 } else {
nbharwani3 0:500360bf256b 234 myled4 = 0;
nbharwani3 0:500360bf256b 235 }
nbharwani3 0:500360bf256b 236
nbharwani3 0:500360bf256b 237 Thread::wait(10);
nbharwani3 0:500360bf256b 238 }
nbharwani3 0:500360bf256b 239
nbharwani3 0:500360bf256b 240 }
nbharwani3 0:500360bf256b 241