Initial add of the current firmware.
This commit is contained in:
481
Firmware/micro_motors/micro_motors.ino
Normal file
481
Firmware/micro_motors/micro_motors.ino
Normal file
@@ -0,0 +1,481 @@
|
|||||||
|
#include <WiFi.h>
|
||||||
|
#include <ArduinoOTA.h>
|
||||||
|
|
||||||
|
|
||||||
|
const char* ssid = "CIA";
|
||||||
|
const char* password = "hi123456";
|
||||||
|
|
||||||
|
#define motorAttach ledcAttach // Get more appropriate names
|
||||||
|
#define motorSpeed ledcWrite // same "
|
||||||
|
|
||||||
|
// Motor FR (Front Right)
|
||||||
|
#define FR_IN1 27
|
||||||
|
#define FR_IN2 14
|
||||||
|
#define FR_PWM 0
|
||||||
|
|
||||||
|
// Motor FL (Front Left)
|
||||||
|
#define FL_IN1 12
|
||||||
|
#define FL_IN2 13
|
||||||
|
#define FL_PWM 2
|
||||||
|
|
||||||
|
// Motor BR (Back Right)
|
||||||
|
#define BR_IN1 33
|
||||||
|
#define BR_IN2 32
|
||||||
|
#define BR_PWM 23
|
||||||
|
|
||||||
|
// Motor BL (Back Left)
|
||||||
|
#define BL_IN1 26
|
||||||
|
#define BL_IN2 25
|
||||||
|
#define BL_PWM 22
|
||||||
|
|
||||||
|
|
||||||
|
// PWM settings
|
||||||
|
#define PWM_FREQ 1000
|
||||||
|
#define PWM_RESOLUTION 8 // 0-255
|
||||||
|
|
||||||
|
#define SERIAL_RX 21
|
||||||
|
|
||||||
|
// Sensor data received from micro sensors
|
||||||
|
int sens_FL = -1;
|
||||||
|
int sens_FR = -1;
|
||||||
|
int sens_BL = -1;
|
||||||
|
int sens_BR = -1;
|
||||||
|
int sens_LF = -1;
|
||||||
|
int sens_LB = -1;
|
||||||
|
int sens_RF = -1;
|
||||||
|
int sens_RB = -1;
|
||||||
|
|
||||||
|
// Trim values to calibrate wheel speeds
|
||||||
|
int trimFL = 0;
|
||||||
|
int trimFR = 0;
|
||||||
|
int trimBL = 0;
|
||||||
|
int trimBR = 0;
|
||||||
|
|
||||||
|
|
||||||
|
// Telnet server to watch serial
|
||||||
|
WiFiServer telnetServer(23);
|
||||||
|
WiFiClient telnetClient;
|
||||||
|
|
||||||
|
// Function declaritons
|
||||||
|
void otaTask(void *parameter); // Get micro ready for OTA
|
||||||
|
void logPrint(const char* msg); // Print function for serial and telnet, strings
|
||||||
|
void logPrint(int val); // Print function for serial and telnet, integers
|
||||||
|
void logPrint(float val); // Print function for serial and telnet, floats
|
||||||
|
void logPrintln(const char* msg); // Print line function for serial and telnet
|
||||||
|
void logPrintln(int val); // Print line function for serial and telnet, integers
|
||||||
|
void logPrintln(float val); // Print line function for serial and telnet, floats
|
||||||
|
void telnetTask(void *parameter); // Configure telnet serial
|
||||||
|
void motorFR(int speed); // Front right motor, speed from -255 to 255, positive goes forward, negative goes backward
|
||||||
|
void motorFL(int speed); // Front left motor, speed from -255 to 255, positive goes forward, negative goes backward
|
||||||
|
void motorBR(int speed); // Back right motor, speed from -255 to 255, positive goes forward, negative goes backward
|
||||||
|
void motorBL(int speed); // Back left motor, speed from -255 to 255, positive goes forward, negative goes backward
|
||||||
|
void motorsStop(); // Stop all motors
|
||||||
|
void motorsGo(int speed); // All motors move the same speed, from -255 to 255, positive goes forward, negative goes backward
|
||||||
|
void Go45right();
|
||||||
|
void Go45left();
|
||||||
|
void Go90right();
|
||||||
|
void Go90left();
|
||||||
|
void slideRight();
|
||||||
|
void slideLeft();
|
||||||
|
void receiveSensorData(); // Receive the sensors data from micro sensors
|
||||||
|
void sensorsTest();
|
||||||
|
void motorsTest();
|
||||||
|
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
Serial.begin(115200);
|
||||||
|
|
||||||
|
// Configure WIFI
|
||||||
|
WiFi.mode(WIFI_STA);
|
||||||
|
WiFi.begin(ssid, password);
|
||||||
|
|
||||||
|
int wifiAttempts = 0;
|
||||||
|
while (WiFi.waitForConnectResult() != WL_CONNECTED && wifiAttempts < 10) {
|
||||||
|
Serial.println("WiFi connecting...");
|
||||||
|
wifiAttempts++;
|
||||||
|
delay(500);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (WiFi.status() == WL_CONNECTED) {
|
||||||
|
Serial.print("IP Address: ");
|
||||||
|
Serial.println(WiFi.localIP());
|
||||||
|
} else {
|
||||||
|
Serial.println("WiFi not available - running without network");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
if (WiFi.status() == WL_CONNECTED) {
|
||||||
|
ArduinoOTA.setHostname("Motors");
|
||||||
|
ArduinoOTA.onStart([]() {
|
||||||
|
Serial.println("OTA Update starting...");
|
||||||
|
});
|
||||||
|
ArduinoOTA.onEnd([]() {
|
||||||
|
Serial.println("\nOTA Update complete!");
|
||||||
|
});
|
||||||
|
ArduinoOTA.onError([](ota_error_t error) {
|
||||||
|
Serial.printf("OTA Error [%u]\n", error);
|
||||||
|
});
|
||||||
|
ArduinoOTA.begin();
|
||||||
|
telnetServer.begin();
|
||||||
|
}
|
||||||
|
|
||||||
|
Serial2.begin(115200, SERIAL_8N1, SERIAL_RX, -1);
|
||||||
|
|
||||||
|
// Motor direction pins
|
||||||
|
pinMode(FR_IN1, OUTPUT);
|
||||||
|
pinMode(FR_IN2, OUTPUT);
|
||||||
|
pinMode(FL_IN1, OUTPUT);
|
||||||
|
pinMode(FL_IN2, OUTPUT);
|
||||||
|
pinMode(BR_IN1, OUTPUT);
|
||||||
|
pinMode(BR_IN2, OUTPUT);
|
||||||
|
pinMode(BL_IN1, OUTPUT);
|
||||||
|
pinMode(BL_IN2, OUTPUT);
|
||||||
|
|
||||||
|
// PWM setup for ESP32
|
||||||
|
motorAttach(FR_PWM, PWM_FREQ, PWM_RESOLUTION);
|
||||||
|
motorAttach(FL_PWM, PWM_FREQ, PWM_RESOLUTION);
|
||||||
|
motorAttach(BR_PWM, PWM_FREQ, PWM_RESOLUTION);
|
||||||
|
motorAttach(BL_PWM, PWM_FREQ, PWM_RESOLUTION);
|
||||||
|
|
||||||
|
// Start with motors stopped
|
||||||
|
digitalWrite(FR_IN1, LOW);
|
||||||
|
digitalWrite(FR_IN2, LOW);
|
||||||
|
digitalWrite(FL_IN1, LOW);
|
||||||
|
digitalWrite(FL_IN2, LOW);
|
||||||
|
motorSpeed(FR_PWM, 0);
|
||||||
|
motorSpeed(FL_PWM, 0);
|
||||||
|
digitalWrite(BR_IN1, LOW);
|
||||||
|
digitalWrite(BR_IN2, LOW);
|
||||||
|
digitalWrite(BL_IN1, LOW);
|
||||||
|
digitalWrite(BL_IN2, LOW);
|
||||||
|
motorSpeed(BR_PWM, 0);
|
||||||
|
motorSpeed(BL_PWM, 0);
|
||||||
|
|
||||||
|
// FreeRTOS tasks
|
||||||
|
if (WiFi.status() == WL_CONNECTED) {
|
||||||
|
xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL);
|
||||||
|
xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// motorsTest();
|
||||||
|
sensorsTest();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
void otaTask(void *parameter) {
|
||||||
|
for (;;) {
|
||||||
|
ArduinoOTA.handle();
|
||||||
|
vTaskDelay(10 / portTICK_PERIOD_MS);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void telnetTask(void *parameter) {
|
||||||
|
for (;;) {
|
||||||
|
if (telnetServer.hasClient()) {
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.stop();
|
||||||
|
}
|
||||||
|
telnetClient = telnetServer.available();
|
||||||
|
telnetClient.println("Connected to Motors");
|
||||||
|
}
|
||||||
|
vTaskDelay(100 / portTICK_PERIOD_MS);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrint(const char* msg) {
|
||||||
|
Serial.print(msg);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.print(msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrint(int val) {
|
||||||
|
Serial.print(val);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.print(val);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrint(float val) {
|
||||||
|
Serial.print(val);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.print(val);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrintln(const char* msg) {
|
||||||
|
Serial.println(msg);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.println(msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrintln(int val) {
|
||||||
|
Serial.println(val);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.println(val);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrintln(float val) {
|
||||||
|
Serial.println(val);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.println(val);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void motorFR(int speed) {
|
||||||
|
if (speed > 0) {
|
||||||
|
digitalWrite(FR_IN1, HIGH);
|
||||||
|
digitalWrite(FR_IN2, LOW);
|
||||||
|
motorSpeed(FR_PWM, constrain(speed + trimFR, 0, 255));
|
||||||
|
} else if (speed < 0) {
|
||||||
|
digitalWrite(FR_IN1, LOW);
|
||||||
|
digitalWrite(FR_IN2, HIGH);
|
||||||
|
motorSpeed(FR_PWM, constrain(-speed + trimFR, 0, 255));
|
||||||
|
} else {
|
||||||
|
digitalWrite(FR_IN1, LOW);
|
||||||
|
digitalWrite(FR_IN2, LOW);
|
||||||
|
motorSpeed(FR_PWM, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void motorFL(int speed) {
|
||||||
|
if (speed > 0) {
|
||||||
|
digitalWrite(FL_IN1, HIGH);
|
||||||
|
digitalWrite(FL_IN2, LOW);
|
||||||
|
motorSpeed(FL_PWM, constrain(speed + trimFL, 0, 255));
|
||||||
|
} else if (speed < 0) {
|
||||||
|
digitalWrite(FL_IN1, LOW);
|
||||||
|
digitalWrite(FL_IN2, HIGH);
|
||||||
|
motorSpeed(FL_PWM, constrain(-speed + trimFL, 0, 255));
|
||||||
|
} else {
|
||||||
|
digitalWrite(FL_IN1, LOW);
|
||||||
|
digitalWrite(FL_IN2, LOW);
|
||||||
|
motorSpeed(FL_PWM, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void motorBR(int speed) {
|
||||||
|
if (speed > 0) {
|
||||||
|
digitalWrite(BR_IN1, HIGH);
|
||||||
|
digitalWrite(BR_IN2, LOW);
|
||||||
|
motorSpeed(BR_PWM, constrain(speed + trimBR, 0, 255));
|
||||||
|
} else if (speed < 0) {
|
||||||
|
digitalWrite(BR_IN1, LOW);
|
||||||
|
digitalWrite(BR_IN2, HIGH);
|
||||||
|
motorSpeed(BR_PWM, constrain(-speed + trimBR, 0, 255));
|
||||||
|
} else {
|
||||||
|
digitalWrite(BR_IN1, LOW);
|
||||||
|
digitalWrite(BR_IN2, LOW);
|
||||||
|
motorSpeed(BR_PWM, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void motorBL(int speed) {
|
||||||
|
if (speed > 0) {
|
||||||
|
digitalWrite(BL_IN1, HIGH);
|
||||||
|
digitalWrite(BL_IN2, LOW);
|
||||||
|
motorSpeed(BL_PWM, constrain(speed + trimBL, 0, 255));
|
||||||
|
} else if (speed < 0) {
|
||||||
|
digitalWrite(BL_IN1, LOW);
|
||||||
|
digitalWrite(BL_IN2, HIGH);
|
||||||
|
motorSpeed(BL_PWM, constrain(-speed + trimBL, 0, 255));
|
||||||
|
} else {
|
||||||
|
digitalWrite(BL_IN1, LOW);
|
||||||
|
digitalWrite(BL_IN2, LOW);
|
||||||
|
motorSpeed(BL_PWM, 0);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void motorsStop() {
|
||||||
|
motorFR(0);
|
||||||
|
motorFL(0);
|
||||||
|
motorBR(0);
|
||||||
|
motorBL(0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void receiveSensorData() {
|
||||||
|
if (Serial2.available()) {
|
||||||
|
String data = Serial2.readStringUntil('\n');
|
||||||
|
int values[8];
|
||||||
|
int idx = 0;
|
||||||
|
int start = 0;
|
||||||
|
|
||||||
|
for (int i = 0; i <= data.length() && idx < 8; i++) {
|
||||||
|
if (i == data.length() || data[i] == ',') {
|
||||||
|
values[idx++] = data.substring(start, i).toInt();
|
||||||
|
start = i + 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (idx == 8) {
|
||||||
|
sens_FL = values[0];
|
||||||
|
sens_FR = values[1];
|
||||||
|
sens_BL = values[2];
|
||||||
|
sens_BR = values[3];
|
||||||
|
sens_LF = values[4];
|
||||||
|
sens_LB = values[5];
|
||||||
|
sens_RF = values[6];
|
||||||
|
sens_RB = values[7];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void sensorsTest() {
|
||||||
|
|
||||||
|
receiveSensorData();
|
||||||
|
|
||||||
|
logPrint("FL:"); logPrint(sens_FL);
|
||||||
|
logPrint(" FR:"); logPrint(sens_FR);
|
||||||
|
logPrint(" BL:"); logPrint(sens_BL);
|
||||||
|
logPrint(" BR:"); logPrint(sens_BR);
|
||||||
|
logPrint(" LF:"); logPrint(sens_LF);
|
||||||
|
logPrint(" LB:"); logPrint(sens_LB);
|
||||||
|
logPrint(" RF:"); logPrint(sens_RF);
|
||||||
|
logPrint(" RB:"); logPrintln(sens_RB);
|
||||||
|
|
||||||
|
delay(100);
|
||||||
|
}
|
||||||
|
|
||||||
|
void motorsTest() {
|
||||||
|
|
||||||
|
logPrintln("Right forward...");
|
||||||
|
motorBR(80);
|
||||||
|
motorFR(80);
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
logPrintln("Right stop...");
|
||||||
|
motorBR(0);
|
||||||
|
motorFR(0);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
logPrintln("Right forward...");
|
||||||
|
motorBR(250);
|
||||||
|
motorFR(250);
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
logPrintln("Right stop...");
|
||||||
|
motorBR(0);
|
||||||
|
motorFR(0);
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
logPrintln("Left forward...");
|
||||||
|
motorBL(80);
|
||||||
|
motorFL(80);
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
logPrintln("Left stop...");
|
||||||
|
motorsStop();
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
logPrintln("Left forward...");
|
||||||
|
motorBL(250);
|
||||||
|
motorFL(250);
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
logPrintln("Left stop...");
|
||||||
|
motorsStop();
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
logPrintln("Front forward...");
|
||||||
|
motorFR(80);
|
||||||
|
motorFL(80);
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
logPrintln("Front stop...");
|
||||||
|
motorsStop();
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
logPrintln("Front forward...");
|
||||||
|
motorFR(250);
|
||||||
|
motorFL(250);
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
logPrintln("Front stop...");
|
||||||
|
motorsStop();
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
logPrintln("Back forward...");
|
||||||
|
motorBR(80);
|
||||||
|
motorBL(80);
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
logPrintln("Back stop...");
|
||||||
|
motorsStop();
|
||||||
|
delay(1000);
|
||||||
|
|
||||||
|
logPrintln("Back forward...");
|
||||||
|
motorBR(250);
|
||||||
|
motorBL(250);
|
||||||
|
delay(2000);
|
||||||
|
|
||||||
|
logPrintln("Back stop...");
|
||||||
|
motorsStop();
|
||||||
|
delay(1000);
|
||||||
|
}
|
||||||
|
|
||||||
|
void motorsGo(int speed) {
|
||||||
|
motorFR(speed);
|
||||||
|
motorFL(speed);
|
||||||
|
motorBR(speed);
|
||||||
|
motorBL(speed);
|
||||||
|
}
|
||||||
|
|
||||||
|
void Go45right() {
|
||||||
|
motorFL(150);
|
||||||
|
motorFR(-150);
|
||||||
|
motorBL(150);
|
||||||
|
motorBR(-150);
|
||||||
|
delay(875);
|
||||||
|
motorsStop();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Go45left() {
|
||||||
|
motorFL(-150);
|
||||||
|
motorFR(150);
|
||||||
|
motorBL(-150);
|
||||||
|
motorBR(150);
|
||||||
|
delay(875);
|
||||||
|
motorsStop();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Go90right() {
|
||||||
|
motorFL(150);
|
||||||
|
motorFR(-150);
|
||||||
|
motorBL(150);
|
||||||
|
motorBR(-150);
|
||||||
|
delay(1735);
|
||||||
|
motorsStop();
|
||||||
|
}
|
||||||
|
|
||||||
|
void Go90left() {
|
||||||
|
motorFL(-150);
|
||||||
|
motorFR(150);
|
||||||
|
motorBL(-150);
|
||||||
|
motorBR(150);
|
||||||
|
delay(1735);
|
||||||
|
motorsStop();
|
||||||
|
}
|
||||||
|
|
||||||
|
void slideRight() {
|
||||||
|
motorFL(-150);
|
||||||
|
motorFR(150);
|
||||||
|
motorBL(150);
|
||||||
|
motorBR(-150);
|
||||||
|
}
|
||||||
|
|
||||||
|
void slideLeft() {
|
||||||
|
motorFL(150);
|
||||||
|
motorFR(-150);
|
||||||
|
motorBL(-150);
|
||||||
|
motorBR(150);
|
||||||
|
}
|
||||||
465
Firmware/micro_sensors/micro_sensors.ino
Normal file
465
Firmware/micro_sensors/micro_sensors.ino
Normal file
@@ -0,0 +1,465 @@
|
|||||||
|
#include <WiFi.h>
|
||||||
|
#include <ArduinoOTA.h>
|
||||||
|
#include "Adafruit_VL53L0X.h"
|
||||||
|
|
||||||
|
// Sensors I2C addresses
|
||||||
|
#define SFL_ADDRESS 0x30
|
||||||
|
#define SFR_ADDRESS 0x31
|
||||||
|
#define SBL_ADDRESS 0x32
|
||||||
|
#define SBR_ADDRESS 0x33
|
||||||
|
#define SLF_ADDRESS 0x34
|
||||||
|
#define SLB_ADDRESS 0x35
|
||||||
|
#define SRF_ADDRESS 0x36
|
||||||
|
#define SRB_ADDRESS 0x37
|
||||||
|
|
||||||
|
// Sensors shutdown pins
|
||||||
|
#define SHT_SFL 19
|
||||||
|
#define SHT_SFR 18
|
||||||
|
#define SHT_SBL 16
|
||||||
|
#define SHT_SBR 4
|
||||||
|
#define SHT_SLF 0
|
||||||
|
#define SHT_SLB 2
|
||||||
|
#define SHT_SRF 17
|
||||||
|
#define SHT_SRB 5
|
||||||
|
|
||||||
|
#define SERIAL_TX 15
|
||||||
|
|
||||||
|
// Sensors objects
|
||||||
|
Adafruit_VL53L0X SFL = Adafruit_VL53L0X();
|
||||||
|
Adafruit_VL53L0X SFR = Adafruit_VL53L0X();
|
||||||
|
Adafruit_VL53L0X SBL = Adafruit_VL53L0X();
|
||||||
|
Adafruit_VL53L0X SBR = Adafruit_VL53L0X();
|
||||||
|
Adafruit_VL53L0X SLF = Adafruit_VL53L0X();
|
||||||
|
Adafruit_VL53L0X SLB = Adafruit_VL53L0X();
|
||||||
|
Adafruit_VL53L0X SRF = Adafruit_VL53L0X();
|
||||||
|
Adafruit_VL53L0X SRB = Adafruit_VL53L0X();
|
||||||
|
|
||||||
|
// this holds the measurement
|
||||||
|
VL53L0X_RangingMeasurementData_t m_FL;
|
||||||
|
VL53L0X_RangingMeasurementData_t m_FR;
|
||||||
|
VL53L0X_RangingMeasurementData_t m_BL;
|
||||||
|
VL53L0X_RangingMeasurementData_t m_BR;
|
||||||
|
VL53L0X_RangingMeasurementData_t m_LF;
|
||||||
|
VL53L0X_RangingMeasurementData_t m_LB;
|
||||||
|
VL53L0X_RangingMeasurementData_t m_RF;
|
||||||
|
VL53L0X_RangingMeasurementData_t m_RB;
|
||||||
|
|
||||||
|
// Track which sensors initialized
|
||||||
|
bool SFL_OK = false;
|
||||||
|
bool SFR_OK = false;
|
||||||
|
bool SBL_OK = false;
|
||||||
|
bool SBR_OK = false;
|
||||||
|
bool SLF_OK = false;
|
||||||
|
bool SLB_OK = false;
|
||||||
|
bool SRF_OK = false;
|
||||||
|
bool SRB_OK = false;
|
||||||
|
|
||||||
|
|
||||||
|
const char* ssid = "CIA";
|
||||||
|
const char* password = "hi123456";
|
||||||
|
|
||||||
|
// Telnet server to watch serial
|
||||||
|
WiFiServer telnetServer(23);
|
||||||
|
WiFiClient telnetClient;
|
||||||
|
|
||||||
|
// Function declaritons
|
||||||
|
void otaTask(void *parameter); // Get micro ready for OTA
|
||||||
|
void logPrint(const char* msg); // Print function for serial and telnet, strings
|
||||||
|
void logPrint(int val); // Print function for serial and telnet, integers
|
||||||
|
void logPrint(float val); // Print function for serial and telnet, floats
|
||||||
|
void logPrintln(const char* msg); // Print line function for serial and telnet
|
||||||
|
void logPrintln(int val); // Print line function for serial and telnet, integers
|
||||||
|
void logPrintln(float val); // Print line function for serial and telnet, floats
|
||||||
|
void telnetTask(void *parameter); // Configure telnet serial
|
||||||
|
void setID(); // Set I2C on sensors
|
||||||
|
void read_sensors(); // Read sensors test
|
||||||
|
void sendSensorData(); // Send sensors data to micro motors
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
|
||||||
|
Serial.begin(115200);
|
||||||
|
|
||||||
|
// Configure WIFI
|
||||||
|
WiFi.mode(WIFI_STA);
|
||||||
|
WiFi.begin(ssid, password);
|
||||||
|
|
||||||
|
int wifiAttempts = 0;
|
||||||
|
while (WiFi.waitForConnectResult() != WL_CONNECTED && wifiAttempts < 10) {
|
||||||
|
Serial.println("WiFi connecting...");
|
||||||
|
wifiAttempts++;
|
||||||
|
delay(500);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (WiFi.status() == WL_CONNECTED) {
|
||||||
|
Serial.print("IP Address: ");
|
||||||
|
Serial.println(WiFi.localIP());
|
||||||
|
} else {
|
||||||
|
Serial.println("WiFi not available - running without network");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (WiFi.status() == WL_CONNECTED) {
|
||||||
|
ArduinoOTA.setHostname("Sensors");
|
||||||
|
ArduinoOTA.onStart([]() {
|
||||||
|
Serial.println("OTA Update starting...");
|
||||||
|
});
|
||||||
|
ArduinoOTA.onEnd([]() {
|
||||||
|
Serial.println("\nOTA Update complete!");
|
||||||
|
});
|
||||||
|
ArduinoOTA.onError([](ota_error_t error) {
|
||||||
|
Serial.printf("OTA Error [%u]\n", error);
|
||||||
|
});
|
||||||
|
ArduinoOTA.begin();
|
||||||
|
telnetServer.begin();
|
||||||
|
}
|
||||||
|
|
||||||
|
Serial2.begin(115200, SERIAL_8N1, -1, SERIAL_TX);
|
||||||
|
|
||||||
|
pinMode(SHT_SFL, OUTPUT);
|
||||||
|
pinMode(SHT_SFR, OUTPUT);
|
||||||
|
pinMode(SHT_SBL, OUTPUT);
|
||||||
|
pinMode(SHT_SBR, OUTPUT);
|
||||||
|
pinMode(SHT_SLF, OUTPUT);
|
||||||
|
pinMode(SHT_SLB, OUTPUT);
|
||||||
|
pinMode(SHT_SRF, OUTPUT);
|
||||||
|
pinMode(SHT_SRB, OUTPUT);
|
||||||
|
|
||||||
|
Serial.println(F("Shutdown pins inited..."));
|
||||||
|
|
||||||
|
digitalWrite(SHT_SFL, LOW);
|
||||||
|
digitalWrite(SHT_SFR, LOW);
|
||||||
|
digitalWrite(SHT_SBL, LOW);
|
||||||
|
digitalWrite(SHT_SBR, LOW);
|
||||||
|
digitalWrite(SHT_SLF, LOW);
|
||||||
|
digitalWrite(SHT_SLB, LOW);
|
||||||
|
digitalWrite(SHT_SRF, LOW);
|
||||||
|
digitalWrite(SHT_SRB, LOW);
|
||||||
|
|
||||||
|
|
||||||
|
setID();
|
||||||
|
delay(50);
|
||||||
|
|
||||||
|
// FreeRTOS tasks
|
||||||
|
if (WiFi.status() == WL_CONNECTED) {
|
||||||
|
xTaskCreate(otaTask, "OTA", 4096, NULL, 1, NULL);
|
||||||
|
xTaskCreate(telnetTask, "Telnet", 4096, NULL, 1, NULL);
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
|
||||||
|
|
||||||
|
read_sensors();
|
||||||
|
sendSensorData();
|
||||||
|
delay(100);
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void otaTask(void *parameter) {
|
||||||
|
for (;;) {
|
||||||
|
ArduinoOTA.handle();
|
||||||
|
vTaskDelay(10 / portTICK_PERIOD_MS);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void telnetTask(void *parameter) {
|
||||||
|
for (;;) {
|
||||||
|
if (telnetServer.hasClient()) {
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.stop();
|
||||||
|
}
|
||||||
|
telnetClient = telnetServer.available();
|
||||||
|
telnetClient.println("Connected to Sensors");
|
||||||
|
}
|
||||||
|
vTaskDelay(100 / portTICK_PERIOD_MS);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrint(const char* msg) {
|
||||||
|
Serial.print(msg);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.print(msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrint(int val) {
|
||||||
|
Serial.print(val);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.print(val);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrint(float val) {
|
||||||
|
Serial.print(val);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.print(val);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrintln(const char* msg) {
|
||||||
|
Serial.println(msg);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.println(msg);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrintln(int val) {
|
||||||
|
Serial.println(val);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.println(val);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void logPrintln(float val) {
|
||||||
|
Serial.println(val);
|
||||||
|
if (telnetClient && telnetClient.connected()) {
|
||||||
|
telnetClient.println(val);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void setID() {
|
||||||
|
// all reset
|
||||||
|
digitalWrite(SHT_SFL, LOW);
|
||||||
|
digitalWrite(SHT_SFR, LOW);
|
||||||
|
digitalWrite(SHT_SBL, LOW);
|
||||||
|
digitalWrite(SHT_SBR, LOW);
|
||||||
|
digitalWrite(SHT_SLF, LOW);
|
||||||
|
digitalWrite(SHT_SLB, LOW);
|
||||||
|
digitalWrite(SHT_SRF, LOW);
|
||||||
|
digitalWrite(SHT_SRB, LOW);
|
||||||
|
delay(10);
|
||||||
|
// all unreset
|
||||||
|
digitalWrite(SHT_SFL, HIGH);
|
||||||
|
digitalWrite(SHT_SFR, HIGH);
|
||||||
|
digitalWrite(SHT_SBL, HIGH);
|
||||||
|
digitalWrite(SHT_SBR, HIGH);
|
||||||
|
digitalWrite(SHT_SLF, HIGH);
|
||||||
|
digitalWrite(SHT_SLB, HIGH);
|
||||||
|
digitalWrite(SHT_SRF, HIGH);
|
||||||
|
digitalWrite(SHT_SRB, HIGH);
|
||||||
|
delay(10);
|
||||||
|
|
||||||
|
// activating SFL while keeping all others shutdown
|
||||||
|
digitalWrite(SHT_SFL, HIGH);
|
||||||
|
digitalWrite(SHT_SFR, LOW);
|
||||||
|
digitalWrite(SHT_SBL, LOW);
|
||||||
|
digitalWrite(SHT_SBR, LOW);
|
||||||
|
digitalWrite(SHT_SLF, LOW);
|
||||||
|
digitalWrite(SHT_SLB, LOW);
|
||||||
|
digitalWrite(SHT_SRF, LOW);
|
||||||
|
digitalWrite(SHT_SRB, LOW);
|
||||||
|
|
||||||
|
// initing SFL
|
||||||
|
if(!SFL.begin(SFL_ADDRESS)) {
|
||||||
|
logPrintln("Failed to boot SFL");
|
||||||
|
} else {
|
||||||
|
logPrintln("SFL OK");
|
||||||
|
SFL_OK = true;
|
||||||
|
} delay(10);
|
||||||
|
|
||||||
|
// activating SFR
|
||||||
|
digitalWrite(SHT_SFR, HIGH);
|
||||||
|
delay(10);
|
||||||
|
//initing SFR
|
||||||
|
if(!SFR.begin(SFR_ADDRESS)) {
|
||||||
|
logPrintln("Failed to boot SFR");
|
||||||
|
} else {
|
||||||
|
logPrintln("SFR OK");
|
||||||
|
SFR_OK = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// activating SBL
|
||||||
|
digitalWrite(SHT_SBL, HIGH);
|
||||||
|
delay(10);
|
||||||
|
//initing SBL
|
||||||
|
if(!SBL.begin(SBL_ADDRESS)) {
|
||||||
|
logPrintln("Failed to boot SBL");
|
||||||
|
} else {
|
||||||
|
logPrintln("SBL OK");
|
||||||
|
SBL_OK = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// activating SBR
|
||||||
|
digitalWrite(SHT_SBR, HIGH);
|
||||||
|
delay(10);
|
||||||
|
//initing SBR
|
||||||
|
if(!SBR.begin(SBR_ADDRESS)) {
|
||||||
|
logPrintln("Failed to boot SBR");
|
||||||
|
} else {
|
||||||
|
logPrintln("SBR OK");
|
||||||
|
SBR_OK = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// activating SLF
|
||||||
|
digitalWrite(SHT_SLF, HIGH);
|
||||||
|
delay(10);
|
||||||
|
//initing SLF
|
||||||
|
if(!SLF.begin(SLF_ADDRESS)) {
|
||||||
|
logPrintln("Failed to boot SLF");
|
||||||
|
} else {
|
||||||
|
logPrintln("SLF OK");
|
||||||
|
SLF_OK = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// activating SLB
|
||||||
|
digitalWrite(SHT_SLB, HIGH);
|
||||||
|
delay(10);
|
||||||
|
//initing SLB
|
||||||
|
if(!SLB.begin(SLB_ADDRESS)) {
|
||||||
|
logPrintln("Failed to boot SLB");
|
||||||
|
} else {
|
||||||
|
logPrintln("SLB OK");
|
||||||
|
SLB_OK = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// activating SRF
|
||||||
|
digitalWrite(SHT_SRF, HIGH);
|
||||||
|
delay(10);
|
||||||
|
//initing SRF
|
||||||
|
if(!SRF.begin(SRF_ADDRESS)) {
|
||||||
|
logPrintln("Failed to boot SRF");
|
||||||
|
} else {
|
||||||
|
logPrintln("SRF OK");
|
||||||
|
SRF_OK = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// activating SRB
|
||||||
|
digitalWrite(SHT_SRB, HIGH);
|
||||||
|
delay(10);
|
||||||
|
//initing SRB
|
||||||
|
if(!SRB.begin(SRB_ADDRESS)) {
|
||||||
|
logPrintln("Failed to boot SRB");
|
||||||
|
} else {
|
||||||
|
logPrintln("SRB OK");
|
||||||
|
SRB_OK = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void read_sensors() {
|
||||||
|
|
||||||
|
if(SFL_OK) {SFL.rangingTest(&m_FL, false);}
|
||||||
|
if(SFR_OK) {SFR.rangingTest(&m_FR, false);}
|
||||||
|
if(SBL_OK) {SBL.rangingTest(&m_BL, false);}
|
||||||
|
if(SBR_OK) {SBR.rangingTest(&m_BR, false);}
|
||||||
|
if(SLF_OK) {SLF.rangingTest(&m_LF, false);}
|
||||||
|
if(SLB_OK) {SLB.rangingTest(&m_LB, false);}
|
||||||
|
if(SRF_OK) {SRF.rangingTest(&m_RF, false);}
|
||||||
|
if(SRB_OK) {SRB.rangingTest(&m_RB, false);}
|
||||||
|
|
||||||
|
|
||||||
|
// Print Front left sensor reading
|
||||||
|
logPrint("Front left: ");
|
||||||
|
if(!SFL_OK) {
|
||||||
|
logPrint("NC");
|
||||||
|
} else if(m_FL.RangeStatus != 4) {
|
||||||
|
logPrint(m_FL.RangeMilliMeter);
|
||||||
|
} else {
|
||||||
|
logPrint("Out of range");
|
||||||
|
}
|
||||||
|
|
||||||
|
logPrint(" ");
|
||||||
|
|
||||||
|
// Print Front right sensor reading
|
||||||
|
logPrint("Front right: ");
|
||||||
|
if(!SFR_OK) {
|
||||||
|
logPrint("NC");
|
||||||
|
} else if(m_FR.RangeStatus != 4) {
|
||||||
|
logPrint(m_FR.RangeMilliMeter);
|
||||||
|
} else {
|
||||||
|
logPrint("Out of range");
|
||||||
|
}
|
||||||
|
|
||||||
|
logPrint(" ");
|
||||||
|
|
||||||
|
// Print Back left sensor reading
|
||||||
|
logPrint("Back left: ");
|
||||||
|
if(!SBL_OK) {
|
||||||
|
logPrint("NC");
|
||||||
|
} else if(m_BL.RangeStatus != 4) {
|
||||||
|
logPrint(m_BL.RangeMilliMeter);
|
||||||
|
} else {
|
||||||
|
logPrint("Out of range");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
logPrint(" ");
|
||||||
|
|
||||||
|
// Print Back right sensor reading
|
||||||
|
logPrint("Back right: ");
|
||||||
|
if(!SBR_OK) {
|
||||||
|
logPrint("NC");
|
||||||
|
} else if(m_BR.RangeStatus != 4) {
|
||||||
|
logPrint(m_BR.RangeMilliMeter);
|
||||||
|
} else {
|
||||||
|
logPrint("Out of range");
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
logPrint(" ");
|
||||||
|
|
||||||
|
// Print Left front sensor reading
|
||||||
|
logPrint("Left front: ");
|
||||||
|
if(!SLF_OK) {
|
||||||
|
logPrint("NC");
|
||||||
|
} else if(m_LF.RangeStatus != 4) {
|
||||||
|
logPrint(m_LF.RangeMilliMeter);
|
||||||
|
} else {
|
||||||
|
logPrint("Out of range");
|
||||||
|
}
|
||||||
|
|
||||||
|
logPrint(" ");
|
||||||
|
|
||||||
|
// Print Left back sensor reading
|
||||||
|
logPrint("Left back: ");
|
||||||
|
if(!SLB_OK) {
|
||||||
|
logPrint("NC");
|
||||||
|
} else if(m_LB.RangeStatus != 4) {
|
||||||
|
logPrint(m_LB.RangeMilliMeter);
|
||||||
|
} else {
|
||||||
|
logPrint("Out of range");
|
||||||
|
}
|
||||||
|
|
||||||
|
logPrint(" ");
|
||||||
|
|
||||||
|
// Print Right front sensor reading
|
||||||
|
logPrint("Right front: ");
|
||||||
|
if(!SRF_OK) {
|
||||||
|
logPrint("NC");
|
||||||
|
} else if(m_RF.RangeStatus != 4) {
|
||||||
|
logPrint(m_RF.RangeMilliMeter);
|
||||||
|
} else {
|
||||||
|
logPrint("Out of range");
|
||||||
|
}
|
||||||
|
|
||||||
|
logPrint(" ");
|
||||||
|
|
||||||
|
// Print back front sensor reading
|
||||||
|
logPrint("Right back: ");
|
||||||
|
if(!SRB_OK) {
|
||||||
|
logPrint("NC");
|
||||||
|
} else if(m_RB.RangeStatus != 4) {
|
||||||
|
logPrint(m_RB.RangeMilliMeter);
|
||||||
|
} else {
|
||||||
|
logPrint("Out of range");
|
||||||
|
}
|
||||||
|
|
||||||
|
logPrintln("");
|
||||||
|
}
|
||||||
|
|
||||||
|
void sendSensorData() {
|
||||||
|
// Format: FL,FR,BL,BR,LF,LB,RF,RB\n
|
||||||
|
Serial2.print(SFL_OK ? m_FL.RangeMilliMeter : -1); Serial2.print(",");
|
||||||
|
Serial2.print(SFR_OK ? m_FR.RangeMilliMeter : -1); Serial2.print(",");
|
||||||
|
Serial2.print(SBL_OK ? m_BL.RangeMilliMeter : -1); Serial2.print(",");
|
||||||
|
Serial2.print(SBR_OK ? m_BR.RangeMilliMeter : -1); Serial2.print(",");
|
||||||
|
Serial2.print(SLF_OK ? m_LF.RangeMilliMeter : -1); Serial2.print(",");
|
||||||
|
Serial2.print(SLB_OK ? m_LB.RangeMilliMeter : -1); Serial2.print(",");
|
||||||
|
Serial2.print(SRF_OK ? m_RF.RangeMilliMeter : -1); Serial2.print(",");
|
||||||
|
Serial2.println(SRB_OK ? m_RB.RangeMilliMeter : -1);
|
||||||
|
}
|
||||||
|
|
||||||
Reference in New Issue
Block a user