From 86a8d9d93d0a561bd49897b801f1b4b68a111bf3 Mon Sep 17 00:00:00 2001
From: Alessevan <44125445+Alessevan@users.noreply.github.com>
Date: Thu, 15 May 2025 12:36:28 +0200
Subject: [PATCH] feat: add leds

---
 prog/prog.ino | 155 ++++++++++++++++++++++++++++++++++++++++++++------
 1 file changed, 139 insertions(+), 16 deletions(-)

diff --git a/prog/prog.ino b/prog/prog.ino
index 1628665..203428e 100644
--- a/prog/prog.ino
+++ b/prog/prog.ino
@@ -1,3 +1,4 @@
+#include <Adafruit_NeoPixel.h>
 #include <ESP32Servo.h>
 #include <Wire.h>
 #include <VL53L0X.h>
@@ -5,6 +6,11 @@
 #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";
@@ -15,11 +21,21 @@ 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;
@@ -34,20 +50,28 @@ struct tube {
     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 = 7, .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 = 5, .is_extender = 0},
+    {.id = -1, .is_extender = 0},
     {.id = 18, .is_extender = 0},
     {.id = 19, .is_extender = 0},
     {.id = -1, .is_extender = 1},
@@ -60,17 +84,17 @@ const struct pin all_servos[6] = {
     {.id = 33, .is_extender = 0},
     {.id = 23, .is_extender = 0},
     {.id = 16, .is_extender = 0},
-    {.id = 14, .is_extender = 0},
+    {.id = 4, .is_extender = 0},
     {.id = 2, .is_extender = 0}
 };
 
 struct tube all_tubes[6];
-const int TUBES_COUNT = 1;
+const int TUBES_COUNT = 2;
 
 const float minHeight = 50;
 const float maxHeight = 445;
 
-int minServo = 50;
+int minServo = 0;
 int maxServo = 180;
 
 
@@ -137,6 +161,30 @@ void changePinMode(struct pin pin, int 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(
@@ -185,6 +233,7 @@ String processor(const String& var){
     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;
@@ -194,6 +243,9 @@ String processor(const String& var){
 
 
 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);
 
@@ -249,20 +301,23 @@ void setup_all_tofs() {
     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].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].servo.pin = all_servos[i];
-    
+    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].servo.hard.attach(all_tubes[i].servo.pin.id);
-  all_tubes[i].servo.hard.write(minServo);
+  all_tubes[i].state = STATE_INIT;
+  change_led(i);
   delay(5000);
   Serial.println("Setuping downards");
   uint16_t h = mesureHeight(i);
@@ -270,6 +325,13 @@ void calibrate_tube(int i){
   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);
@@ -279,6 +341,17 @@ void calibrate_tube(int i){
   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
@@ -292,14 +365,24 @@ int servoPosition(float ratio){
 }
 
 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);
   }
 
-  Wire.begin();
+  pixels.begin();
+  pixels.setBrightness(255);
+  pixels.show();
 
-scan_I2c();
+  setup_servos();
+  delay(5000);
+  setup_servos();
+  delay(5000);
+
+  Wire.begin();
 
   if (!pcf8574.begin(0x38, &Wire)) {
     Serial.println("Couldn't find PCF8574");
@@ -308,20 +391,58 @@ scan_I2c();
   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);
 
@@ -343,14 +464,16 @@ void loop() {
     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();
+  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);
+  
 }