#include <Servo.h>
//----- Servo settings
Servo servo_joint1;
Servo servo_joint2;
Servo servo_joint3;
int servo_joint1_pin = 5;
int servo_joint2_pin = 6;
int servo_joint3_pin = 9;
int cur_angle_joint1 = 0; // current servo position in degrees
int cur_angle_joint2 = 0;
int cur_angle_joint3 = 0;
int current_servo_index; // [1: joint1], [2: joint2], [3: claw]
int CENTER_SERVO = 90; // center servo position
int joint1_min_angle = 45; // minimum servo position
int joint1_max_angle = 135; // maximum servo position
int joint2_min_angle = 75; // minimum servo position
int joint2_max_angle = 180; // maximum servo position
int joint3_min_angle = 30; // minimum servo position
int joint3_max_angle = 180; // maximum servo position
int turnRate = 1; // servo turn rate increment (larger value, faster rate)
//----- Joystick settings
const unsigned long BUTTON_READ_INTERVAL = 200;
const unsigned long SEL_CHANGE_INTERVAL = 750;
const int VERT = 1; // analog A1
const int HORIZ = 2; // analog A2
const int SEL = 4; // digital D4
const int SEL_LED = 13;
boolean selStatus = false; // false = joint2 is selected(13 LED off), true = joint3 is selected(13 LED on)
unsigned long last_button_input = 0;
unsigned long last_sel_change = 0;
void setup() {
Serial.begin(9600);
Serial.println("Robot Arm Control");
Serial.println("By pressing 1, 2, 3 you can select servo");
Serial.println("Press a or s to move, spacebar to center");
Serial.println();
servo_joint1.attach(servo_joint1_pin);
servo_joint2.attach(servo_joint2_pin);
servo_joint3.attach(servo_joint3_pin);
cur_angle_joint1 = CENTER_SERVO;
cur_angle_joint2 = CENTER_SERVO;
cur_angle_joint3 = CENTER_SERVO;
servo_joint1.write(cur_angle_joint1);
servo_joint2.write(cur_angle_joint2);
servo_joint3.write(cur_angle_joint3);
changeServo(1); // Set joint1 as current servo
pinMode(SEL,INPUT); // Set button pin as input mode
pinMode(SEL_LED, OUTPUT); // Joint2, Joint3 status LED
}
void loop() {
// Read serial input
if (Serial.available() > 0) {
char command = Serial.read();
// Servo control command: 'a'==97, 's'==115, ' '==32
if (command == 97) { moveServo(-1); } // move -1 step
else if (command == 115) { moveServo(1); } // move 1 step
else if (command == 32) { moveServo(0); } // 0 step means 'move to center position'
// Select servo command: '1'==49, '2'==50, '3'==51
else if (command == 49) { changeServo(1); } // select joint1 servo
else if (command == 50) { changeServo(2); } // select joint2 servo
else if (command == 51) { changeServo(3); } // select claw servo
}
// Read button input
unsigned long current = millis();
if(current - last_button_input > BUTTON_READ_INTERVAL) {
int vertical, horizontal, select;
vertical = analogRead(VERT); // will be 0-1023
horizontal = analogRead(HORIZ); // will be 0-1023
select = digitalRead(SEL); // will be HIGH (1) if not pressed, and LOW (0) if pressed
int verti_move, hori_move;
verti_move = (vertical - 500) / 100; // Scaling joystick movement to servo movement
hori_move = (horizontal - 500) / 100;
hori_move = hori_move * -1; // convert direction
if(verti_move != 0) {
if(selStatus) {
// Joint3 mode
verti_move = verti_move * -1; // convert direction
changeServo(3);
moveServo(verti_move);
} else {
// Joint2 mode
changeServo(2);
moveServo(verti_move);
}
}
if(hori_move != 0) {
changeServo(1);
moveServo(hori_move);
}
if(select == LOW && current - last_sel_change > SEL_CHANGE_INTERVAL) {
selStatus = !selStatus;
digitalWrite(SEL_LED, selStatus);
last_sel_change = current;
}
Serial.print("# Joystick: vert=");
Serial.print(verti_move);
Serial.print("# Joystick: hori=");
Serial.print(hori_move);
Serial.print("# Joystick: sel=");
Serial.println(select);
last_button_input = current;
}
}
void changeServo(int index) {
if(index == 1) {
current_servo_index = 1;
Serial.println("# Joint1 servo selected");
} else if(index == 2) {
current_servo_index = 2;
Serial.println("# Joint2 servo selected");
} else {
current_servo_index = 3;
Serial.println("# Claw servo selected");
}
}
// Move servo to target angle
void moveServo(int steps) {
int c_angle = 0;
int target_angle = 0;
if(current_servo_index == 1) {
c_angle = cur_angle_joint1;
if(steps == 0)
target_angle = CENTER_SERVO;
else
target_angle = cur_angle_joint1 + steps*turnRate;
if(target_angle > joint1_max_angle) target_angle = joint1_max_angle;
if(target_angle < joint1_min_angle) target_angle = joint1_min_angle;
}
else if(current_servo_index == 2) {
c_angle = cur_angle_joint2;
if(steps == 0)
target_angle = CENTER_SERVO;
else
target_angle = cur_angle_joint2 + steps*turnRate;
if(target_angle > joint2_max_angle) target_angle = joint2_max_angle;
if(target_angle < joint2_min_angle) target_angle = joint2_min_angle;
}
else {
c_angle = cur_angle_joint3;
if(steps == 0)
target_angle = CENTER_SERVO;
else
target_angle = cur_angle_joint3 + steps*turnRate;
if(target_angle > joint3_max_angle) target_angle = joint3_max_angle;
if(target_angle < joint3_min_angle) target_angle = joint3_min_angle;
}
int step_angle = 1;
if(target_angle < c_angle) { step_angle = -1; }
// This code makes smooth movement
for(int i = c_angle; i != target_angle; i+=step_angle) {
//if((target_angle - i) < step_angle && (target_angle - i) > step_angle * -1) // close to target position, break this loop
// break;
if(current_servo_index == 1) {
servo_joint1.write(i);
}
else if(current_servo_index == 2) {
servo_joint2.write(i);
}
else {
servo_joint3.write(i);
}
delay(15);
}
if(current_servo_index == 1) {
cur_angle_joint1 = target_angle;
}
else if(current_servo_index == 2) {
cur_angle_joint2 = target_angle;
}
else {
cur_angle_joint3 = target_angle;
}
// for debug
if(current_servo_index == 1) {
Serial.print("Servo 1 angle = ");
Serial.println(cur_angle_joint1);
}
else if(current_servo_index == 2) {
Serial.print("Servo 2 angle = ");
Serial.println(cur_angle_joint2);
}
else {
Serial.print("Servo 3 angle = ");
Serial.println(cur_angle_joint3);
}
}