157 lines
3.2 KiB
C++
157 lines
3.2 KiB
C++
//This example code is in the Public Domain (or CC0 licensed, at your option.)
|
|
//By Evandro Copercini - 2018
|
|
//
|
|
//This example creates a bridge between Serial and Classical Bluetooth (SPP)
|
|
//and also demonstrate that SerialBT have the same functionalities of a normal Serial
|
|
|
|
#include "BluetoothSerial.h"
|
|
#include <CodeRacer.h>
|
|
|
|
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
|
|
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
|
|
#endif
|
|
|
|
BluetoothSerial SerialBT;
|
|
bool motioncontrol = false;
|
|
|
|
String recvddata = "";
|
|
int init_azimuth = 1000;
|
|
int azimuth = 0;
|
|
|
|
int minroll = 20;
|
|
int roll = 20;
|
|
int maxroll = 90;
|
|
int minpitch = 10;
|
|
int pitch = 0;
|
|
int maxpitch = 90;
|
|
|
|
int defaultleft = 200;
|
|
int left = 0;
|
|
int defaultright = 200;
|
|
int right = 0;
|
|
|
|
CodeRacer coderacer;
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
SerialBT.begin("ESP32test"); //Bluetooth device name
|
|
Serial.println("The device started, now you can pair it with bluetooth!");
|
|
coderacer.begin();
|
|
}
|
|
|
|
|
|
|
|
void loop() {
|
|
if (Serial.available()) {
|
|
SerialBT.print(Serial.readString());
|
|
}
|
|
if (SerialBT.available()) {
|
|
Serial.println("New incoming message");
|
|
SerialBT.setTimeout(10);
|
|
recvddata = SerialBT.readStringUntil('\0');
|
|
recvddata.remove(recvddata.length()-2,2); //remove delimiter from string
|
|
Serial.println(recvddata);
|
|
BTcontrol();
|
|
}
|
|
delay(20);
|
|
}
|
|
|
|
void BTcontrol()
|
|
{
|
|
|
|
|
|
if(recvddata.startsWith("motion_on"))
|
|
{
|
|
SerialBT.print("motion_ready");
|
|
init_azimuth = 1000;
|
|
motioncontrol = true;
|
|
return;
|
|
}
|
|
|
|
if(recvddata.startsWith("motion_off"))
|
|
{
|
|
SerialBT.print("motion_disabled");
|
|
motioncontrol = false;
|
|
coderacer.stop_driving();
|
|
return;
|
|
}
|
|
|
|
if(recvddata.startsWith("apr="))
|
|
{
|
|
if(motioncontrol == true)
|
|
{
|
|
SerialBT.print(BTmotioncontrol(recvddata));
|
|
}
|
|
else
|
|
{
|
|
SerialBT.print("motion_disabled");
|
|
}
|
|
}
|
|
|
|
|
|
}
|
|
|
|
String BTmotioncontrol(String recvddata)
|
|
{
|
|
// split the string ...
|
|
String apr_substr=recvddata.substring(recvddata.indexOf("=")+1);
|
|
String a = apr_substr.substring(0,apr_substr.indexOf(";"));
|
|
String p = apr_substr.substring(apr_substr.indexOf(";")+1,apr_substr.lastIndexOf(";"));
|
|
String r = apr_substr.substring(apr_substr.lastIndexOf(";")+1);
|
|
|
|
azimuth = a.toInt();
|
|
pitch = p.toInt();
|
|
roll = r.toInt();
|
|
|
|
if(init_azimuth == 1000)
|
|
{
|
|
init_azimuth = azimuth;
|
|
}
|
|
|
|
azimuth = azimuth - init_azimuth;
|
|
if(abs(pitch)<minpitch){
|
|
pitch = 0;
|
|
coderacer.stop_driving();
|
|
}
|
|
else
|
|
{
|
|
pitch = pitch;
|
|
|
|
|
|
if(pitch > 0)
|
|
{
|
|
left = defaultleft+pitch;
|
|
if(left > 256) left = 256;
|
|
right = defaultright+pitch;
|
|
if(right > 256) right = 256;
|
|
coderacer.drive_forward(left,right);
|
|
}
|
|
else
|
|
{
|
|
left = defaultleft-pitch;
|
|
if(left > 256) left = 256;
|
|
right = defaultright-pitch;
|
|
if(right > 256) right = 256;
|
|
coderacer.drive_backward(left,right);
|
|
}
|
|
}
|
|
|
|
|
|
if(abs(roll)<minroll){
|
|
roll = 0;
|
|
}
|
|
/*
|
|
int defaultleft = 240;
|
|
int left = 0;
|
|
int defaultright = 240;
|
|
int right = 0;
|
|
*/
|
|
|
|
Serial.println(azimuth);
|
|
Serial.println(pitch);
|
|
Serial.println(roll);
|
|
|
|
return "motion_values_ok";
|
|
}
|
|
|