// robot.ino - het inspectierobotje van de klimaatmodule (hoofdstuk 33)
// Arduino Uno, motordriver TB6612FNG, drie reflectiesensoren (A0-A2), lekdetectie (A3), HC-SR04, SHT31 op een mastje.
#include <Wire.h>
#include <Adafruit_SHT31.h>

// ---------------------------------------------------------------- pinnen
const int TRIG = 2, ECHO = 3, STBY = 4;
const int PWM_L = 5, IN1_L = 7, IN2_L = 8;                 // linkermotor (kanaal A)
const int PWM_R = 6, IN1_R = 9, IN2_R = 10;                // rechtermotor (kanaal B)
const int KNOP = 11, LAAD = 12, LED = 13;                  // startknop, laadcontacten, statusled
const int REFL_L = A0, REFL_M = A1, REFL_R = A2, LEK = A3;

// ---------------------------------------------------------------- instellingen
const float B_SPOOR = 0.14;                                // m tussen de twee wielen
const float V_MAX = 0.60;                                  // m/s bij PWM 255
const float D0 = 0.18;                                     // dode zone: onder 18 % PWM draait het wiel niet
const float V_RIJ = 0.20, V_TERUG = 0.12, V_DRAAI = 0.10;  // m/s
const float K_P = 0.25;                                    // rad/s per mm dat de lijn opzij ligt
const int WIT = 100, ZWART = 850;                          // ADC-waarden na het ijken
const float STOP_CM = 25.0, VRIJ_CM = 30.0;                // hysterese op de afstand
const unsigned long REGEL_MS = 20, SONAR_MS = 60;
const unsigned long METEN_MS = 10000, WACHT_MAX_MS = 20000, KWIJT_MS = 1500;
const unsigned long DRAAI_MIN_MS = 1800, DRAAI_MAX_MS = 5000;

enum Toestand { LADEN, VOLGEN, METEN, WACHTEN, KEREN, TERUG, FOUT };
const char* NAAM[] = {"LADEN", "VOLGEN", "METEN", "WACHTEN", "KEREN", "TERUG", "FOUT"};

Adafruit_SHT31 sht;
Toestand toestand = LADEN, naWachten = VOLGEN;
unsigned long sinds = 0, vorigeRegel = 0, vorigeSonar = 0, kwijtSinds = 0;
int punten = 0, kant = 1;                                  // meetpunten deze ronde; waar de lijn laatst lag
bool opStreep = false, kwijt = false;
float afstand = 999.0, luchtT = 20.0;

// ---------------------------------------------------------------- motoren (TB6612FNG)
void zetMotor(int pwm, int in1, int in2, float v) {       // v in m/s, negatief = achteruit
  float f = min(fabs(v) / V_MAX, 1.0);
  int d = (v == 0.0) ? 0 : (int)(255 * (D0 + (1.0 - D0) * f) + 0.5);   // de dode zone overbruggen
  digitalWrite(in1, v > 0 ? HIGH : LOW);
  digitalWrite(in2, v < 0 ? HIGH : LOW);
  analogWrite(pwm, d);
}

void rijden(float vL, float vR) {
  zetMotor(PWM_L, IN1_L, IN2_L, vL);
  zetMotor(PWM_R, IN1_R, IN2_R, vR);
}

// ---------------------------------------------------------------- sensoren
float wit01(int pin) {                                     // 0 = wit, 1 = zwart
  return constrain((analogRead(pin) - WIT) / (float)(ZWART - WIT), 0.0, 1.0);
}

float lijnPositie(bool& streep) {                          // mm, + = de lijn ligt links; NAN = kwijt
  float wl = wit01(REFL_L), wm = wit01(REFL_M), wr = wit01(REFL_R);
  streep = wl > 0.6 && wm > 0.6 && wr > 0.6;               // dwarsstreep: alle drie zwart
  float g = wl + wm + wr;
  if (g < 0.2) return NAN;
  return (wl * 15.0 - wr * 15.0) / g;                      // gewogen gemiddelde van +15, 0 en -15 mm
}

float meetAfstand() {                                      // HC-SR04: s = v * t / 2
  digitalWrite(TRIG, LOW);
  delayMicroseconds(2);
  digitalWrite(TRIG, HIGH);
  delayMicroseconds(10);
  digitalWrite(TRIG, LOW);
  unsigned long us = pulseIn(ECHO, HIGH, 25000UL);         // na 25 ms (4 m heen en terug) opgeven
  if (us == 0) return 999.0;                               // geen echo: niets binnen bereik
  float v = 331.3 + 0.606 * luchtT;                        // geluidssnelheid in m/s
  return us * 1e-6 * v / 2.0 * 100.0;                      // cm
}

// ---------------------------------------------------------------- toestanden
void melding(const char* tekst) {
  Serial.print(millis() / 1000.0, 1);
  Serial.print(" s  ");
  Serial.println(tekst);
}

void meetpunt() {                                          // temperatuur, vocht en lekdetectie
  float t, rv;
  sht.readBoth(&t, &rv);
  if (!isnan(t)) luchtT = t;
  Serial.print(millis() / 1000.0, 1);
  Serial.print(" s  meetpunt ");
  Serial.print(punten);
  Serial.print(": ");
  Serial.print(t, 1);
  Serial.print(" C, ");
  Serial.print(rv, 0);
  Serial.println(analogRead(LEK) < 500 ? " %, NAT: lek onder de goot!" : " %, droog");
}

void naar(Toestand nieuw, unsigned long nu) {
  toestand = nieuw;
  sinds = nu;
  melding(NAAM[nieuw]);
  switch (nieuw) {                                         // acties bij binnenkomen
    case KEREN:  rijden(-V_DRAAI, V_DRAAI); break;         // op de plaats draaien (tegen de klok in)
    case METEN:  rijden(0, 0); meetpunt(); break;
    case VOLGEN:
    case TERUG:  kwijt = false; break;
    default:     rijden(0, 0); break;                      // LADEN, WACHTEN, FOUT: stilstaan
  }
}

void volg(float pos, float v0) {                           // P-regelaar: draai naar de lijn toe
  if (isnan(pos)) pos = 15.0 * kant;                       // kwijt: zoek aan de kant waar ze laatst lag
  else kant = pos > 0 ? 1 : -1;
  float omega = K_P * pos;                                 // rad/s
  rijden(v0 - omega * B_SPOOR / 2, v0 + omega * B_SPOOR / 2);
}

bool lijnKwijt(float pos, unsigned long nu) {
  if (!isnan(pos)) { kwijt = false; return false; }
  if (!kwijt) { kwijt = true; kwijtSinds = nu; }
  return nu - kwijtSinds > KWIJT_MS;
}

void setup() {
  int uit[] = {TRIG, STBY, PWM_L, IN1_L, IN2_L, PWM_R, IN1_R, IN2_R, LED};
  for (int p : uit) pinMode(p, OUTPUT);
  pinMode(ECHO, INPUT);
  pinMode(KNOP, INPUT_PULLUP);
  pinMode(LAAD, INPUT);
  digitalWrite(STBY, HIGH);                                // motordriver aan
  Serial.begin(115200);
  Wire.begin();
  sht.begin(0x44);
  naar(digitalRead(LAAD) == HIGH ? LADEN : FOUT, millis());   // niet op het laadstation: eerst terugzetten
}

void loop() {
  unsigned long nu = millis();
  if (nu - vorigeSonar >= SONAR_MS) {                      // hoogstens om de 60 ms meten (datasheet)
    vorigeSonar += SONAR_MS;
    afstand = meetAfstand();
  }
  if (nu - vorigeRegel < REGEL_MS) return;                 // 50 keer per seconde beslissen
  vorigeRegel += REGEL_MS;
  bool streep;
  float pos = lijnPositie(streep);
  bool nieuweStreep = streep && !opStreep;
  opStreep = streep;
  unsigned long t = nu - sinds;
  digitalWrite(LED, toestand == FOUT ? (nu / 200) % 2 : toestand != LADEN);

  switch (toestand) {
    case LADEN:
      if (digitalRead(KNOP) == LOW) { punten = 0; naar(VOLGEN, nu); }
      break;
    case VOLGEN:
      if (afstand < STOP_CM) { naWachten = VOLGEN; naar(WACHTEN, nu); }
      else if (nieuweStreep) { punten++; naar(METEN, nu); }
      else if (lijnKwijt(pos, nu)) naar(FOUT, nu);
      else volg(pos, V_RIJ);
      break;
    case METEN:
      if (t >= METEN_MS) naar(punten >= 3 ? TERUG : VOLGEN, nu);
      break;
    case WACHTEN:
      if (afstand > VRIJ_CM && t >= 1000) naar(naWachten, nu);
      else if (t >= WACHT_MAX_MS) {
        if (naWachten == VOLGEN) { melding("gang geblokkeerd: omkeren"); naar(KEREN, nu); }
        else { melding("vast: de weg terug is ook geblokkeerd"); naar(FOUT, nu); }
      }
      break;
    case KEREN:
      if (t >= DRAAI_MIN_MS && wit01(REFL_M) > 0.6) naar(TERUG, nu);   // lijn weer onder het midden
      else if (t >= DRAAI_MAX_MS) naar(FOUT, nu);
      break;
    case TERUG:                                            // trager, dwarsstrepen tellen niet meer
      if (digitalRead(LAAD) == HIGH) naar(LADEN, nu);
      else if (afstand < STOP_CM) { naWachten = TERUG; naar(WACHTEN, nu); }
      else if (lijnKwijt(pos, nu)) naar(FOUT, nu);
      else volg(pos, V_TERUG);
      break;
    case FOUT:                                             // stil en knipperen tot iemand hem op het laadstation zet
      if (digitalRead(LAAD) == HIGH) naar(LADEN, nu);
      break;
  }
}