#include <Arduino.h>

// NOTE: There is no built-in LED on the ESP32! (learned the hard way) (the red light indicates power and is not controllable)

// MARK: Define pins
// NOTE: Pins are grouped by destination. Each group is physically contiguous.

#define LEFT_FWD 32 // IN1, standard output
#define LEFT_REV 33 // IN2, standard output
#define LEFT_SPEED 25 // ENA, PWM output

#define RIGHT_FWD 26 // IN3, standard output
#define RIGHT_REV 27 // IN4, standard output
#define RIGHT_SPEED 14 // ENB, PWM output

#define CLAW 16 // MG90 PWM output
#define ARM 17 // SG90 PWM output

#define TRIG 22 // HC-SR04 standard output
#define ECHO 23 // HC-SR04 standard input. ECHO pin goes through voltage divider first.



// MARK: void setup()

void setup() {
  Serial.begin(115200);
  delay(1000);
  Serial.println("Hello World!");

  // Set mode of all pins.
  pinMode(LEFT_FWD, OUTPUT);
  pinMode(LEFT_REV, OUTPUT);
  pinMode(LEFT_SPEED, OUTPUT);
  pinMode(RIGHT_FWD, OUTPUT);
  pinMode(RIGHT_REV, OUTPUT);
  pinMode(RIGHT_SPEED, OUTPUT);
  pinMode(CLAW, OUTPUT);
  pinMode(ARM, OUTPUT);
  pinMode(TRIG, OUTPUT);
  pinMode(ECHO, INPUT); // The only INPUT pin.

  // Initial write.
  digitalWrite(LEFT_FWD, LOW);
  digitalWrite(LEFT_REV, LOW);
  analogWrite(LEFT_SPEED, 0);
  digitalWrite(RIGHT_FWD, LOW);
  digitalWrite(RIGHT_REV, LOW);
  analogWrite(RIGHT_SPEED, 0);
  digitalWrite(TRIG, LOW);
}



// MARK: void loop()

void loop() {
  // Test: Move left forward for 1 second, then stop.
  digitalWrite(LEFT_FWD, HIGH);
  analogWrite(LEFT_SPEED, 255); // Full speed.
  delay(1000);
  digitalWrite(LEFT_FWD, LOW);
  analogWrite(LEFT_SPEED, 0); // Stop.
}