#include <Wire.h> 
//#include <SPI.h> 
//#include <Adafruit_I2CDevice.h>
#include <Adafruit_PWMServoDriver.h>

#define MIN_PULSE_WIDTH       650
#define MAX_PULSE_WIDTH       2350
#define FREQUENCY             50
#define SERIAL_DEBUG

#define  GRAB_ANGLE_PW 180
#define  OPEN_ANGLE_PW 90
Adafruit_PWMServoDriver Af_pwm = Adafruit_PWMServoDriver();

int potWrist    = A3;
int potElbow    = A2;                     //Assign Potentiometers to pins on Arduino Uno
int potShoulder = A1;
int potBase     = A0;
int Port_SwGrab = 7;

int hand      = 11;
int wrist     = 12;
int elbow     = 13;               //Assign Motors to pins on Servo Driver Board
int shoulder  = 14;
int base      = 15;

int sensorPin = A0;    // select the input pin for the potentiometer
int ledPin = 13;      // select the pin for the LED
int sensorValue = 0;  // variable to store the value coming from the sensor
bool Alter_OnOff;
//========================================//
void setup() 
{
  // declare the ledPin as an OUTPUT:
  pinMode(ledPin, OUTPUT);
  
  Af_pwm.begin();
  Af_pwm.setPWMFreq(FREQUENCY);
  Af_pwm.setPWM(hand, 0, 90);                //Set Gripper to 90 degrees (Close Gripper)  
  pinMode(Port_SwGrab,INPUT_PULLUP);
  Serial.begin(38400);
}
//========================================// 
void moveMotor(int controlIn, int motorOut)
{
  int pulse_wide, pulse_width, potVal; 
  potVal = analogRead(controlIn);                                        //Read value of Potentiometer  
#if defined SERIAL_DEBUG   
    Serial.print(controlIn); Serial.print(" Analog Value Read is "); Serial.println(potVal);
#endif
  pulse_wide = map(potVal, 800, 240, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);
#if defined SERIAL_DEBUG   
    Serial.print(" pulse_wide is"); Serial.println(pulse_wide); 
#endif
  pulse_width = int(float(pulse_wide) / 1000000 * FREQUENCY * 4096);     //Map Potentiometer position to Motor  
#if defined SERIAL_DEBUG   
    Serial.print(pulse_width); Serial.print(" pulse_width in motorOut is "); Serial.println(motorOut);
#endif
  Af_pwm.setPWM(motorOut, 0, pulse_width);
}
//========================================// 
void loop() 
{
  // read the value from the sensor:
  sensorValue = analogRead(sensorPin);
  //analogWrite(ledPin, sensorValue);

  // turn the ledPin on/off
  Alter_OnOff=!Alter_OnOff;
  digitalWrite(ledPin, Alter_OnOff);
  // stop the program for <sensorValue> milliseconds:
  delay(sensorValue);

  moveMotor(potWrist, wrist); 
  moveMotor(potElbow, elbow);             //Assign Motors to corresponding Potentiometers
  moveMotor(potShoulder, shoulder);
  moveMotor(potBase, base);

  int pushButton = digitalRead(Port_SwGrab);
  if(pushButton == LOW)
  {
    Af_pwm.setPWM(hand, 0, GRAB_ANGLE_PW);   //180//Keep Gripper closed when button is not pressed
    Serial.println("Grab");
#if defined SERIAL_DEBUG   
    Serial.print(GRAB_ANGLE_PW); Serial.print(" pulse_width in motorOut is "); Serial.println(hand);
#endif
  }
  else
  {
    Af_pwm.setPWM(hand, 0, OPEN_ANGLE_PW);   //90//Open Gripper when button is pressed
    Serial.println("Release");
#if defined SERIAL_DEBUG   
    Serial.print(OPEN_ANGLE_PW); Serial.print(" pulse_width in motorOut is "); Serial.println(hand);
#endif
  }
}
//========================================//