480 lines
12 KiB
C++
480 lines
12 KiB
C++
#include <Adafruit_NeoPixel.h>
|
|
#include <ESP32Servo.h>
|
|
#include <Wire.h>
|
|
#include <VL53L0X.h>
|
|
#include <Adafruit_PCF8574.h>
|
|
#include <WiFi.h>
|
|
#include <AsyncTCP.h>
|
|
#include <ESPAsyncWebServer.h>
|
|
#ifdef __AVR__
|
|
#include <avr/power.h>
|
|
#endif
|
|
#define PIN 5
|
|
#define NUMPIXELS 6
|
|
|
|
// 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);
|
|
|
|
Adafruit_NeoPixel pixels(NUMPIXELS, PIN, NEO_GRB + NEO_KHZ800);
|
|
|
|
struct pin {
|
|
int id;
|
|
int is_extender;
|
|
};
|
|
|
|
|
|
enum state {
|
|
STATE_NONE,
|
|
STATE_INIT,
|
|
STATE_OK,
|
|
STATE_ERROR
|
|
};
|
|
|
|
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;
|
|
enum state state;
|
|
int id;
|
|
};
|
|
|
|
enum mode {
|
|
NONE,
|
|
SINUS,
|
|
};
|
|
|
|
enum mode current_mode = NONE;
|
|
|
|
const struct pin all_xshuts[6] = {
|
|
{.id = 17, .is_extender = 0},
|
|
{.id = 2, .is_extender = 1},
|
|
{.id = 3, .is_extender = 1},
|
|
{.id = 5, .is_extender = 1},
|
|
{.id = 7, .is_extender = 1},
|
|
{.id = 1, .is_extender = 1}
|
|
};
|
|
|
|
const struct pin all_gpios[6] = {
|
|
{.id = -1, .is_extender = 0},
|
|
{.id = 18, .is_extender = 0},
|
|
{.id = 19, .is_extender = 0},
|
|
{.id = -1, .is_extender = 1},
|
|
{.id = 6, .is_extender = 1},
|
|
{.id = 0, .is_extender = 1}
|
|
};
|
|
|
|
const struct pin all_servos[6] = {
|
|
{.id = 32, .is_extender = 0},
|
|
{.id = 33, .is_extender = 0},
|
|
{.id = 23, .is_extender = 0},
|
|
{.id = 16, .is_extender = 0},
|
|
{.id = 4, .is_extender = 0},
|
|
{.id = 2, .is_extender = 0}
|
|
};
|
|
|
|
struct tube all_tubes[6];
|
|
const int TUBES_COUNT = 2;
|
|
|
|
const float minHeight = 50;
|
|
const float maxHeight = 445;
|
|
|
|
int minServo = 0;
|
|
int maxServo = 180;
|
|
|
|
|
|
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);
|
|
}
|
|
}
|
|
|
|
void change_led(int id) {
|
|
Serial.print("Changing led color of #");
|
|
Serial.println(id);
|
|
int led_id = NUMPIXELS -1 - id;
|
|
switch (all_tubes[id].state) {
|
|
case STATE_INIT: {
|
|
pixels.setPixelColor(led_id, pixels.Color(255, 210, 70));
|
|
break;
|
|
}
|
|
case STATE_OK: {
|
|
pixels.setPixelColor(led_id, pixels.Color(0, 0, 190));
|
|
break;
|
|
}
|
|
case STATE_ERROR: {
|
|
pixels.setPixelColor(led_id, pixels.Color(0, 190, 0));
|
|
break;
|
|
}
|
|
default: {
|
|
pixels.setPixelColor(led_id, pixels.Color(3, 3, 3));
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// 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() {
|
|
#if defined(__AVR_ATtiny85__) && (F_CPU == 16000000)
|
|
clock_prescale_set(clock_div_1);
|
|
#endif
|
|
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"));
|
|
all_tubes[i].state = STATE_ERROR;
|
|
change_led(i);
|
|
pixels.show();
|
|
goto error;
|
|
}
|
|
all_tubes[i].tof.hard.setAddress(0x31 + i);
|
|
all_tubes[i].tof.hard.startContinuous();
|
|
all_tubes[i].state = STATE_NONE;
|
|
error:
|
|
}
|
|
Serial.println("Done setuping TOFs");
|
|
}
|
|
|
|
void calibrate_tube(int i){
|
|
Serial.printf("Calibrating tube %d.\n", i);
|
|
all_tubes[i].state = STATE_INIT;
|
|
change_led(i);
|
|
delay(5000);
|
|
Serial.println("Setuping downards");
|
|
uint16_t h = mesureHeight(i);
|
|
int angle = minServo;
|
|
while (h > maxHeight) {
|
|
Serial.printf("%dmm %d°\n",h, angle);
|
|
++angle;
|
|
if (angle > maxServo) {
|
|
all_tubes[i].state = STATE_ERROR;
|
|
Serial.printf("Tube %d HS !\n", i);
|
|
change_led(i);
|
|
pixels.show();
|
|
return;
|
|
}
|
|
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);
|
|
all_tubes[i].state = STATE_OK;
|
|
change_led(i);
|
|
pixels.show();
|
|
}
|
|
|
|
void setup_servos() {
|
|
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() {
|
|
#if defined(__AVR_ATtiny85__) && (F_CPU == 16000000)
|
|
clock_prescale_set(clock_div_1);
|
|
#endif
|
|
Serial.begin(115200); // Starts the serial communication
|
|
while (! Serial) {
|
|
delay(1);
|
|
}
|
|
|
|
pixels.begin();
|
|
pixels.setBrightness(255);
|
|
pixels.show();
|
|
|
|
setup_servos();
|
|
delay(5000);
|
|
setup_servos();
|
|
delay(5000);
|
|
|
|
Wire.begin();
|
|
|
|
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 sinus_center = 300;
|
|
int sinus_radius = 100;
|
|
float step = 0;
|
|
float rate = 0.01;
|
|
|
|
void loop() {
|
|
|
|
|
|
|
|
pixels.clear();
|
|
pixels.show();
|
|
Serial.println("Cleared");
|
|
delay(500);
|
|
for(int i=0; i<NUMPIXELS; i++) {
|
|
Serial.print("Set light");
|
|
Serial.println(i);
|
|
pixels.setPixelColor(i, pixels.Color(0, 150, 0));
|
|
pixels.show();
|
|
|
|
delay(500);
|
|
}
|
|
switch (current_mode) {
|
|
case SINUS: {
|
|
for (int i = 0; i < TUBES_COUNT; i++) {
|
|
all_tubes[i].target = 300 + ((int) sinus_radius * sin(rate * (step + i * 0.1) * 3.14));
|
|
}
|
|
step += 0.1;
|
|
break;
|
|
}
|
|
default: {
|
|
}
|
|
}
|
|
for (int i = 0; i<TUBES_COUNT; i++){
|
|
change_led(i);
|
|
if (all_tubes[i].state != STATE_OK) {
|
|
continue;
|
|
}
|
|
all_tubes[i].height = mesureHeight(i);
|
|
Serial.printf("%d", all_tubes[i].height);
|
|
|
|
int angle = 0;
|
|
int diff = all_tubes[i].height - all_tubes[i].target;
|
|
|
|
|
|
if (abs(diff) > 5.0f) {
|
|
angle = all_tubes[i].servo.equilibrium + (diff * 1 / (7));
|
|
} else if (abs(diff) > 2.0f) {
|
|
angle = all_tubes[i].servo.equilibrium + (diff * 1 / (7));
|
|
} else {
|
|
angle = all_tubes[i].servo.equilibrium;
|
|
}
|
|
|
|
Serial.printf("%d - %d = %d => %d\n", all_tubes[i].height, all_tubes[i].target, diff, angle);
|
|
|
|
|
|
all_tubes[i].servo.last_angle = min(max(angle, minServo), maxServo);
|
|
all_tubes[i].servo.hard.write(all_tubes[i].servo.last_angle);
|
|
|
|
}
|
|
pixels.show();
|
|
|
|
// 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);
|
|
|
|
}
|
|
|