#include <Servo.h>
// Initialize Servo objects
Servo base, shoulder, elbow, gripper;
// Pin Definitions
#define J1_X A0
#define J1_Y A1
#define J2_X A2
#define J2_Y A3
#define J1_SW 2
#define J2_SW 3
// Servo Signal Pins (Matched to Phase 4 Wiring)
const int PIN_BASE = 11;
const int PIN_SHOULDER = 10;
const int PIN_ELBOW = 9;
const int PIN_GRIPPER = 6;
// Starting Positions (Degrees)
int baseAngle = 90;
int shoulderAngle = 90;
int elbowAngle = 90;
int gripperAngle = 90;
/**
* deadZone Function
* Returns 1 for forward movement, -1 for backward, and 0 for stationary.
*/
int deadZone(int val, int center = 512, int threshold = 50) {
if (val > (center + threshold)) return 1;
if (val < (center - threshold)) return -1;
return 0;
}
void setup() {
base.attach(PIN_BASE);
shoulder.attach(PIN_SHOULDER);
elbow.attach(PIN_ELBOW);
gripper.attach(PIN_GRIPPER);
pinMode(J1_SW, INPUT_PULLUP);
pinMode(J2_SW, INPUT_PULLUP);
base.write(baseAngle);
shoulder.write(shoulderAngle);
elbow.write(elbowAngle);
gripper.write(gripperAngle);
Serial.begin(9600);
Serial.println("--- RAW DATA DEBUG MODE ---");
}
void loop() {
// Read Raw Joystick Values
int rawJ1X = analogRead(J1_X);
int rawJ1Y = analogRead(J1_Y);
int rawJ2X = analogRead(J2_X);
int rawJ2Y = analogRead(J2_Y);
// Calculate Movement
int baseMove = deadZone(rawJ1X);
int shoulderMove = deadZone(rawJ1Y);
int elbowMove = deadZone(rawJ2X);
int gripperMove = deadZone(rawJ2Y);
// Update angles
baseAngle = constrain(baseAngle + baseMove, 0, 180);
shoulderAngle = constrain(shoulderAngle + shoulderMove, 20, 160);
elbowAngle = constrain(elbowAngle + elbowMove, 0, 180);
gripperAngle = constrain(gripperAngle + gripperMove, 0, 90);
base.write(baseAngle);
shoulder.write(shoulderAngle);
elbow.write(elbowAngle);
gripper.write(gripperAngle);
// DEBUG: Show raw joystick numbers
// If these are 0 while the stick is centered, the joystick has no power!
Serial.print("J1_X: "); Serial.print(rawJ1X);
Serial.print(" | J1_Y: "); Serial.print(rawJ1Y);
Serial.print(" | J2_X: "); Serial.print(rawJ2X);
Serial.print(" | J2_Y: "); Serial.println(rawJ2Y);
delay(20); // Slightly slower for easier reading
}