359 lines
9.6 KiB
C++
359 lines
9.6 KiB
C++
#include <ESP32Servo.h>
|
|
#include <Wire.h>
|
|
#include <VL53L0X.h>
|
|
#include <Adafruit_PCF8574.h>
|
|
#include <WiFi.h>
|
|
#include <AsyncTCP.h>
|
|
#include <ESPAsyncWebServer.h>
|
|
|
|
// Web server configuration
|
|
const char* ssid = "ESP_ping";
|
|
const char* password = "pong";
|
|
|
|
const char* PARAM_INPUT_1 = "output";
|
|
const char* PARAM_INPUT_2 = "state";
|
|
|
|
AsyncWebServer server(80);
|
|
|
|
struct pin {
|
|
int id;
|
|
int is_extender;
|
|
};
|
|
|
|
struct tube {
|
|
uint16_t height;
|
|
uint16_t target;
|
|
struct {
|
|
Servo hard;
|
|
struct pin pin;
|
|
int last_angle;
|
|
int equilibrium;
|
|
} servo;
|
|
struct {
|
|
VL53L0X hard;
|
|
struct pin gpio;
|
|
struct pin xshut;
|
|
} tof;
|
|
int last_measure[5];
|
|
int last_pos_index;
|
|
int id;
|
|
};
|
|
|
|
const struct pin all_xshuts[6] = {
|
|
{.id = 7, .is_extender = 1},
|
|
{.id = 0, .is_extender = 1},
|
|
{.id = 1, .is_extender = 1},
|
|
{.id = 6, .is_extender = 1},
|
|
{.id = 5, .is_extender = 1},
|
|
{.id = 4, .is_extender = 1}
|
|
};
|
|
|
|
|
|
const struct pin all_servos[6] = {
|
|
{.id = 32, .is_extender = 0},
|
|
{.id = 33, .is_extender = 0},
|
|
{.id = 18, .is_extender = 0},
|
|
{.id = 2, .is_extender = 1},
|
|
{.id = 3, .is_extender = 1},
|
|
{.id = 19, .is_extender = 0}
|
|
};
|
|
|
|
struct tube all_tubes[6];
|
|
const int TUBES_COUNT = 6;
|
|
|
|
const float minHeight = 50;
|
|
const float maxHeight = 445;
|
|
|
|
int minServo = 0;
|
|
int maxServo = 150;
|
|
|
|
|
|
Adafruit_PCF8574 pcf8574;
|
|
|
|
void scan_I2c(){
|
|
byte error, address;
|
|
int nDevices;
|
|
Serial.println("Scanning...");
|
|
nDevices = 0;
|
|
for(address = 1; address < 127; address++ ) {
|
|
Wire.beginTransmission(address);
|
|
error = Wire.endTransmission();
|
|
if (error == 0) {
|
|
Serial.print("I2C device found at address 0x");
|
|
if (address<16) {
|
|
Serial.print("0");
|
|
}
|
|
Serial.println(address,HEX);
|
|
nDevices++;
|
|
}
|
|
else if (error==4) {
|
|
Serial.print("Unknow error at address 0x");
|
|
if (address<16) {
|
|
Serial.print("0");
|
|
}
|
|
Serial.println(address,HEX);
|
|
}
|
|
}
|
|
if (nDevices == 0) {
|
|
Serial.println("No I2C devices found\n");
|
|
}
|
|
else {
|
|
Serial.println("done\n");
|
|
}
|
|
delay(5000);
|
|
}
|
|
|
|
uint16_t mesureHeight(int i){
|
|
uint16_t h = all_tubes[i].tof.hard.readRangeContinuousMillimeters();
|
|
//Serial.printf("measure: %d\n", h);
|
|
if (h > 2000){
|
|
//Serial.printf("returning height\n");
|
|
return all_tubes[i].height;
|
|
} else {
|
|
//Serial.printf("returning: %d\n", h);
|
|
return h;
|
|
}
|
|
}
|
|
|
|
void change_pin(struct pin pin, int state) {
|
|
if (pin.is_extender) {
|
|
pcf8574.digitalWrite(pin.id, state);
|
|
} else {
|
|
digitalWrite(pin.id, state);
|
|
}
|
|
}
|
|
|
|
void changePinMode(struct pin pin, int state) {
|
|
if (pin.is_extender) {
|
|
pcf8574.pinMode(pin.id, state);
|
|
} else {
|
|
pinMode(pin.id, state);
|
|
}
|
|
}
|
|
|
|
// Web Server configuration
|
|
|
|
const char index_html[] PROGMEM = R"rawliteral(
|
|
<!DOCTYPE HTML><html>
|
|
<head>
|
|
<title>Balle Ping Pong flottante</title>
|
|
<meta name="viewport" content="width=device-width, initial-scale=1">
|
|
<link rel="icon" href="data:,">
|
|
<style>
|
|
html {font-family: Arial; display: inline-block; text-align: center;}
|
|
h2 {font-size: 3.0rem;}
|
|
p {font-size: 3.0rem;}
|
|
body {max-width: 600px; margin:0px auto; padding-bottom: 25px;}
|
|
.switch {position: relative; display: inline-block; width: 120px; height: 68px}
|
|
.switch input {display: none}
|
|
.slider {background-color: #ccc; border-radius: 6px}
|
|
.slider:before {position: absolute; content: ""; height: 52px; width: 52px; left: 8px; bottom: 8px; background-color: #fff; -webkit-transition: .4s; transition: .4s; border-radius: 3px}
|
|
input:checked+.slider {background-color: #b30000}
|
|
input:checked+.slider:before {-webkit-transform: translateX(52px); -ms-transform: translateX(52px); transform: translateX(52px)}
|
|
</style>
|
|
</head>
|
|
<body>
|
|
<h2>Balle Ping Pong flottante</h2>
|
|
%BUTTONPLACEHOLDER%
|
|
<script>
|
|
|
|
function sendSliderValue(id, element) {
|
|
var slider = document.getElementById(`slider${id}`);
|
|
var xhr = new XMLHttpRequest();
|
|
xhr.open("GET", "/update?output="+id+"&state=" + slider.value, true);
|
|
xhr.send();
|
|
}
|
|
|
|
</script>
|
|
</body>
|
|
</html>
|
|
)rawliteral";
|
|
|
|
|
|
String processor(const String& var){
|
|
//Serial.println(var);
|
|
if(var == "BUTTONPLACEHOLDER"){
|
|
String buttons = "";
|
|
buttons += "<div class=\"slidecontainer\"><input type=\"range\" min=\"20\" max=\"450\" value=\"250\" class=\"slider\" id=\"slider0\" onchange=\"sendSliderValue(0, this.value)\"\"></div>";
|
|
buttons += "<div class=\"slidecontainer\"><input type=\"range\" min=\"20\" max=\"450\" value=\"250\" class=\"slider\" id=\"slider1\" onchange=\"sendSliderValue(1, this.value)\"\"></div>";
|
|
buttons += "<div class=\"slidecontainer\"><input type=\"range\" min=\"20\" max=\"450\" value=\"250\" class=\"slider\" id=\"slider2\" onchange=\"sendSliderValue(2, this.value)\"\"></div>";
|
|
buttons += "<div class=\"slidecontainer\"><input type=\"range\" min=\"20\" max=\"450\" value=\"250\" class=\"slider\" id=\"slider3\" onchange=\"sendSliderValue(3, this.value)\"\"></div>";
|
|
buttons += "<div class=\"slidecontainer\"><input type=\"range\" min=\"20\" max=\"450\" value=\"250\" class=\"slider\" id=\"slider4\" onchange=\"sendSliderValue(4, this.value)\"\"></div>";
|
|
buttons += "<div class=\"slidecontainer\"><input type=\"range\" min=\"20\" max=\"450\" value=\"250\" class=\"slider\" id=\"slider5\" onchange=\"sendSliderValue(5, this.value)\"\"></div>";
|
|
|
|
return buttons;
|
|
}
|
|
return String();
|
|
}
|
|
|
|
|
|
void wifisetup() {
|
|
Serial.print("Setting AP (Access Point)…");
|
|
WiFi.softAP(ssid);
|
|
|
|
IPAddress IP = WiFi.softAPIP();
|
|
Serial.print("AP IP address: ");
|
|
Serial.println(IP);
|
|
|
|
// Route for root / web page
|
|
server.on("/", HTTP_GET, [](AsyncWebServerRequest *request){
|
|
request->send_P(200, "text/html", index_html, processor);
|
|
});
|
|
|
|
// Send a GET request to <ESP_IP>/update?output=<inputMessage1>&state=<inputMessage2>
|
|
server.on("/update", HTTP_GET, [] (AsyncWebServerRequest *request) {
|
|
int inputMessage1;
|
|
int inputMessage2;
|
|
// GET input1 value on <ESP_IP>/update?output=<inputMessage1>&state=<inputMessage2>
|
|
if (request->hasParam(PARAM_INPUT_1) && request->hasParam(PARAM_INPUT_2)) {
|
|
inputMessage1 = request->getParam(PARAM_INPUT_1)->value().toInt();
|
|
inputMessage2 = request->getParam(PARAM_INPUT_2)->value().toInt();
|
|
Serial.printf("Received web command: %d %d\n", inputMessage1, inputMessage2);
|
|
all_tubes[inputMessage1].target = inputMessage2;
|
|
}
|
|
else {
|
|
// Error
|
|
inputMessage1 = -1;
|
|
inputMessage2 = -1;
|
|
}
|
|
Serial.print("GPIO: ");
|
|
Serial.print(inputMessage1);
|
|
Serial.print(" - Set to: ");
|
|
Serial.println(inputMessage2);
|
|
request->send(200, "text/plain", "OK");
|
|
});
|
|
|
|
// Start server
|
|
server.begin();
|
|
}
|
|
|
|
|
|
// Tube configuration
|
|
void setup_all_tofs() {
|
|
// Reset all TOFs by turning them up and low
|
|
|
|
for (int i = 0; i < TUBES_COUNT; ++i) {
|
|
changePinMode(all_xshuts[i], OUTPUT);
|
|
change_pin(all_xshuts[i], LOW);
|
|
}
|
|
|
|
for (int i = 0; i<TUBES_COUNT; i++){
|
|
Serial.printf("\n\nInitialiazing tube #%d\n", i);
|
|
change_pin(all_xshuts[i], HIGH);
|
|
Serial.println("Beginning boot sequence VL53L0X");
|
|
all_tubes[i].tof.hard.setTimeout(500);
|
|
while (!all_tubes[i].tof.hard.init()) {
|
|
Serial.println(F("Failed to boot VL53L0X"));
|
|
while(1);
|
|
}
|
|
all_tubes[i].tof.hard.setAddress(0x31 + i);
|
|
all_tubes[i].tof.hard.startContinuous();
|
|
|
|
}
|
|
Serial.println("Done setuping TOFs");
|
|
}
|
|
|
|
void calibrate_tube(int i){
|
|
Serial.printf("Calibrating tube %d.\n", i);
|
|
|
|
Serial.println("Setuping downards");
|
|
uint16_t h = mesureHeight(i);
|
|
int angle = minServo;
|
|
while (h > maxHeight) {
|
|
Serial.printf("%dmm %d°\n",h, angle);
|
|
++angle;
|
|
all_tubes[i].servo.hard.write(angle);
|
|
delay(300);
|
|
h = mesureHeight(i);
|
|
}
|
|
angle -= 5;
|
|
all_tubes[i].servo.last_angle = angle;
|
|
all_tubes[i].servo.hard.write(all_tubes[i].servo.last_angle);
|
|
all_tubes[i].servo.equilibrium = angle;
|
|
Serial.printf("Tube %d calibrated.\n", i);
|
|
}
|
|
|
|
void setup_servo(){
|
|
for (int i = 0; i<TUBES_COUNT; i++){
|
|
all_tubes[i].servo.pin = all_servos[i];
|
|
all_tubes[i].servo.hard.attach(all_tubes[i].servo.pin.id);
|
|
all_tubes[i].servo.hard.write(minServo);
|
|
}
|
|
}
|
|
|
|
// Helper functions
|
|
auto heightRatio(uint16_t h){
|
|
float height = h;
|
|
return max(min(1.0f, (height - minHeight) / (maxHeight-minHeight)), 0.0f);
|
|
}
|
|
|
|
int servoPosition(float ratio){
|
|
return (ratio * maxServo) + ((1-ratio) * minServo);
|
|
}
|
|
|
|
void setup() {
|
|
Serial.begin(115200); // Starts the serial communication
|
|
while (! Serial) {
|
|
delay(1);
|
|
}
|
|
Wire.begin();
|
|
//setup_servo();
|
|
delay(500);
|
|
|
|
//setup_servo(); // Backup ?
|
|
//delay(5000);
|
|
scan_I2c();
|
|
|
|
Serial.println("All servo should be fully opened");
|
|
|
|
|
|
|
|
|
|
if (!pcf8574.begin(0x38, &Wire)) {
|
|
Serial.println("Couldn't find PCF8574");
|
|
while (1);
|
|
}
|
|
|
|
for (uint8_t p=0; p<8; p++) {
|
|
pcf8574.pinMode(p, OUTPUT);
|
|
}
|
|
|
|
scan_I2c();
|
|
|
|
Serial.println("PCF8574 is OK");
|
|
wifisetup();
|
|
setup_all_tofs();
|
|
//for (int i = 0; i<TUBES_COUNT; i++){
|
|
// calibrate_tube(i);
|
|
//}
|
|
|
|
|
|
}
|
|
|
|
int get_avg_height(int i){
|
|
int h = 0;
|
|
++all_tubes[i].last_pos_index;
|
|
all_tubes[i].last_pos_index = all_tubes[i].last_pos_index % 5;
|
|
all_tubes[i].last_measure[all_tubes[i].last_pos_index] = mesureHeight(i);
|
|
for(int j = 0; j<5; j++){
|
|
h += all_tubes[i].last_measure[j];
|
|
}
|
|
return h/5;
|
|
}
|
|
|
|
void loop() {
|
|
for (int i = 0; i<TUBES_COUNT; i++){
|
|
|
|
all_tubes[i].height = get_avg_height(i);
|
|
Serial.printf("tof_%d:%d ",i, all_tubes[i].height);
|
|
|
|
}
|
|
Serial.println();
|
|
|
|
//Serial.printf("%d %d %d %d %d %d \n", val0, val1, val2, val3, val4, val5);
|
|
// Serial.printf("%d %d %d \n", val3, val4, val5);
|
|
//Serial.printf("%d %d \n", val0, val1);
|
|
|
|
delay(5);
|
|
}
|