Motor

#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>

// -------------------- OLED --------------------
#define OLED_ADDR 0x3C
Adafruit_SSD1306 display(128, 64, &Wire);

// -------------------- MOTOR PINS --------------------
// RIGHT motor
const int ENA = 3;
const int IN1 = 6;
const int IN2 = 7;
// LEFT motor
const int ENB = 5;
const int IN3 = 4;
const int IN4 = 2;

// initial speeds
int leftSpeed  = 90;
int rightSpeed = 75;

// -------------------- STATES --------------------
enum RobotState {
  NORMAL_FWD,
  REVERSING,
  ZIGZAG_LEFT,
  VOLTAGE_STOP
};
RobotState state = NORMAL_FWD;
unsigned long timing = 0;

// -------------------- SERIAL --------------------
String incomingData = "";
float distance = 0;
float solarV = 0;
float temp = 0;
float hum = 0;
float pres = 0;

// ----------------------------------------------------
// MOTOR CONTROL
// ----------------------------------------------------
void motorLeftForward(int spd)  { digitalWrite(IN3,HIGH); digitalWrite(IN4,LOW); analogWrite(ENB,spd); }
void motorLeftBackward(int spd) { digitalWrite(IN3,LOW); digitalWrite(IN4,HIGH); analogWrite(ENB,spd); }
void motorRightForward(int spd) { digitalWrite(IN1,HIGH); digitalWrite(IN2,LOW); analogWrite(ENA,spd); }
void motorRightBackward(int spd){ digitalWrite(IN1,LOW); digitalWrite(IN2,HIGH); analogWrite(ENA,spd); }
void motorsStop() { analogWrite(ENA,0); analogWrite(ENB,0); }

// ----------------------------------------------------
// OLED UPDATE
// ----------------------------------------------------
void updateOLED() {
  display.clearDisplay();
  display.setCursor(0,0);

  display.print("T:");
  display.print(temp,1); display.print("C ");
  display.print("H:");
  display.print(hum,1); display.print("%");

  display.setCursor(0,16);
  display.print("P:");
  display.print(pres,1); display.print("hPa");

  display.setCursor(0,32);
  display.print("Dist:");
  display.print(distance); display.print("cm ");

  display.setCursor(0,48);
  display.print("State:");
  if(state==NORMAL_FWD) display.print("FWD");
  else if(state==REVERSING) display.print("REV");
  else if(state==ZIGZAG_LEFT) display.print("ZIG-L");
  else if(state==VOLTAGE_STOP) display.print("V-STOP");
  display.print(" V:");
  display.print(solarV,2); display.println("V");

  display.display();
}

// ----------------------------------------------------
void setup() {
  Serial.begin(9600);

  pinMode(ENA, OUTPUT); pinMode(IN1, OUTPUT); pinMode(IN2, OUTPUT);
  pinMode(ENB, OUTPUT); pinMode(IN3, OUTPUT); pinMode(IN4, OUTPUT);

  Wire.begin();
  display.begin(SSD1306_SWITCHCAPVCC, OLED_ADDR);
  delay(200);
  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.display();

  timing = millis();
}

// ----------------------------------------------------
void loop() {

  // --- Read serial from Nano ---
  while(Serial.available()) {
    char c = Serial.read();
    if(c=='\n'){
      incomingData.trim();
      parseIncomingData(incomingData);
      incomingData="";
    } else incomingData += c;
  }

  // Check solar voltage first
  if(solarV > 1.0){
    state = VOLTAGE_STOP;
    motorsStop();
  }

  updateOLED();

  // --- State machine ---
  switch(state){

    case NORMAL_FWD:
      if(distance<30 && distance>0){
        state = REVERSING;
        timing = millis();
      } else {
        motorLeftForward(leftSpeed);
        motorRightForward(rightSpeed);
      }
      break;

    case REVERSING:
      motorLeftBackward(leftSpeed);
      motorRightBackward(rightSpeed);
      if(distance>30){
        state = ZIGZAG_LEFT;
        timing = millis();
      }
      break;

    case ZIGZAG_LEFT:
      motorLeftBackward(leftSpeed/2);
      motorRightForward(rightSpeed);
      if(millis()-timing>700){
        state = NORMAL_FWD;
      }
      break;

    case VOLTAGE_STOP:
      // motors already stopped
      break;
  }
}

// ----------------------------------------------------
// Parse CSV from Nano
// Example: TEMP:25,HUM:50,PRES:1013,DIST:40,SOL:0.95
// ----------------------------------------------------
void parseIncomingData(String data){
  int start=0;
  int end=data.indexOf(',');
  while(end!=-1){
    String value=data.substring(start,end);
    extractValue(value);
    start=end+1;
    end=data.indexOf(',',start);
  }
  if(start<data.length()){
    String value=data.substring(start);
    extractValue(value);
  }
}

// ----------------------------------------------------
void extractValue(String value){
  value.trim();
  if(value.startsWith("TEMP:")) temp = value.substring(5).toFloat();
  else if(value.startsWith("HUM:")) hum = value.substring(4).toFloat();
  else if(value.startsWith("PRES:")) pres = value.substring(5).toFloat();
  else if(value.startsWith("DIST:")) distance = value.substring(5).toFloat();
  else if(value.startsWith("SOL:")) solarV = value.substring(4).toFloat();
}