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
main.cpp
- Committer:
- nbharwani3
- Date:
- 2016-03-14
- Revision:
- 0:500360bf256b
- Child:
- 1:b07198832d0d
File content as of revision 0:500360bf256b:
#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);
}
}