Final Code - simple arm robot 6dof voice controlled geetech 5 commands only

/*
Voice Controlled 6DOF Robotic Arm
Using PWM Servo Driver and Voice Recognition Module

Voice Commands:
  Up    = 1 (0x11)
  Down  = 2 (0x12)
  Left  = 3 (0x13)
  Right = 4 (0x14)
  Grab  = 5 (0x15)
*/

#include <Wire.h>
#include <Adafruit_PWMServoDriver.h>
#include <SoftwareSerial.h>

// ================= CONFIGURATION =================
// Adjust these values according to your setup

// Movement steps in degrees for each servo (changeable from here)
const int STEP_BASE = 40;       // Base rotation (Left/Right)
const int STEP_SHOULDER = 25;   // Shoulder (Up/Down)
const int STEP_ELBOW = 25;      // Elbow
const int STEP_WRIST = 25;      // Wrist
const int STEP_WRIST_ROT = 25;  // Wrist rotation
const int STEP_GRIPPER = 40;    // Gripper

// Default movement step (for any servos not specifically configured)
const int DEFAULT_STEP = 25;

// Servo angle limits for each joint (min, max)
const int SERVO_LIMITS[6][2] = {
  {10,  170},  // Servo 0: Base rotation (left/right)
  {100, 170},  // Servo 1: Shoulder (up/down)
  {90,  170},  // Servo 2: Elbow
  {90,  170},  // Servo 3: Wrist
  {0,   180},  // Servo 4: Wrist rotation
  {50,  120}   // Servo 5: Gripper
};

// Movement steps array for each servo
const int STEP_SIZES[6] = {
  STEP_BASE,      // Base
  STEP_SHOULDER,  // Shoulder
  STEP_ELBOW,     // Elbow
  STEP_WRIST,     // Wrist
  STEP_WRIST_ROT, // Wrist rotation
  STEP_GRIPPER    // Gripper
};

// Initial positions for servos
int servoPositions[6] = {90, 120, 90, 90, 90, 90};

// PWM Settings
#define SERVOMIN  125  // Minimum pulse length count
#define SERVOMAX  575  // Maximum pulse length count

// Voice module commands
#define CMD_UP    0x11
#define CMD_DOWN  0x12
#define CMD_LEFT  0x13
#define CMD_RIGHT 0x14
#define CMD_GRAB  0x15

// Additional movement commands you can add
#define CMD_ELBOW_UP    0x16
#define CMD_ELBOW_DOWN  0x17
#define CMD_WRIST_UP    0x18
#define CMD_WRIST_DOWN  0x19

// ================= OBJECT DECLARATIONS =================
Adafruit_PWMServoDriver pwmBoard = Adafruit_PWMServoDriver(0x40);
SoftwareSerial voiceSerial(4, 3);  // RX, TX

// ================= FUNCTION PROTOTYPES =================
void initializeVoiceModule();
void initializeServos();
void moveServo(uint8_t servoNum, int angle);
void moveUp();
void moveDown();
void moveLeft();
void moveRight();
void toggleGripper();
int angleToPulse(int angle);
void safeMoveServo(uint8_t servoNum, int targetAngle);
void moveServoBy(uint8_t servoNum, int delta);
void printServoPositions();
void printStepSizes();
void processVoiceCommand(byte command);
void moveElbowUp();
void moveElbowDown();
void moveWristUp();
void moveWristDown();

// ================= SETUP =================
void setup() {
  Serial.begin(9600);
  voiceSerial.begin(9600);

  Serial.println(F("=========================================="));
  Serial.println(F("Voice Controlled 6DOF Robotic Arm"));
  Serial.println(F("=========================================="));

  // Print step sizes
  printStepSizes();

  // Initialize voice recognition module
  initializeVoiceModule();

  // Initialize servo driver and servos
  initializeServos();

  Serial.println(F("\nSystem Ready!"));
  Serial.println(F("Voice commands: Up, Down, Left, Right, Grab"));
  Serial.println(F("=========================================="));
}

// ================= MAIN LOOP =================
void loop() {
  // Check for voice commands
  if (voiceSerial.available()) {
    byte command = voiceSerial.read();
    processVoiceCommand(command);
  }
}

// ================= VOICE MODULE INITIALIZATION =================
void initializeVoiceModule() {
  Serial.println(F("Initializing voice module..."));

  voiceSerial.write(0xAA);
  voiceSerial.write('0x00');  // Use 0x00 instead of '0x00'
  delay(100);

  voiceSerial.write(0xAA);
  voiceSerial.write(0x37);  // Compact mode
  delay(100);

  voiceSerial.write(0xAA);
  voiceSerial.write('0x00');  // Use 0x00 instead of '0x00'
  delay(100);

  voiceSerial.write(0xAA);
  voiceSerial.write(0x21);  // Import group 1

  Serial.println(F("Voice module initialized"));
}

// ================= SERVO INITIALIZATION =================
void initializeServos() {
  Serial.println(F("Initializing servos..."));

  Wire.begin();
  pwmBoard.begin();
  pwmBoard.setPWMFreq(60);

  // Set all servos to initial positions
  for (int i = 0; i < 6; i++) {
    moveServo(i, servoPositions[i]);
    delay(100);
  }

  Serial.println(F("Servos initialized"));
}

// ================= VOICE COMMAND PROCESSING =================
void processVoiceCommand(byte command) {
  switch (command) {
    case CMD_UP:
      Serial.println(F("Voice Command: UP"));
      moveUp();
      break;

    case CMD_DOWN:
      Serial.println(F("Voice Command: DOWN"));
      moveDown();
      break;

    case CMD_LEFT:
      Serial.println(F("Voice Command: LEFT"));
      moveLeft();
      break;

    case CMD_RIGHT:
      Serial.println(F("Voice Command: RIGHT"));
      moveRight();
      break;

    case CMD_GRAB:
      Serial.println(F("Voice Command: GRAB"));
      toggleGripper();
      break;

    // Uncomment these if you add more voice commands to your module
    /*
    case CMD_ELBOW_UP:
      Serial.println(F("Voice Command: ELBOW UP"));
      moveElbowUp();
      break;

    case CMD_ELBOW_DOWN:
      Serial.println(F("Voice Command: ELBOW DOWN"));
      moveElbowDown();
      break;

    case CMD_WRIST_UP:
      Serial.println(F("Voice Command: WRIST UP"));
      moveWristUp();
      break;

    case CMD_WRIST_DOWN:
      Serial.println(F("Voice Command: WRIST DOWN"));
      moveWristDown();
      break;
    */

    default:
      Serial.print(F("Unknown command: 0x"));
      Serial.println(command, HEX);
      break;
  }

  // Display current servo positions
  printServoPositions();
}

// ================= SERVO CONTROL FUNCTIONS =================
void moveUp() {
  Serial.print(F("Moving shoulder UP by "));
  Serial.print(STEP_SIZES[1]);
  Serial.println(F(" degrees..."));
  moveServoBy(1, STEP_SIZES[1]);  // Shoulder servo
}

void moveDown() {
  Serial.print(F("Moving shoulder DOWN by "));
  Serial.print(STEP_SIZES[1]);
  Serial.println(F(" degrees..."));
  moveServoBy(1, -STEP_SIZES[1]);  // Shoulder servo
}

void moveLeft() {
  Serial.print(F("Moving base LEFT by "));
  Serial.print(STEP_SIZES[0]);
  Serial.println(F(" degrees..."));
  moveServoBy(0, STEP_SIZES[0]);  // Base servo
}

void moveRight() {
  Serial.print(F("Moving base RIGHT by "));
  Serial.print(STEP_SIZES[0]);
  Serial.println(F(" degrees..."));
  moveServoBy(0, -STEP_SIZES[0]);  // Base servo
}

// Additional movement functions (uncomment if you add more voice commands)
void moveElbowUp() {
  Serial.print(F("Moving elbow UP by "));
  Serial.print(STEP_SIZES[2]);
  Serial.println(F(" degrees..."));
  moveServoBy(2, STEP_SIZES[2]);  // Elbow servo
}

void moveElbowDown() {
  Serial.print(F("Moving elbow DOWN by "));
  Serial.print(STEP_SIZES[2]);
  Serial.println(F(" degrees..."));
  moveServoBy(2, -STEP_SIZES[2]);  // Elbow servo
}

void moveWristUp() {
  Serial.print(F("Moving wrist UP by "));
  Serial.print(STEP_SIZES[3]);
  Serial.println(F(" degrees..."));
  moveServoBy(3, STEP_SIZES[3]);  // Wrist servo
}

void moveWristDown() {
  Serial.print(F("Moving wrist DOWN by "));
  Serial.print(STEP_SIZES[3]);
  Serial.println(F(" degrees..."));
  moveServoBy(3, -STEP_SIZES[3]);  // Wrist servo
}

void toggleGripper() {
  static bool isGripperOpen = false;

  if (isGripperOpen) {
    Serial.println(F("Closing gripper..."));
    safeMoveServo(5, SERVO_LIMITS[5][1]);  // Closed position
  } else {
    Serial.println(F("Opening gripper..."));
    safeMoveServo(5, SERVO_LIMITS[5][0]);  // Open position
  }

  isGripperOpen = !isGripperOpen;
}

// ================= SMART SERVO CONTROL =================
void moveServoBy(uint8_t servoNum, int delta) {
  int currentPos = servoPositions[servoNum];
  int targetPos = currentPos + delta;

  // Check if movement is within limits
  if (targetPos < SERVO_LIMITS[servoNum][0]) {
    Serial.print(F("Servo "));
    Serial.print(servoNum);
    Serial.print(F(" reached minimum limit ("));
    Serial.print(SERVO_LIMITS[servoNum][0]);
    Serial.println(F("°)!"));
    targetPos = SERVO_LIMITS[servoNum][0];
  } else if (targetPos > SERVO_LIMITS[servoNum][1]) {
    Serial.print(F("Servo "));
    Serial.print(servoNum);
    Serial.print(F(" reached maximum limit ("));
    Serial.print(SERVO_LIMITS[servoNum][1]);
    Serial.println(F("°)!"));
    targetPos = SERVO_LIMITS[servoNum][1];
  }

  // Only move if target is different from current
  if (targetPos != currentPos) {
    safeMoveServo(servoNum, targetPos);
  } else {
    Serial.println(F("Movement blocked by limit"));
  }
}

void safeMoveServo(uint8_t servoNum, int targetAngle) {
  // Constrain angle to servo limits
  targetAngle = constrain(targetAngle,
                         SERVO_LIMITS[servoNum][0],
                         SERVO_LIMITS[servoNum][1]);

  // Update position and move servo
  servoPositions[servoNum] = targetAngle;
  moveServo(servoNum, targetAngle);

  Serial.print(F("Servo "));
  Serial.print(servoNum);
  Serial.print(F(" moved to: "));
  Serial.print(targetAngle);
  Serial.println(F("°"));
}

void moveServo(uint8_t servoNum, int angle) {
  // Note: +1 offset because PWM channels start from 0
  pwmBoard.setPWM(servoNum + 1, 0, angleToPulse(angle));
  delay(50);  // Small delay for smooth movement
}

// ================= UTILITY FUNCTIONS =================
int angleToPulse(int angle) {
  return map(angle, 0, 180, SERVOMIN, SERVOMAX);
}

void printServoPositions() {
  Serial.println(F("\nCurrent Servo Positions:"));
  Serial.println(F("------------------------"));

  const char* servoNames[] = {"Base", "Shoulder", "Elbow", "Wrist", "WristRot", "Gripper"};

  for (int i = 0; i < 6; i++) {
    Serial.print(servoNames[i]);
    Serial.print(" (S");
    Serial.print(i);
    Serial.print("): ");
    Serial.print(servoPositions[i]);
    Serial.print("°  [");
    Serial.print(SERVO_LIMITS[i][0]);
    Serial.print("° - ");
    Serial.print(SERVO_LIMITS[i][1]);
    Serial.print("°]  Step: ");
    Serial.print(STEP_SIZES[i]);
    Serial.println("°");
  }

  Serial.println(F("------------------------"));
}

void printStepSizes() {
  Serial.println(F("Movement Step Sizes:"));
  Serial.println(F("---------------------"));
  Serial.print(F("Base (Left/Right):    "));
  Serial.print(STEP_BASE);
  Serial.println(F("°"));
  Serial.print(F("Shoulder (Up/Down):   "));
  Serial.print(STEP_SHOULDER);
  Serial.println(F("°"));
  Serial.print(F("Elbow:                "));
  Serial.print(STEP_ELBOW);
  Serial.println(F("°"));
  Serial.print(F("Wrist:                "));
  Serial.print(STEP_WRIST);
  Serial.println(F("°"));
  Serial.print(F("Wrist Rotation:       "));
  Serial.print(STEP_WRIST_ROT);
  Serial.println(F("°"));
  Serial.print(F("Gripper:              "));
  Serial.print(STEP_GRIPPER);
  Serial.println(F("°"));
  Serial.println(F("---------------------"));
}