feat: mutiple tubes
This commit is contained in:
Pierre Tellier
parent
ca35d80054
commit
a66fef2516
444
prog/prog.ino
444
prog/prog.ino
@ -1,53 +1,19 @@
|
||||
#include <ESP32Servo.h>
|
||||
#include <Wire.h>
|
||||
#include "Adafruit_VL53L0X.h"
|
||||
#include <VL53L0X.h>
|
||||
#include <Adafruit_PCF8574.h>
|
||||
#include <WiFi.h>
|
||||
#include <AsyncTCP.h>
|
||||
#include <ESPAsyncWebServer.h>
|
||||
|
||||
/*
|
||||
#include "terminal.hpp"
|
||||
//
|
||||
// ATTENTION : pour que les commandes se déclarent automatiquement au démarrage
|
||||
// auprès du terminal, le fichier conteant la fonction setup() et loop() doit
|
||||
// être mis en derniere position dans la liste des fichiers lors de l'édition
|
||||
// des liens.
|
||||
// Dans platformio, pour faire cela, il suffit de commencer le nom du fichier
|
||||
// contenant les fonctions loop() et setup() par la lettre z.
|
||||
// Par exemple : zzz_main.cpp.
|
||||
//
|
||||
// Web server configuration
|
||||
const char* ssid = "ESP_ping";
|
||||
const char* password = "pong";
|
||||
|
||||
extern terminal_n::Terminal terminal;
|
||||
|
||||
|
||||
TERMINAL_PARAMETER_INT(
|
||||
terminal,
|
||||
power_mode, // variable name to declare
|
||||
"power_mode", // EEPROM name (15 char. max),
|
||||
// If you dont want to save in EEPROM, set ""
|
||||
"the power mode", // description
|
||||
0, // default constructor value
|
||||
false, // auto save in eeprom after command modification
|
||||
terminal_n::filter_nothing, // function to filter the value
|
||||
terminal_n::do_nothing // function executed at the end of the command
|
||||
);
|
||||
|
||||
TERMINAL_COMMAND(terminal, ping, "Print pong followoing by the received arguments")
|
||||
{
|
||||
terminal.print("pong");
|
||||
for (uint32_t i=0; i<argc; i++) {
|
||||
terminal.print(" ");
|
||||
terminal.print(argv[i]);
|
||||
}
|
||||
terminal.println("");
|
||||
}
|
||||
TERMINAL_ADD_DEFAULT_COMMANDS(terminal) // help, params, load_config, reset_config, echo ...
|
||||
|
||||
terminal_n::Terminal terminal("Pingpong flottant.\n\r\n\r");
|
||||
|
||||
*/
|
||||
const char* PARAM_INPUT_1 = "output";
|
||||
const char* PARAM_INPUT_2 = "state";
|
||||
|
||||
AsyncWebServer server(80);
|
||||
|
||||
struct pin {
|
||||
int id;
|
||||
@ -55,8 +21,8 @@ struct pin {
|
||||
};
|
||||
|
||||
struct tube {
|
||||
float height;
|
||||
float target;
|
||||
uint16_t height;
|
||||
uint16_t target;
|
||||
struct {
|
||||
Servo hard;
|
||||
struct pin pin;
|
||||
@ -64,32 +30,114 @@ struct tube {
|
||||
int equilibrium;
|
||||
} servo;
|
||||
struct {
|
||||
Adafruit_VL53L0X hard;
|
||||
VL53L0X hard;
|
||||
struct pin gpio;
|
||||
struct pin xshut;
|
||||
} tof;
|
||||
int id;
|
||||
};
|
||||
|
||||
const struct pin all_xshuts[6] = {
|
||||
{.id = 17, .is_extender = 0},
|
||||
{.id = 2, .is_extender = 1},
|
||||
{.id = 3, .is_extender = 1},
|
||||
{.id = 7, .is_extender = 1},
|
||||
{.id = 5, .is_extender = 1},
|
||||
{.id = 1, .is_extender = 1}
|
||||
};
|
||||
|
||||
const int TUBES_COUNT = 6;
|
||||
struct tube all_tubes[6] = {0};
|
||||
const struct pin all_gpios[6] = {
|
||||
{.id = 5, .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 = 14, .is_extender = 0},
|
||||
{.id = 2, .is_extender = 0}
|
||||
};
|
||||
|
||||
struct tube all_tubes[6];
|
||||
const int TUBES_COUNT = 1;
|
||||
|
||||
const float minHeight = 50;
|
||||
const float maxHeight = 445;
|
||||
|
||||
int minServo = 0;
|
||||
int minServo = 50;
|
||||
int maxServo = 180;
|
||||
|
||||
// Replace with your network credentials
|
||||
const char* ssid = "ESP_ping";
|
||||
const char* password = "pong";
|
||||
|
||||
const char* PARAM_INPUT_1 = "output";
|
||||
const char* PARAM_INPUT_2 = "state";
|
||||
Adafruit_PCF8574 pcf8574;
|
||||
|
||||
// Create AsyncWebServer object on port 80
|
||||
AsyncWebServer server(80);
|
||||
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>
|
||||
@ -128,7 +176,6 @@ const char index_html[] PROGMEM = R"rawliteral(
|
||||
)rawliteral";
|
||||
|
||||
|
||||
// Replaces placeholder with button section in your web page
|
||||
String processor(const String& var){
|
||||
//Serial.println(var);
|
||||
if(var == "BUTTONPLACEHOLDER"){
|
||||
@ -146,26 +193,6 @@ String processor(const String& var){
|
||||
}
|
||||
|
||||
|
||||
auto mesureHeight(Adafruit_VL53L0X lox, struct tube *tube){
|
||||
VL53L0X_RangingMeasurementData_t measure;
|
||||
lox.rangingTest(&measure, false); // pass in 'true' to get debug data printout!
|
||||
if (measure.RangeStatus != 4) { // phase failures have incorrect data
|
||||
float height = measure.RangeMilliMeter;
|
||||
if (height < 1000.0){
|
||||
tube->height = height;
|
||||
return height;
|
||||
} else {
|
||||
Serial.println("Incoherent measurement. Returning previous one.");
|
||||
return tube->height;
|
||||
}
|
||||
}
|
||||
else {
|
||||
//Serial.println(" out of range ");
|
||||
return tube->height;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void wifisetup() {
|
||||
Serial.print("Setting AP (Access Point)…");
|
||||
WiFi.softAP(ssid);
|
||||
@ -181,17 +208,19 @@ void wifisetup() {
|
||||
|
||||
// Send a GET request to <ESP_IP>/update?output=<inputMessage1>&state=<inputMessage2>
|
||||
server.on("/update", HTTP_GET, [] (AsyncWebServerRequest *request) {
|
||||
String inputMessage1;
|
||||
String inputMessage2;
|
||||
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();
|
||||
inputMessage2 = request->getParam(PARAM_INPUT_2)->value();
|
||||
all_tubes[0].target = inputMessage2.toFloat();
|
||||
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 {
|
||||
inputMessage1 = "No message sent";
|
||||
inputMessage2 = "No message sent";
|
||||
// Error
|
||||
inputMessage1 = -1;
|
||||
inputMessage2 = -1;
|
||||
}
|
||||
Serial.print("GPIO: ");
|
||||
Serial.print(inputMessage1);
|
||||
@ -204,128 +233,63 @@ void wifisetup() {
|
||||
server.begin();
|
||||
}
|
||||
|
||||
Adafruit_PCF8574 pcf8574;
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
Adafruit_VL53L0X tofs_talk_through[6] = {Adafruit_VL53L0X(), Adafruit_VL53L0X(), Adafruit_VL53L0X(), Adafruit_VL53L0X(), Adafruit_VL53L0X(), Adafruit_VL53L0X()};
|
||||
|
||||
|
||||
|
||||
void init_tube(struct tube *tube, char id, struct pin servo_pin, struct pin gpio, struct pin xshut) {
|
||||
Serial.print(" Initialiazing tube #");
|
||||
Serial.println((int) id);
|
||||
tube->id = id;
|
||||
tube->servo.pin = servo_pin;
|
||||
tube->tof.gpio = gpio;
|
||||
tube->tof.xshut = xshut;
|
||||
tube->height = 250.0f;
|
||||
tube->target = 250.0f;
|
||||
change_pin(gpio, HIGH);
|
||||
change_pin(xshut, HIGH);
|
||||
//change_pin(servo_pin, HIGH);
|
||||
Serial.println("Beginning boot sequence VL53L0X");
|
||||
/*
|
||||
if (!tofs_talk_through[id].begin()) {
|
||||
Serial.println(F("Failed to boot VL53L0X"));
|
||||
Serial.println((int)id);
|
||||
while(1);
|
||||
}*/
|
||||
while (!tofs_talk_through[id].begin()) {
|
||||
Serial.println(F("Failed to boot VL53L0X"));
|
||||
Serial.println("Adafruit VL53L0X XShut set Low to Force HW Reset");
|
||||
digitalWrite(xshut.id, LOW);
|
||||
delay(100);
|
||||
digitalWrite(xshut.id, HIGH);
|
||||
Serial.println("Adafruit VL53L0X XShut set high to Allow Boot");
|
||||
delay(5000);
|
||||
}
|
||||
tofs_talk_through[id].setAddress(0x30 + id);
|
||||
tube->servo.hard.attach(tube->servo.pin.id);
|
||||
tube->servo.hard.write(minServo);
|
||||
delay(5000);
|
||||
Serial.println("Setuping downards");
|
||||
float h = mesureHeight(tofs_talk_through[id], tube);
|
||||
Serial.println(h);
|
||||
int angle = minServo;
|
||||
while (h > maxHeight) {
|
||||
++angle;
|
||||
tube->servo.hard.write(angle);
|
||||
delay(300);
|
||||
h = mesureHeight(tofs_talk_through[id], tube);
|
||||
}
|
||||
angle -= 5;
|
||||
tube->servo.last_angle = angle;
|
||||
tube->servo.hard.write(tube->servo.last_angle);
|
||||
tube->servo.equilibrium = angle;
|
||||
Serial.println("Done.");
|
||||
}
|
||||
|
||||
|
||||
|
||||
void setup_all_tubes() {
|
||||
const struct pin all_xshuts[6] = {
|
||||
{.id = 17, .is_extender = 0},
|
||||
{.id = 2, .is_extender = 1},
|
||||
{.id = 3, .is_extender = 1},
|
||||
{.id = 7, .is_extender = 1},
|
||||
{.id = 5, .is_extender = 1},
|
||||
{.id = 1, .is_extender = 1}
|
||||
};
|
||||
const struct pin all_gpios[6] = {
|
||||
{.id = 5, .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 = 14, .is_extender = 0},
|
||||
{.id = 2, .is_extender = 0}
|
||||
};
|
||||
|
||||
// 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);
|
||||
}
|
||||
delay(1000);
|
||||
/*
|
||||
for (int i = 0; i < TUBES_COUNT; ++i) {
|
||||
|
||||
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();
|
||||
all_tubes[i].servo.pin = all_servos[i];
|
||||
|
||||
}
|
||||
delay(10);
|
||||
for (int i = 0; i < TUBES_COUNT; ++i) {
|
||||
change_pin(all_xshuts[i], LOW);
|
||||
}
|
||||
*/
|
||||
|
||||
for (int i = 0; i < TUBES_COUNT; ++i) {
|
||||
init_tube(all_tubes, i, all_servos[i], all_gpios[i], all_xshuts[i]);
|
||||
}
|
||||
|
||||
Serial.println("Done setuping TOFs");
|
||||
}
|
||||
|
||||
void calibrate_tube(int i){
|
||||
Serial.printf("Calibrating tube %d.\n", i);
|
||||
all_tubes[i].servo.hard.attach(all_tubes[i].servo.pin.id);
|
||||
all_tubes[i].servo.hard.write(minServo);
|
||||
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;
|
||||
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);
|
||||
}
|
||||
|
||||
// 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
|
||||
@ -333,6 +297,10 @@ void setup() {
|
||||
delay(1);
|
||||
}
|
||||
|
||||
Wire.begin();
|
||||
|
||||
scan_I2c();
|
||||
|
||||
if (!pcf8574.begin(0x38, &Wire)) {
|
||||
Serial.println("Couldn't find PCF8574");
|
||||
while (1);
|
||||
@ -341,70 +309,48 @@ void setup() {
|
||||
pcf8574.pinMode(p, OUTPUT);
|
||||
}
|
||||
|
||||
Serial.println("PCF8574 seemks OK");
|
||||
|
||||
Serial.println("PCF8574 is OK");
|
||||
wifisetup();
|
||||
setup_all_tofs();
|
||||
for (int i = 0; i<TUBES_COUNT; i++){
|
||||
calibrate_tube(i);
|
||||
}
|
||||
|
||||
|
||||
//terminal.setup(&Serial);
|
||||
|
||||
setup_all_tubes();
|
||||
|
||||
Serial.println("Done setuping have funning");
|
||||
}
|
||||
|
||||
|
||||
auto heightRatio(float height){
|
||||
return max(min(1.0f, (height - minHeight) / (maxHeight-minHeight)), 0.0f);
|
||||
}
|
||||
|
||||
int servoPosition(float ratio){
|
||||
return (ratio * maxServo) + ((1-ratio) * minServo);
|
||||
}
|
||||
|
||||
void loop() {
|
||||
//terminal.update();
|
||||
for (int i = 0; i < TUBES_COUNT; i++) {
|
||||
struct tube *tube = all_tubes + i;
|
||||
/*
|
||||
Serial.print("Tube ");
|
||||
Serial.println(tube->id);
|
||||
*/
|
||||
tube->height = mesureHeight(tofs_talk_through[tube->id], tube);
|
||||
/*
|
||||
Serial.print("Measure = ");
|
||||
Serial.println(tube->height);
|
||||
*/
|
||||
int angle = 0;
|
||||
float diff = tube->height - tube->target;
|
||||
if (abs(diff) > 5.0f) {
|
||||
angle = tube->servo.equilibrium + (diff * 1 / (7));
|
||||
} else if (abs(diff) > 2.0f) {
|
||||
angle = tube->servo.equilibrium + (diff * 1 / (7));
|
||||
} else {
|
||||
angle = tube->servo.equilibrium;
|
||||
}
|
||||
/*
|
||||
Serial.print("angle = ");
|
||||
Serial.print(angle);
|
||||
Serial.print("; equilibrium = ");
|
||||
Serial.println(tube->servo.equilibrium);
|
||||
*/
|
||||
tube->servo.last_angle = min(max(angle, minServo), maxServo);
|
||||
/* Serial.print("Diff = ");
|
||||
Serial.print(diff);
|
||||
Serial.print("; Requested = ");
|
||||
Serial.print(tube->target);
|
||||
Serial.print("; Angle = ");
|
||||
Serial.println(tube->servo.last_angle);
|
||||
*/
|
||||
tube->servo.hard.write(tube->servo.last_angle);
|
||||
}
|
||||
|
||||
for (int i = 0; i<TUBES_COUNT; i++){
|
||||
Serial.print((&all_tubes[i])->height);
|
||||
Serial.print("\t");
|
||||
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);
|
||||
|
||||
|
||||
}
|
||||
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(1);
|
||||
delay(5);
|
||||
}
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user