Robot Helping Arm

/*
  539 Project Zhikang Xu
  Robot Helping Arm
*/

#include <Servo.h>
#include "Wire.h"
#include "Adafruit_LiquidCrystal.h"

// setup for other staff
Adafruit_LiquidCrystal lcd(0);
const int analogInPin = A0;   // pot
int sensorValue = 0;
int outputValue = 0;
int speed1 = 1;


// setup for head servo
const int buttonPin_head = 2;
int servoPin_head = 9;
int buttonState_head = 0;
int counter_head = 0;
int pos_head = 180;
int direction_head = 1;
Servo servo_head;

// setup for base servo
const int buttonPin_base = 4;
int servoPin_base = 10;
int buttonState_base = 0;
int counter_base = 0;
int pos_base = 90;
int direction_base = 1;
Servo servo_base;

// setup for right servo
const int buttonPin_right = 7;
int servoPin_right = 11;
int buttonState_right = 0;
int counter_right = 0;
int pos_right = 90;
int direction_right = 1;
Servo servo_right;

// setup for left servo
const int buttonPin_left = 8;
int servoPin_left = 6;
int buttonState_left = 0;
int counter_left = 0;
int pos_left = 90;
int direction_left = 1;
Servo servo_left;

void setup()
{
  // setup for lcd
  lcd.begin(16, 2);
  lcd.print("Good Morning!");

  // setup for servos
  servo_head.attach(servoPin_head);
  pinMode(buttonPin_head, INPUT);
  servo_head.write(pos_head);

  servo_base.attach(servoPin_base);
  pinMode(buttonPin_base, INPUT);
  servo_base.write(pos_base);

  servo_right.attach(servoPin_right);
  pinMode(buttonPin_right, INPUT);
  servo_right.write(pos_right);
  
  servo_left.attach(servoPin_left);
  pinMode(buttonPin_left, INPUT);
  servo_left.write(pos_left);
}

void loop()
{
//  Serial.begin(9600);

  // handle speed
  sensorValue = analogRead(analogInPin); // input for set speed
  outputValue = map(sensorValue, 0, 1023, 1, 10); // transfer speed into 1-10
  speed1 = outputValue + 0; // universal speed variable for four motors

  // ################## Beginning of Head Servo ##############
  // handle button logic
  int buttonState_head;
  buttonState_head = digitalRead(buttonPin_head);
  if (buttonState_head == HIGH) {
    counter_head = 1;
    delay(15);
  }
  if (buttonState_head == LOW) {
    counter_head = 0;
    delay(15);
  }

  // if pressed, then move and show
  if (counter_head == 1) {
    servo_head.write (pos_head);
    lcd.setCursor(0, 0);
    lcd.print("Moving Caws!");
    lcd.setCursor(0, 1);
    lcd.print("Pos: ");
    lcd.print(pos_head);
    lcd.print("Speed: ");
    lcd.print(speed1);
    //    lcd.print('\n');
    delay(10);
    //    }
  }

  // Control the loop movement of paws
  if (pos_head <= 0) {
    direction_head = 1;
  }
  if (pos_head >= 180) {
    direction_head = 0;
  }
  if  (direction_head == 1) {
    pos_head = pos_head + speed1;
  }
  if  (direction_head == 0) {
    pos_head = pos_head - speed1;
  }
  // ################## End of Head Servo ####################

  // ################## Beginning of Base Servo ##############
  // handle button logic
  int buttonState_base;
  buttonState_base = digitalRead(buttonPin_base);
  if (buttonState_base == HIGH) {
    counter_base = 1;
    delay(15);
  }
  if (buttonState_base == LOW) {
    counter_base = 0;
    delay(15);
  }

  // if pressed, then move and show
  if (counter_base == 1) {
    servo_base.write (pos_base);
    lcd.setCursor(0, 0);
    lcd.print("Moving Base!");
    lcd.setCursor(0, 1);
    lcd.print("Pos: ");
    lcd.print(pos_base);
    lcd.print("Speed: ");
    lcd.print(speed1);
    //    lcd.print('\n');
    delay(10);
    //    }
  }

  // Control the loop movement of base
  if (pos_base <= 0) {
    direction_base = 1;
  }
  if (pos_base >= 90) {
    direction_base = 0;
  }
  if  (direction_base == 1) {
    pos_base = pos_base + speed1;
  }
  if  (direction_base == 0) {
    pos_base = pos_base - speed1;
  }
  // ################## End of Base Servo ####################

  // ################## Beginning of right Servo ##############
  // handle button logic
  int buttonState_right;
  buttonState_right = digitalRead(buttonPin_right);
  if (buttonState_right == HIGH) {
    counter_right = 1;
    delay(15);
  }
  if (buttonState_right == LOW) {
    counter_right = 0;
    delay(15);
  }

  // if pressed, then move and show
  if (counter_right == 1) {
    servo_right.write (pos_right);
    lcd.setCursor(0, 0);
    lcd.print("Right Hands!");
    lcd.setCursor(0, 1);
    lcd.print("Pos: ");
    lcd.print(pos_right);
    lcd.print("Speed: ");
    lcd.print(speed1);
    //    lcd.print('\n');
    delay(10);
    //    }
  }

  // Control the loop movement of base
  if (pos_right <= 0) {
    direction_right = 1;
  }
  if (pos_right >= 180) {
    direction_right = 0;
  }
  if  (direction_right == 1) {
    pos_right = pos_right + speed1;
  }
  if  (direction_right == 0) {
    pos_right = pos_right - speed1;
  }
  // ################## End of right Servo ####################


  // ################## Beginning of left Servo ##############
  // handle button logic
  int buttonState_left;
  buttonState_left = digitalRead(buttonPin_left);
  if (buttonState_left == HIGH) {
    counter_left = 1;
    delay(15);
  }
  if (buttonState_left == LOW) {
    counter_left = 0;
    delay(15);
  }

  // if pressed, then move and show
  if (counter_left == 1) {
    servo_left.write (pos_left);
    lcd.setCursor(0, 0);
    lcd.print("Left Hands!");
    lcd.setCursor(0, 1);
    lcd.print("Pos: ");
    lcd.print(pos_left);
    lcd.print("Speed: ");
    lcd.print(speed1);
    //    lcd.print('\n');
    delay(10);
    //    }
  }

  // Control the loop movement of base
  if (pos_left <= 0) {
    direction_left = 1;
  }
  if (pos_left >= 180) {
    direction_left = 0;
  }
  if  (direction_left == 1) {
    pos_left = pos_left + speed1;
  }
  if  (direction_left == 0) {
    pos_left = pos_left - speed1;
  }
  // ################## End of left Servo ####################

  delay(4);
}

//  servo.write(angle);