feat: tester for differents coponents
This commit is contained in:
Pierre Tellier
parent
86a8d9d93d
commit
71432ff96c
333
servo_tester/servo_tester.ino
Normal file
333
servo_tester/servo_tester.ino
Normal file
@ -0,0 +1,333 @@
|
||||
#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 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 = 14, .is_extender = 0},
|
||||
{.id = 4, .is_extender = 0},
|
||||
{.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 = 50;
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// 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();
|
||||
all_tubes[i].servo.pin = all_servos[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
|
||||
while (! Serial) {
|
||||
delay(1);
|
||||
}
|
||||
|
||||
//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++){
|
||||
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);
|
||||
//calibrate_tube(i);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
int j = 0;
|
||||
|
||||
void loop() {
|
||||
for (int i = 0; i<TUBES_COUNT; i++){
|
||||
all_tubes[i].servo.hard.write(j%150);
|
||||
}
|
||||
j++;
|
||||
delay(100);
|
||||
Serial.println(j%150);
|
||||
}
|
||||
358
tof_tester/tof_tester.ino
Normal file
358
tof_tester/tof_tester.ino
Normal file
@ -0,0 +1,358 @@
|
||||
#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);
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user