DIY Arduino Robotic Arm Testing Rev 2

//Rev 2 changed Joint 2 Minimum Axis Limit

#include <Servo.h>

Servo Servo1;
Servo Servo2;
Servo Servo3;
Servo Servo4;
Servo Servo5;
Servo Servo6;

int J1 = 5; //Joint 1 Servo Attached to Pin 5
int J2 = 6; //Joint 2 Servo Attached to Pin 6
int J3 = 7; //Joint 3 Servo Attached to Pin 7
int J4 = 8; //Joint 4 Servo Attached to Pin 8
int J5 = 9; //Joint 5 Servo Attached to Pin 9
int J6 = 10; //Joint 6/Gripper Servo Attached to Pin 10

int PotPin1 = A0; //Joint 1 Potentiometer is Tied to Pin A0
int PotPin2 = A1; //Joint 2 Potentiometer is Tied to Pin A1
int PotPin3 = A2; //Joint 3 Potentiometer is Tied to Pin A2
int PotPin4 = A3; //Joint 4 Potentiometer is Tied to Pin A3
int PotPin5 = A4; //Joint 5 Potentiometer is Tied to Pin A4
int PotPin6 = A5; //Joint 6 Potentiometer is Tied to Pin A5
int PotVal1 = 0; //Stores Potentiometer 1 Value
int PotVal2 = 0; //Stores Potentiometer 2 Value
int PotVal3 = 0; //Stores Potentiometer 3 Value
int PotVal4 = 0; //Stores Potentiometer 4 Value
int PotVal5 = 0; //Stores Potentiometer 5 Value
int PotVal6 = 0; //Stores Potentiometer 6 Value
int angle1 = 0; //Stores Angle to Move Servo 1 To
int angle2 = 0; //Stores Angle to Move Servo 2 To
int angle3 = 0; //Stores Angle to Move Servo 3 To
int angle4 = 0; //Stores Angle to Move Servo 4 To
int angle5 = 0; //Stores Angle to Move Servo 5 To
int angle6 = 0; //Stores Angle to Move Servo 6 To

void setup() {
  // put your setup code here, to run once:
  Serial.begin(9600);
  pinMode(PotPin1, INPUT); //Sets Pin Potentiometer 1 is Tied to To an Input
  pinMode(PotPin2, INPUT); //Sets Pin Potentiometer 2 is Tied to To an Input
  pinMode(PotPin3, INPUT); //Sets Pin Potentiometer 3 is Tied to To an Input
  pinMode(PotPin4, INPUT); //Sets Pin Potentiometer 4 is Tied to To an Input
  pinMode(PotPin5, INPUT); //Sets Pin Potentiometer 5 is Tied to To an Input
  pinMode(PotPin6, INPUT); //Sets Pin Potentiometer 6 is Tied to To an Input
  Servo1.attach(J1); //Calls this is like a pinmode command but also does more from the servo library
  Servo2.attach(J2); //Calls this is like a pinmode command but also does more from the servo library
  Servo3.attach(J3); //Calls this is like a pinmode command but also does more from the servo library
  Servo4.attach(J4); //Calls this is like a pinmode command but also does more from the servo library
  Servo5.attach(J5); //Calls this is like a pinmode command but also does more from the servo library
  Servo6.attach(J6); //Calls this is like a pinmode command but also does more from the servo library
}

void loop() {
  // put your main code here, to run repeatedly:
  PotVal1 = analogRead(PotPin1); //Reads Voltage Between 0 and 5 V and Gives Number Between 0 and 1023 Where 2^8 = 1024.  Starts at 0 therefore 1023.
  PotVal2 = analogRead(PotPin2); //Reads Voltage Between 0 and 5 V and Gives Number Between 0 and 1023 Where 2^8 = 1024.  Starts at 0 therefore 1023.
  PotVal3 = analogRead(PotPin3); //Reads Voltage Between 0 and 5 V and Gives Number Between 0 and 1023 Where 2^8 = 1024.  Starts at 0 therefore 1023.
  PotVal4 = analogRead(PotPin4); //Reads Voltage Between 0 and 5 V and Gives Number Between 0 and 1023 Where 2^8 = 1024.  Starts at 0 therefore 1023.
  PotVal5 = analogRead(PotPin5); //Reads Voltage Between 0 and 5 V and Gives Number Between 0 and 1023 Where 2^8 = 1024.  Starts at 0 therefore 1023.
  PotVal6 = analogRead(PotPin6); //Reads Voltage Between 0 and 5 V and Gives Number Between 0 and 1023 Where 2^8 = 1024.  Starts at 0 therefore 1023.
  Serial.print("PotVal1: ");
  Serial.print(PotVal1);
  Serial.print(", ");
  Serial.print("PotVal2: ");
  Serial.print(PotVal2);
  Serial.print(", ");
  Serial.print("PotVal3: ");
  Serial.print(PotVal3);
  Serial.print(", ");
  Serial.print("PotVal4: ");
  Serial.print(PotVal4);
  Serial.print(", ");
  Serial.print("PotVal5: ");
  Serial.print(PotVal5);
  Serial.print(", ");
  Serial.print("PotVal6: ");
  Serial.print(PotVal6);
  Serial.println(".");
  // scale the numbers from the pot
  angle1 = map(PotVal1, 0, 1023, 0, 180); //Scales Converted Values Between 0 and 1023 to Angles Setting Servo Range
  angle2 = map(PotVal2, 0, 1023, 50, 170); //Scales Converted Values Between 0 and 1023 to Angles Setting Servo Range - Default Value 100 to 170 - But if leave at 100 it cannot lift the arm back up
  angle3 = map(PotVal3, 0, 1023, 20, 180); //Scales Converted Values Between 0 and 1023 to Angles Setting Servo Range
  angle4 = map(PotVal4, 0, 1023, 0, 160); //Scales Converted Values Between 0 and 1023 to Angles Setting Servo Range
  angle5 = map(PotVal5, 0, 1023, 20, 150); //Scales Converted Values Between 0 and 1023 to Angles Setting Servo Range
  angle6 = map(PotVal6, 0, 1023, 45, 125); //Scales Converted Values Between 0 and 1023 to Angles Setting Servo Range
  // print out the angle for the servo motor
  Serial.print("Angle 1: ");
  Serial.print(angle1);
  Serial.print(", ");
  Serial.print("Angle 2: ");
  Serial.print(angle2);
  Serial.print(", ");
  Serial.print("Angle 3: ");
  Serial.print(angle3);
  Serial.print(", ");
  Serial.print("Angle 4: ");
  Serial.print(angle4);
  Serial.print(", ");
  Serial.print("Angle 5: ");
  Serial.print(angle5);
  Serial.print(", ");
  Serial.print("Angle 6: ");
  Serial.print(angle6);
  Serial.println(".");

  Servo1.write(angle1); //Moves Specified Servo to Specified Angle
  Servo2.write(angle2); //Moves Specified Servo to Specified Angle
  Servo3.write(angle3); //Moves Specified Servo to Specified Angle
  //Servo4.write(angle4); //Moves Specified Servo to Specified Angle
  Servo5.write(angle5); //Moves Specified Servo to Specified Angle
  Servo6.write(angle6); //Moves Specified Servo to Specified Angle

  delay(15); //Gives Servos time to move to position
}