#include <Servo.h>;
#define grip_axis A3
#define forward_axis A2
#define up_axis A1
#define rotate_axis A0
#define grip_pin 7
#define up_pin 6
#define forward_pin 5
#define rotate_pin 4
int grip_value, up_value, forward_value, rotate_value;
int grip_angle, up_angle, forward_angle, rotate_angle;
Servo grip_Servo, up_Servo, forward_Servo, rotate_Servo;
void setup() {
Serial.begin(9600);
pinMode(grip_axis, INPUT);
pinMode(forward_axis, INPUT);
pinMode(up_axis, INPUT);
pinMode(rotate_axis, INPUT);
grip_Servo.attach(grip_pin);
forward_Servo.attach(forward_pin);
rotate_Servo.attach(rotate_pin);
up_Servo.attach(up_pin);
}
void loop() {
grip_value = analogRead(grip_axis);
forward_value = analogRead(forward_axis);
up_value = analogRead(up_axis);
rotate_value = analogRead(rotate_axis);
up_angle = map(up_value, 0, 1023, 0, 180);
rotate_angle = map(rotate_value, 0, 1023, 0, 180);
forward_angle = map(forward_value, 0, 1023, 0, 180);
rotate_angle = map(rotate_value, 0, 1023, 0, 180);
servoMove('u',up_angle);
servoMove('f',forward_angle);
servoMove('r',rotate_angle);
servoMove('g',grip_angle);
}
void servoMove(char servo, int angle){
switch (servo){
case 'u':
up_Servo.write(angle);
break;
case 'f':
forward_Servo.write(angle);
break;
case 'r':
rotate_Servo.write(angle);
break;
case 'g':
grip_Servo.write(angle);
break;
}
}