#include <Servo.h>
#include <EEPROM.h>
// Servo objects
Servo baseServo, elbowServo, wristServo, gripServo;
// Joystick pins
const int joystick1X = A0; // Base (X-axis)
const int joystick1Y = A1; // Elbow (Y-axis)
const int joystick2X = A2; // Grip (X-axis)
const int joystick2Y = A3; // Wrist (Y-axis)
const int select1Pin = 7; // Joystick1 SELECT button
const int select2Pin = 8; // Joystick2 SELECT button
const int playButtonPin = 10; // Play button
// Servo control pins
const int basePin = 3, elbowPin = 5, wristPin = 6, gripPin = 9;
// Servo angles
int baseAngle = 90, elbowAngle = 90, wristAngle = 90, gripAngle = 90;
// Joystick thresholds
const int deadZone = 10; // Ignore small joystick movements
// Preset data structure
struct Preset {
int base;
int elbow;
int wrist;
int grip;
};
// Maximum number of presets
const int maxPresets = 5;
Preset presets[maxPresets];
int currentPreset = 0; // Active preset index
bool playMode = false; // Play mode flag
void setup() {
// Attach servos to pins
baseServo.attach(basePin);
elbowServo.attach(elbowPin);
wristServo.attach(wristPin);
gripServo.attach(gripPin);
// Set initial servo positions
baseServo.write(baseAngle);
elbowServo.write(elbowAngle);
wristServo.write(wristAngle);
gripServo.write(gripAngle);
// Initialize buttons
pinMode(select1Pin, INPUT_PULLUP);
pinMode(select2Pin, INPUT_PULLUP);
pinMode(playButtonPin, INPUT_PULLUP);
// Initialize serial monitor
Serial.begin(9600);
// Load presets from EEPROM at startup
loadPresetsFromEEPROM();
}
void loop() {
// Check for play mode
if (digitalRead(playButtonPin) == LOW) {
playMode = true;
playPresets();
}
// If not in play mode, handle joystick control
if (!playMode) {
handleJoystickControl();
}
}
// Smoothly move a servo
void moveServoSmoothly(Servo &servo, int ¤tAngle, int targetAngle, int stepDelay) {
if (currentAngle < targetAngle) {
for (int pos = currentAngle; pos <= targetAngle; pos++) {
servo.write(pos);
delay(stepDelay);
}
} else if (currentAngle > targetAngle) {
for (int pos = currentAngle; pos >= targetAngle; pos--) {
servo.write(pos);
delay(stepDelay);
}
}
currentAngle = targetAngle;
}
// Handle joystick control for manual servo adjustments
void handleJoystickControl() {
int joystick1XValue = analogRead(joystick1X) - 512; // Center at 0
int joystick1YValue = analogRead(joystick1Y) - 512; // Center at 0
int joystick2XValue = analogRead(joystick2X) - 512; // Center at 0
int joystick2YValue = analogRead(joystick2Y) - 512; // Center at 0
int stepDelay = 5; // Delay for smooth movement
// Adjust base angle
if (abs(joystick1XValue) > deadZone) {
int targetBaseAngle = constrain(baseAngle + (joystick1XValue / 128), 0, 180);
moveServoSmoothly(baseServo, baseAngle, targetBaseAngle, stepDelay);
}
// Adjust elbow angle
if (abs(joystick1YValue) > deadZone) {
int targetElbowAngle = constrain(elbowAngle + (joystick1YValue / 128), 0, 180);
moveServoSmoothly(elbowServo, elbowAngle, targetElbowAngle, stepDelay);
}
// Adjust wrist angle
if (abs(joystick2YValue) > deadZone) {
int targetWristAngle = constrain(wristAngle + (joystick2YValue / 128), 0, 180);
moveServoSmoothly(wristServo, wristAngle, targetWristAngle, stepDelay);
}
// Adjust grip angle
if (abs(joystick2XValue) > deadZone) {
int targetGripAngle = constrain(gripAngle + (joystick2XValue / 128), 0, 180);
moveServoSmoothly(gripServo, gripAngle, targetGripAngle, stepDelay);
}
// Save current angles to a preset with SELECT1
if (digitalRead(select1Pin) == LOW) {
presets[currentPreset] = { baseAngle, elbowAngle, wristAngle, gripAngle };
Serial.print("Preset ");
Serial.print(currentPreset);
Serial.println(" saved.");
delay(300); // Debounce
}
// Save all presets to EEPROM with SELECT2
if (digitalRead(select2Pin) == LOW) {
savePresetsToEEPROM();
Serial.println("Presets saved to EEPROM.");
delay(300); // Debounce
}
}
// Play presets sequentially
void playPresets() {
for (int i = 0; i < maxPresets; i++) {
Serial.print("Playing preset ");
Serial.println(i);
moveServoSmoothly(baseServo, baseAngle, presets[i].base, 10);
moveServoSmoothly(elbowServo, elbowAngle, presets[i].elbow, 10);
moveServoSmoothly(wristServo, wristAngle, presets[i].wrist, 10);
moveServoSmoothly(gripServo, gripAngle, presets[i].grip, 10);
delay(1000); // Pause between presets
// Exit play mode if the Play button is pressed again
if (digitalRead(playButtonPin) == LOW) {
playMode = false;
Serial.println("Play mode stopped.");
return;
}
}
}
// Save all presets to EEPROM
void savePresetsToEEPROM() {
for (int i = 0; i < maxPresets; i++) {
int addr = i * sizeof(Preset);
EEPROM.put(addr, presets[i]);
}
}
// Load presets from EEPROM
void loadPresetsFromEEPROM() {
for (int i = 0; i < maxPresets; i++) {
int addr = i * sizeof(Preset);
EEPROM.get(addr, presets[i]);
}
}