Voice-Controlled ASR Robot

// This sketch is designed to run on the M5StickC Plus 2
// and interface with the M5 ASR Unit (UART) and the 8-Servo Hat (I2C).

// --- Libraries ---
#include <M5Unified.h>
#include <unit_asr.hpp> // Official library for the M5 ASR Unit
#include <Wire.h>       // For I2C communication with the Servo Hat
#include "Hat_8Servos.h" // Library for the M5 8-Servo Hat

// --- ASR Constants ---
const int ASR_RX_PIN = 33; 
const int ASR_TX_PIN = 32; 
HardwareSerial ASRSerial(2);
ASRUnit asr;

// --- I2C & Servo Hat Constants ---
#define SERVOS_SDA_PIN 0    
#define SERVOS_SCL_PIN 26   
#define SERVOS_I2C_SPEED 25000UL // Stable I2C speed for Hat
#define SERVOS_DEVICE_ADDR 0x36 

// --- MOTOR DEFINITIONS ---
const int LEFT_MOTOR_PORT = 0; // Servo port 0
const int RIGHT_MOTOR_PORT = 4; // Servo port 4

const int STOP_POS = 90; // Neutral angle for continuous servos (stop)

// --- CALIBRATION CONSTANTS (Adjust these for your specific robot) ---
// Note: 90 is stop. 180 is max speed one direction, 0 is max speed other direction.
const int CALIBRATED_FWD_LEFT = 180;    
const int CALIBRATED_FWD_RIGHT = 0; 

const int CALIBRATED_BACK_LEFT = 0;    // Reverse of FWD_LEFT
const int CALIBRATED_BACK_RIGHT = 180; // Reverse of FWD_RIGHT

// Simple Pivot Turn: One wheel drives, one wheel stops
const int CALIBRATED_LEFT_TURN_LEFT = CALIBRATED_BACK_LEFT;   // Left wheel stops
const int CALIBRATED_LEFT_TURN_RIGHT = CALIBRATED_FWD_RIGHT; // Right wheel drives FWD

const int CALIBRATED_RIGHT_TURN_LEFT = CALIBRATED_FWD_LEFT;   // Left wheel drives FWD
const int CALIBRATED_RIGHT_TURN_RIGHT = CALIBRATED_BACK_RIGHT;

// --- TIME/STATE MANAGEMENT ---
unsigned long motionStartTimeMs = 0;
unsigned long currentMotionDurationMs = 0; // If 0, no timed motion is active

Hat_8Servos drive; 

// Array to hold the current angle/speed for all 8 servos
uint8_t currentServoAngles[8] = {STOP_POS, STOP_POS, STOP_POS, STOP_POS, STOP_POS, STOP_POS, STOP_POS, STOP_POS};

// --- MOTOR CONTROL HELPER ---
void setMotorSpeeds(int left, int right) {
    currentServoAngles[LEFT_MOTOR_PORT] = left;
    currentServoAngles[RIGHT_MOTOR_PORT] = right;
    // Send the command to the physical servo hat
    drive.setServoAnglesFromArray(currentServoAngles); 
}


// --- DRIVE COMMAND HANDLERS ---

void stopHandler(); // Forward declaration for timed movement

void forwardHandler()
{
    // Clear any active timed movement
    currentMotionDurationMs = 0; 
    setMotorSpeeds(CALIBRATED_FWD_LEFT, CALIBRATED_FWD_RIGHT);

    Serial.println("Command: FORWARD (Continuous)");
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_GREEN);
    M5.Display.println("-> MOVING FORWARD");
}

void backwardHandler()
{
    // Clear any active timed movement
    currentMotionDurationMs = 0; 
    setMotorSpeeds(CALIBRATED_BACK_LEFT, CALIBRATED_BACK_RIGHT);

    Serial.println("Command: BACKWARD (Continuous)");
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_ORANGE);
    M5.Display.println("-> MOVING BACKWARD");
}

void turnLeftHandler()
{
    // Set 1 second duration for the turn
    currentMotionDurationMs = 850; 
    motionStartTimeMs = millis();
    setMotorSpeeds(CALIBRATED_LEFT_TURN_LEFT, CALIBRATED_LEFT_TURN_RIGHT);

    Serial.println("Command: LEFT (Timed for 850ms)");
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_YELLOW);
    M5.Display.println("-> TURNING LEFT");
}

void turnRightHandler()
{
    // Set 1 second duration for the turn
    currentMotionDurationMs = 850; 
    motionStartTimeMs = millis();
    setMotorSpeeds(CALIBRATED_RIGHT_TURN_LEFT, CALIBRATED_RIGHT_TURN_RIGHT);

    Serial.println("Command: RIGHT (Timed for 850ms)");
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_YELLOW);
    M5.Display.println("-> TURNING RIGHT");
}

void leftHandler()
{
    // Set 1 second duration for the turn
    currentMotionDurationMs = 200; 
    motionStartTimeMs = millis();
    setMotorSpeeds(CALIBRATED_LEFT_TURN_LEFT, CALIBRATED_LEFT_TURN_RIGHT);

    Serial.println("Command: LEFT (Timed for 850ms)");
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_YELLOW);
    M5.Display.println("-> TURNING LEFT");
}

void rightHandler()
{
    // Set 1 second duration for the turn
    currentMotionDurationMs = 200; 
    motionStartTimeMs = millis();
    setMotorSpeeds(CALIBRATED_RIGHT_TURN_LEFT, CALIBRATED_RIGHT_TURN_RIGHT);

    Serial.println("Command: RIGHT (Timed for 850ms)");
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_YELLOW);
    M5.Display.println("-> TURNING RIGHT");
}

void aboutTurnHandler()
{
    // Set 1 second duration for the turn
    currentMotionDurationMs = 1670; 
    motionStartTimeMs = millis();
    setMotorSpeeds(CALIBRATED_RIGHT_TURN_LEFT, CALIBRATED_RIGHT_TURN_RIGHT);

    Serial.println("About turn (Timed for 1650ms)");
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_YELLOW);
    M5.Display.println("-> TURNING RIGHT");
}

void stopHandler()
{
    // Clear any active timed movement
    currentMotionDurationMs = 0; 
    setMotorSpeeds(STOP_POS, STOP_POS);

    Serial.println("Command: STOP"); 
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_RED);
    M5.Display.println("-> STOPPED");
}

void numberCommandReceivedHandler()
{
    // Command IDs for numbers are 0x21 ('one') through 0x29 ('nine').
    uint8_t commandId = asr.getCurrentCommandNum();
    uint8_t durationSeconds = commandId - 0x20; // 0x21-0x20=1, 0x29-0x20=9
    
    // Set timed movement
    currentMotionDurationMs = durationSeconds * 1000;
    motionStartTimeMs = millis();
    setMotorSpeeds(CALIBRATED_FWD_LEFT, CALIBRATED_FWD_RIGHT);

    Serial.printf("Command: %s. Moving FORWARD for %d seconds\n", asr.getCurrentCommandWord().c_str(), durationSeconds);
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_BLUE);
    M5.Display.printf("-> FWD for %d secs (%s)", durationSeconds, asr.getCurrentCommandWord().c_str());
}

// --- General ASR Handlers (Kept for completeness) ---

void turnOnHandler()
{
    Serial.println("turn on command received!");
    M5.Display.setCursor(0, 60); 
    M5.Display.fillRect(0, 60, M5.Display.width(), 20, TFT_BLACK);
    M5.Display.setTextColor(TFT_GREEN);
    M5.Display.println("Status: Turn On");
}

void turnOffHandler()
{
    Serial.println("turn off command received!");
    M5.Display.setCursor(0, 60); 
    M5.Display.fillRect(0, 60, M5.Display.width(), 20, TFT_BLACK);
    M5.Display.setTextColor(TFT_RED);
    M5.Display.println("Status: Turn Off");
}

void wakeupHandler()
{
    Serial.println("Hi, M Five command received!");
    M5.Display.setCursor(0, 0);
    M5.Display.fillRect(0, 0, M5.Display.width(), 60, TFT_BLACK);
    M5.Display.setTextColor(TFT_YELLOW);
    M5.Display.println("-> WAKEUP: Ready!");
}

// --- Setup ---

void setup() {
  // Initialize the M5StickC Plus 2.
  auto cfg = M5.config();
  M5.begin(cfg);
  
  // Disable M5StickC Plus 2 speaker to avoid conflict with ASR unit's speaker.
  M5.Speaker.begin();
  M5.Speaker.setVolume(0); 

  // Initialize display
  M5.Display.setRotation(1); // Horizontal display orientation
  M5.Display.setFont(&fonts::FreeMonoBold12pt7b);
  M5.Display.setTextDatum(top_left);
  M5.Display.setTextSize(1);
  M5.Display.setTextColor(TFT_WHITE, TFT_BLACK);
  M5.Display.fillScreen(TFT_BLACK);
  
  // Initialize Serial Monitor
  Serial.begin(115200);
  delay(1500); // Wait for Serial to initialize
  Serial.println("\n--- M5 ASR Robot Control Started ---");
  Serial.println("ASR Unit will be initialized on pins RX:33, TX:32.");

  // --- Initialize ASR Unit ---
  asr.begin(&ASRSerial, 115200, ASR_RX_PIN, ASR_TX_PIN); 

  // --- Register all Command Handlers ---
  
  // Continuous Motion
  asr.addCommandWord(0x07, "forward", forwardHandler);
  asr.addCommandWord(0x09, "backward", backwardHandler); // NEW: Backward (0x09)
  asr.addCommandWord(0x13, "stop", stopHandler);

  // Timed Motion (Turns) - 1 second duration
  asr.addCommandWord(0x03, "left", leftHandler); // NEW: Left (0x03)
  asr.addCommandWord(0x05, "right", rightHandler); // NEW: Right (0x05)

  asr.addCommandWord(0x04, "turn left", turnLeftHandler); // Turn Left (0x04)
  asr.addCommandWord(0x06, "turn right", turnRightHandler); // Turn Right (0x06)

  asr.addCommandWord(0x0A, "back", aboutTurnHandler);

  // Timed Motion (Numbers) - Duration in seconds
  // Registers "one" (0x21) through "nine" (0x29) to the same handler
  asr.addCommandWord(0x21, "one", numberCommandReceivedHandler);
  asr.addCommandWord(0x22, "two", numberCommandReceivedHandler);
  asr.addCommandWord(0x23, "three", numberCommandReceivedHandler);
  asr.addCommandWord(0x24, "four", numberCommandReceivedHandler);
  asr.addCommandWord(0x25, "five", numberCommandReceivedHandler);
  asr.addCommandWord(0x26, "six", numberCommandReceivedHandler);
  asr.addCommandWord(0x27, "seven", numberCommandReceivedHandler);
  asr.addCommandWord(0x28, "eight", numberCommandReceivedHandler);
  asr.addCommandWord(0x29, "nine", numberCommandReceivedHandler);

  // General Status Commands
  asr.addCommandWord(0xFF, "Hi, M Five", wakeupHandler);
  asr.addCommandWord(0x14, "turn on", turnOnHandler);
  asr.addCommandWord(0x15, "turn off", turnOffHandler);

  // --- Initialize I2C and Servo Hat ---
  Wire.begin(SERVOS_SDA_PIN, SERVOS_SCL_PIN, SERVOS_I2C_SPEED); 
  Serial.println("-> Initializing Servo Hat on Wire (25kHz)...");
  
  while (!drive.begin(&Wire, SERVOS_SDA_PIN, SERVOS_SCL_PIN, SERVOS_DEVICE_ADDR)) {
      Serial.println("Error: 8-Servo Hat not found (0x36). Checking connection...");
      M5.Lcd.fillScreen(TFT_RED);
      M5.Lcd.setCursor(10, 10);
      M5.Lcd.print("HAT ERROR!");
      delay(1000);
      M5.Lcd.fillScreen(TFT_BLACK);
  }
  Serial.println("Servo Hat Ready on Wire.");

  drive.enableServoPower(1); // Enable power to the servo ports
  setMotorSpeeds(STOP_POS, STOP_POS); // Initial stop

  M5.Display.setCursor(0, 0);
  M5.Display.fillRect(0, 0, M5.Display.width(), 80, TFT_BLACK);
  M5.Display.setTextColor(TFT_CYAN);
  M5.Display.println("Robot Ready!");
  //M5.Display.println("Say \"Hi, M Five\" to begin.");
}

// --- Loop ---

void loop()
{
    M5.update();
    
    // --- TIMED MOTION CHECK ---
    // If a timed motion is active (duration > 0) AND the time has elapsed, stop the robot.
    if (currentMotionDurationMs > 0 && (millis() - motionStartTimeMs >= currentMotionDurationMs)) {
        stopHandler();
        Serial.println("Timed motion complete. Stopping.");
    }

    // Check for recognized commands from the ASR unit
    if (asr.update()) {
        // Log all detailed information to the Serial Monitor
        Serial.print("Last Recognized Word: ");
        Serial.println(asr.getCurrentCommandWord());
        Serial.print("Command ID: ");
        Serial.println(asr.getCurrentCommandNum());
        // The motor action happened inside the handler function!
    }

    if (M5.BtnA.wasPressed()) { 
        stopHandler();
    }
}