#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.
}