288 lines
7.9 KiB
Arduino
288 lines
7.9 KiB
Arduino
|
#include <Adafruit_DS3502.h>
|
||
|
|
#include <Adafruit_ADS1015.h>
|
||
|
|
#include <BLEDevice.h>
|
||
|
|
#include <BLEServer.h>
|
||
|
|
#include <BLEUtils.h>
|
||
|
|
#include <BLE2902.h>
|
||
|
|
|
||
|
|
Adafruit_DS3502 ds3502 = Adafruit_DS3502();
|
||
|
|
Adafruit_ADS1115 ads; // Construct an ads1015 at the default address: 0x48
|
||
|
|
|
||
|
|
/* For this example, make the following connections:
|
||
|
|
* DS3502 RH to 5V
|
||
|
|
* DS3502 RL to GND
|
||
|
|
* DS3502 RW to the pin specified by WIPER_VALUE_PIN
|
||
|
|
*/
|
||
|
|
|
||
|
|
BLEServer *pServer = NULL;
|
||
|
|
BLECharacteristic * pTxCharacteristic;
|
||
|
|
bool deviceConnected = false;
|
||
|
|
bool oldDeviceConnected = false;
|
||
|
|
uint8_t txValue = 0;
|
||
|
|
|
||
|
|
// See the following for generating UUIDs:
|
||
|
|
// https://www.uuidgenerator.net/
|
||
|
|
|
||
|
|
#define SERVICE_UUID "6E400001-B5A3-F393-E0A9-E50E24DCCA9E" // UART service UUID
|
||
|
|
#define CHARACTERISTIC_UUID_RX "6E400002-B5A3-F393-E0A9-E50E24DCCA9E"
|
||
|
|
#define CHARACTERISTIC_UUID_TX "6E400003-B5A3-F393-E0A9-E50E24DCCA9E"
|
||
|
|
|
||
|
|
class MyServerCallbacks: public BLEServerCallbacks {
|
||
|
|
void onConnect(BLEServer* pServer) {
|
||
|
|
deviceConnected = true;
|
||
|
|
};
|
||
|
|
|
||
|
|
void onDisconnect(BLEServer* pServer) {
|
||
|
|
deviceConnected = false;
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
String inputString;
|
||
|
|
bool data_ok = false;
|
||
|
|
|
||
|
|
class MyCallbacks: public BLECharacteristicCallbacks {
|
||
|
|
void onWrite(BLECharacteristic *pCharacteristic) {
|
||
|
|
std::string rxValue = pCharacteristic->getValue();
|
||
|
|
|
||
|
|
if (rxValue.length() > 0) {
|
||
|
|
Serial.println("*********");
|
||
|
|
Serial.print("Received Value: ");
|
||
|
|
inputString = String();
|
||
|
|
for (int i = 0; i < rxValue.length(); i++)
|
||
|
|
{
|
||
|
|
inputString += rxValue[i];
|
||
|
|
}
|
||
|
|
data_ok = true;
|
||
|
|
//Serial.print(rxValue[i]);
|
||
|
|
Serial.print(inputString);
|
||
|
|
Serial.println();
|
||
|
|
Serial.println("*********");
|
||
|
|
}
|
||
|
|
}
|
||
|
|
};
|
||
|
|
|
||
|
|
void ble_ini()
|
||
|
|
{
|
||
|
|
// Create the BLE Device
|
||
|
|
BLEDevice::init("Looper Service");
|
||
|
|
|
||
|
|
// Create the BLE Server
|
||
|
|
pServer = BLEDevice::createServer();
|
||
|
|
pServer->setCallbacks(new MyServerCallbacks());
|
||
|
|
|
||
|
|
// Create the BLE Service
|
||
|
|
BLEService *pService = pServer->createService(SERVICE_UUID);
|
||
|
|
|
||
|
|
// Create a BLE Characteristic
|
||
|
|
pTxCharacteristic = pService->createCharacteristic(
|
||
|
|
CHARACTERISTIC_UUID_TX,
|
||
|
|
BLECharacteristic::PROPERTY_NOTIFY
|
||
|
|
);
|
||
|
|
|
||
|
|
pTxCharacteristic->addDescriptor(new BLE2902());
|
||
|
|
|
||
|
|
BLECharacteristic * pRxCharacteristic = pService->createCharacteristic(
|
||
|
|
CHARACTERISTIC_UUID_RX,
|
||
|
|
BLECharacteristic::PROPERTY_WRITE
|
||
|
|
);
|
||
|
|
|
||
|
|
pRxCharacteristic->setCallbacks(new MyCallbacks());
|
||
|
|
|
||
|
|
// Start the service
|
||
|
|
pService->start();
|
||
|
|
|
||
|
|
// Start advertising
|
||
|
|
pServer->getAdvertising()->start();
|
||
|
|
Serial.println("Waiting a client connection to notify...");
|
||
|
|
}
|
||
|
|
|
||
|
|
#define DIR 27
|
||
|
|
#define PWM 13
|
||
|
|
#define LIMIT_FRONT 100
|
||
|
|
|
||
|
|
uint8_t pot_value = 63;
|
||
|
|
bool auto_mode = false;
|
||
|
|
|
||
|
|
// the setup routine runs once when you press reset:
|
||
|
|
void setup() {
|
||
|
|
// initialize the digital pin as an output.
|
||
|
|
pinMode(DIR, OUTPUT);
|
||
|
|
ledcSetup(0, 5000, 8);
|
||
|
|
ledcAttachPin(PWM, 0);
|
||
|
|
stop_giro();
|
||
|
|
Serial.begin(115200);
|
||
|
|
Serial1.begin(9600);
|
||
|
|
ble_ini();
|
||
|
|
Serial.println("Adafruit DS3502 Test");
|
||
|
|
|
||
|
|
if (!ds3502.begin()) {
|
||
|
|
Serial.println("Couldn't find DS3502 chip");
|
||
|
|
while (1);
|
||
|
|
}
|
||
|
|
// ads.setGain(GAIN_TWOTHIRDS); // 2/3x gain +/- 6.144V 1 bit = 3mV 0.1875mV (default)
|
||
|
|
// ads.setGain(GAIN_ONE); // 1x gain +/- 4.096V 1 bit = 2mV 0.125mV
|
||
|
|
// ads.setGain(GAIN_TWO); // 2x gain +/- 2.048V 1 bit = 1mV 0.0625mV
|
||
|
|
// ads.setGain(GAIN_FOUR); // 4x gain +/- 1.024V 1 bit = 0.5mV 0.03125mV
|
||
|
|
// ads.setGain(GAIN_EIGHT); // 8x gain +/- 0.512V 1 bit = 0.25mV 0.015625mV
|
||
|
|
// ads.setGain(GAIN_SIXTEEN); // 16x gain +/- 0.256V 1 bit = 0.125mV 0.0078125mV
|
||
|
|
ads.begin();
|
||
|
|
ds3502.setWiper(pot_value);
|
||
|
|
}
|
||
|
|
|
||
|
|
// the loop routine runs over and over again forever:
|
||
|
|
bool flag_auto = false;
|
||
|
|
|
||
|
|
void loop() {
|
||
|
|
if (!deviceConnected && oldDeviceConnected) {
|
||
|
|
delay(500); // give the bluetooth stack the chance to get things ready
|
||
|
|
pServer->startAdvertising(); // restart advertising
|
||
|
|
Serial.println("start advertising");
|
||
|
|
oldDeviceConnected = deviceConnected;
|
||
|
|
}
|
||
|
|
// connecting
|
||
|
|
if (deviceConnected && !oldDeviceConnected) {
|
||
|
|
// do stuff here on connecting
|
||
|
|
oldDeviceConnected = deviceConnected;
|
||
|
|
}
|
||
|
|
if ((distance()<LIMIT_FRONT)&&(pot_value>63))
|
||
|
|
{
|
||
|
|
pot_value=63;
|
||
|
|
motor(pot_value);
|
||
|
|
}
|
||
|
|
|
||
|
|
if (data_ok)
|
||
|
|
{
|
||
|
|
//data_ok = false;
|
||
|
|
if ((inputString == "D")||(inputString == "d"))
|
||
|
|
{
|
||
|
|
//left();
|
||
|
|
if (auto_mode) left(255);
|
||
|
|
}
|
||
|
|
else if ((inputString == "B")||(inputString == "b"))
|
||
|
|
{
|
||
|
|
//right();
|
||
|
|
if (auto_mode) right(255);
|
||
|
|
}
|
||
|
|
else if ((inputString == "E")||(inputString == "e"))
|
||
|
|
{
|
||
|
|
if ((auto_mode)&&(distance()>LIMIT_FRONT))
|
||
|
|
{
|
||
|
|
if (pot_value<127) pot_value++;
|
||
|
|
motor(pot_value);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if ((inputString == "G")||(inputString == "g"))
|
||
|
|
{
|
||
|
|
if (auto_mode)
|
||
|
|
{
|
||
|
|
if (pot_value>0) pot_value--;
|
||
|
|
motor(pot_value);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if ((inputString == "H")||(inputString == "h"))
|
||
|
|
{
|
||
|
|
if (auto_mode)
|
||
|
|
{
|
||
|
|
pot_value=63;
|
||
|
|
motor(pot_value);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if ((inputString == "f")||(inputString == "F"))
|
||
|
|
{
|
||
|
|
if (!flag_auto)
|
||
|
|
{
|
||
|
|
auto_mode=!auto_mode;
|
||
|
|
Serial.println(auto_mode);
|
||
|
|
flag_auto = true;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
else if (inputString == 0)
|
||
|
|
{
|
||
|
|
if (auto_mode) stop_giro();
|
||
|
|
flag_auto = false;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
if (!auto_mode) pot();
|
||
|
|
}
|
||
|
|
|
||
|
|
void motor(uint8_t value)
|
||
|
|
{
|
||
|
|
ds3502.setWiper(value);
|
||
|
|
delay(10);
|
||
|
|
}
|
||
|
|
void right(uint8_t value)
|
||
|
|
{
|
||
|
|
digitalWrite(DIR, HIGH);
|
||
|
|
ledcWrite(0, value);
|
||
|
|
}
|
||
|
|
|
||
|
|
void left(uint8_t value)
|
||
|
|
{
|
||
|
|
digitalWrite(DIR, LOW);
|
||
|
|
ledcWrite(0, value);
|
||
|
|
}
|
||
|
|
|
||
|
|
void stop_giro()
|
||
|
|
{
|
||
|
|
digitalWrite(DIR, LOW);
|
||
|
|
ledcWrite(0, 0);
|
||
|
|
}
|
||
|
|
|
||
|
|
//#define VCC 5000
|
||
|
|
#define POT 10000
|
||
|
|
|
||
|
|
void pot() {
|
||
|
|
float wiper_value_in = ads.readADC_SingleEnded(1)*0.1875;
|
||
|
|
// Serial.print("Voltage input: ");
|
||
|
|
// Serial.print(wiper_value_in);
|
||
|
|
// Serial.print(" mV, ");
|
||
|
|
voltage(wiper_value_in);
|
||
|
|
}
|
||
|
|
|
||
|
|
void voltage(float volt)
|
||
|
|
{
|
||
|
|
float VCC = ads.readADC_SingleEnded(2)*0.1875;
|
||
|
|
// Serial.print("Voltage suppy: ");
|
||
|
|
// Serial.print(VCC);
|
||
|
|
// Serial.print(" mV, ");
|
||
|
|
float resistor = (volt*POT)/(VCC);
|
||
|
|
uint8_t value = uint8_t ((resistor*128)/POT);
|
||
|
|
ds3502.setWiper(value);
|
||
|
|
float wiper_value_out = ads.readADC_SingleEnded(0)*0.1875;
|
||
|
|
// Serial.print("Voltage output: ");
|
||
|
|
// Serial.print(wiper_value_out);
|
||
|
|
// Serial.println(" mV");
|
||
|
|
}
|
||
|
|
|
||
|
|
unsigned char data[4]={};
|
||
|
|
float dist;
|
||
|
|
|
||
|
|
float distance()
|
||
|
|
{
|
||
|
|
do{
|
||
|
|
for(int i=0;i<4;i++)
|
||
|
|
{
|
||
|
|
data[i]=Serial2.read();
|
||
|
|
}
|
||
|
|
}while(Serial2.read()==0xff);
|
||
|
|
if(data[0]==0xff)
|
||
|
|
{
|
||
|
|
int sum;
|
||
|
|
sum =(data[0]+data[1]+data[2])&0x00FF;
|
||
|
|
if(sum==data[3])
|
||
|
|
{
|
||
|
|
dist=(data[1]<<8)+data[2];
|
||
|
|
if(dist>30)
|
||
|
|
{
|
||
|
|
//Serial.print("distance=");
|
||
|
|
Serial.println(dist/10);
|
||
|
|
return dist/10;
|
||
|
|
//Serial.println("cm");
|
||
|
|
}else
|
||
|
|
{
|
||
|
|
Serial.println("Below the lower limit");
|
||
|
|
}
|
||
|
|
}else Serial.println("ERROR");
|
||
|
|
}
|
||
|
|
}
|