library/Arduino/BTCoderacer/BTCoderacer.ino
2018-12-03 16:12:07 +01:00

156 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";
}