FIRST Robotics Purdue Polytechnic
/
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
Diff: main.cpp
- Revision:
- 0:500360bf256b
- Child:
- 1:b07198832d0d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Mar 14 20:22:02 2016 +0000
@@ -0,0 +1,241 @@
+#include "mbed.h"
+#include "Motor.h"
+#include "rtos.h"
+#include "wave_player.h"
+#include "SDFileSystem.h"
+#include "PowerControl/PowerControl.h"
+#include "PowerControl/EthernetPowerControl.h"
+
+Serial blue(p13,p14);
+Motor m1(p25, p26, p27); // pwm, fwd, rev
+Motor m2(p24, p30, p29); // pwm, fwd, rev
+AnalogOut DACout(p18);
+wave_player waver(&DACout);
+AnalogIn distance(p20);
+SDFileSystem sd(p5, p6, p7, p8, "sd");
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+
+float past_values[3];
+int past_index = 0;
+
+int warningCounter = 0;
+int clearCounter = 0;
+int warning_flag = 0;
+
+
+#define USR_POWERDOWN (0x104)
+int semihost_powerdown()
+{
+ uint32_t arg;
+ return __semihost(USR_POWERDOWN, &arg);
+}
+
+void bluetooth_thread(void const *args)
+{
+ while(1) {
+ char bnum=0;
+ char bhit=0;
+ if (blue.getc()=='!') {
+ if (blue.getc()=='B') { //button data packet
+ bnum = blue.getc(); //button number
+ bhit = blue.getc(); //1=hit, 0=release
+ if (blue.getc()==char(~('!' + 'B' + bnum + bhit))) { //checksum OK?
+ switch (bnum) {
+ case '1': //number button 1
+ if (bhit=='1') {
+ //add hit code here
+ } else {
+ //add release code here
+ }
+ break;
+ case '2': //number button 2
+ if (bhit=='1') {
+ //add hit code here
+ } else {
+ //add release code here
+ }
+ break;
+ case '3': //number button 3
+ if (bhit=='1') {
+ //add hit code here
+ } else {
+ //add release code here
+ }
+ break;
+ case '4': //number button 4
+ if (bhit=='1') {
+ //add hit code here
+ } else {
+ //add release code here
+ }
+ break;
+ case '5': //button 5 up arrow
+ if (bhit=='1') {
+ if (warning_flag != 1){
+ m1.speed(0.5);
+ m2.speed(0.55);
+ } else {
+ m1.speed(0.0);
+ m2.speed(0.0);
+ }
+
+ } else {
+ m1.speed(0.0);
+ m2.speed(0.0);
+ //add release code here
+ }
+ break;
+ case '6': //button 6 down arrow
+ if (bhit=='1') {
+ //add hit code here
+ m1.speed(-0.5);
+ m2.speed(-0.54);
+ } else {
+ m1.speed(0.0);
+ m2.speed(0.0);
+ //add release code here
+ }
+ break;
+ case '7': //button 7 left arrow
+ if (bhit=='1') {
+ m1.speed(0.5);
+ m2.speed(-0.54);
+ //add hit code here
+ } else {
+ m1.speed(0.0);
+ m2.speed(0.0);
+ //add release code here
+ }
+ break;
+ case '8': //button 8 right arrow
+ if (bhit=='1') {
+ //add hit code here
+ m1.speed(-0.5);
+ m2.speed(0.5);
+ } else {
+ m1.speed(0.0);
+ m2.speed(0.0);
+ //add release code here
+ }
+ break;
+ default:
+ break;
+ }
+ }
+ }
+ }
+ Thread::wait(200);
+ }
+}
+
+
+
+
+
+void audio_thread(void const *args)
+{
+
+
+ while(1) {
+
+
+// if (distance > 0.8f && warningFlag == 0) {
+// warningCounter += 1;
+// } else if (warningFlag == 1 && distance < 0.8f) {
+// clearCounter +=1;
+// }
+//
+// if (warningCounter >= 3) {
+// warningFlag = 1;
+// warningCounter = 0;
+// } else if (clearCounter >= 3) {
+// clearCounter = 0;
+// warningFlag = 0;
+// }
+//
+ if (warning_flag == 1) {
+ myled2 = 1;
+ FILE *wave_file;
+
+ wave_file=fopen("/sd/Warning_Beep.wav","r");
+ wait(0.001);
+
+ if (wave_file == NULL) {
+ myled1 = 1;
+ } else {
+ myled1 = 0;
+ waver.play(wave_file);
+ fclose(wave_file);
+ }
+ } else {
+ myled2 = 0;
+ }
+ Thread::wait(10);
+
+ }
+
+}
+
+
+int main()
+{
+
+
+ PHY_PowerDown();
+ int result = semihost_powerdown();
+ Thread thread1(bluetooth_thread);
+ Thread thread2(audio_thread);
+
+ //thread2.set_priority(osPriorityRealtime);
+ //thread1.set_priority(osPriorityNormal);
+
+
+ past_values[0] = 0;
+ past_values[1] = 0;
+ past_values[2] = 0;
+
+ warning_flag = 0;
+
+ while(1) {
+ past_values[past_index] = distance;
+ past_index = (past_index + 1) % 3;
+
+ //Running average
+ float running_sum = 0.0;
+ for (int i = 0; i < 3; i++){
+ running_sum+=past_values[i];
+ }
+ float average = running_sum/3.0;
+
+ //Hysteresis
+ if (average >= 0.6f && warning_flag == 0){
+ warning_flag = 1;
+ //myled2 = 1;
+
+ } else if (average <= 0.4f && warning_flag == 1){
+ warning_flag = 0;
+ //myled2 = 0;
+ }
+
+// //Set the warning state
+// if (warning_flag == 1){
+// myled2 = 1;
+// } else {
+// myled2 = 0;
+// }
+
+ if (distance > 0.6f && average > 0.6f){
+ myled4 = 1;
+ } else {
+ myled4 = 0;
+ }
+
+ Thread::wait(10);
+ }
+
+}
+