#include <Servo.h>
// Create 6 distinct servo objects
Servo baseServo;
Servo shoulderServo;
Servo elbowServo;
Servo wristPitchServo;
Servo wristRollServo;
Servo gripperServo;
// Initial start angles (90 degrees is dead center)
int baseAngle = 90;
int shoulderAngle = 90;
int elbowAngle = 90;
int wristPitchAngle = 90;
int wristRollAngle = 90;
int gripperAngle = 45; // Slightly open claw
const int stepSize = 5; // Degrees to shift per keypress
void setup() {
Serial.begin(9600);
// Attach the servos to the Arduino's Digital Pins
baseServo.attach(3);
shoulderServo.attach(5);
elbowServo.attach(6);
wristPitchServo.attach(9);
wristRollServo.attach(10);
gripperServo.attach(11);
// Write initial default positions on boot
baseServo.write(baseAngle);
shoulderServo.write(shoulderAngle);
elbowServo.write(elbowAngle);
wristPitchServo.write(wristPitchAngle);
wristRollServo.write(wristRollAngle);
gripperServo.write(gripperAngle);
Serial.println("======= 6-Axis Arm Control Active =======");
Serial.println("Controls: [A/D] Base | [W/S] Shoulder | [I/K] Elbow");
Serial.println(" [J/L] Wrist Pitch | [U/O] Wrist Roll | [F/G] Gripper");
Serial.println("---------------------------------------------------------");
Serial.println("Click inside this window, type a letter, and hit Enter!");
}
void loop() {
if (Serial.available() > 0) {
char key = Serial.read();
// 1. BASE CONTROL (A / D)
if (key == 'a' || key == 'A') {
baseAngle = constrain(baseAngle - stepSize, 0, 180);
baseServo.write(baseAngle);
Serial.print("Base Angle updated to: "); Serial.println(baseAngle);
} else if (key == 'd' || key == 'D') {
baseAngle = constrain(baseAngle + stepSize, 0, 180);
baseServo.write(baseAngle);
Serial.print("Base Angle updated to: "); Serial.println(baseAngle);
}
// 2. SHOULDER CONTROL (W / S)
else if (key == 'w' || key == 'W') {
shoulderAngle = constrain(shoulderAngle + stepSize, 0, 180);
shoulderServo.write(shoulderAngle);
Serial.print("Shoulder Angle updated to: "); Serial.println(shoulderAngle);
} else if (key == 's' || key == 'S') {
shoulderAngle = constrain(shoulderAngle - stepSize, 0, 180);
shoulderServo.write(shoulderAngle);
Serial.print("Shoulder Angle updated to: "); Serial.println(shoulderAngle);
}
// 3. ELBOW CONTROL (I / K)
else if (key == 'i' || key == 'I') {
elbowAngle = constrain(elbowAngle + stepSize, 0, 180);
elbowServo.write(elbowAngle);
Serial.print("Elbow Angle updated to: "); Serial.println(elbowAngle);
} else if (key == 'k' || key == 'K') {
elbowAngle = constrain(elbowAngle - stepSize, 0, 180);
elbowServo.write(elbowAngle);
Serial.print("Elbow Angle updated to: "); Serial.println(elbowAngle);
}
// 4. WRIST PITCH CONTROL (J / L)
else if (key == 'j' || key == 'J') {
wristPitchAngle = constrain(wristPitchAngle - stepSize, 0, 180);
wristPitchServo.write(wristPitchAngle);
Serial.print("Wrist Pitch updated to: "); Serial.println(wristPitchAngle);
} else if (key == 'l' || key == 'L') {
wristPitchAngle = constrain(wristPitchAngle + stepSize, 0, 180);
wristPitchServo.write(wristPitchAngle);
Serial.print("Wrist Pitch updated to: "); Serial.println(wristPitchAngle);
}
// 5. WRIST ROLL CONTROL (U / O)
else if (key == 'u' || key == 'U') {
wristRollAngle = constrain(wristRollAngle - stepSize, 0, 180);
wristRollServo.write(wristRollAngle);
Serial.print("Wrist Roll updated to: "); Serial.println(wristRollAngle);
} else if (key == 'o' || key == 'O') {
wristRollAngle = constrain(wristRollAngle + stepSize, 0, 180);
wristRollServo.write(wristRollAngle);
Serial.print("Wrist Roll updated to: "); Serial.println(wristRollAngle);
}
// 6. GRIPPER CLAW CONTROL (F / G)
else if (key == 'f' || key == 'F') {
gripperAngle = constrain(gripperAngle - stepSize, 10, 90);
gripperServo.write(gripperAngle);
Serial.print("Gripper Claw updated to: "); Serial.println(gripperAngle);
} else if (key == 'g' || key == 'G') {
gripperAngle = constrain(gripperAngle + stepSize, 10, 90);
gripperServo.write(gripperAngle);
Serial.print("Gripper Claw updated to: "); Serial.println(gripperAngle);
}
}
}