//JM_Robotics Thruster and LED tester//
 
#include <Servo.h>

// Lumen 1
Servo lumen1;
const int pot1 = A0;
int lumen1Pin = 11;
int lumen1Value;

// Lumen 2
Servo lumen2;
const int pot2 = A1;
int lumen2Pin = 10;
int lumen2Value;

// gripper
Servo gripper;
#define GRIPPER_PWM_PIN 9
#define OPEN 7
#define CLOSE 6

#define OPEN_GRIPPER_US     1100 // 1100
#define CLOSE_GRIPPER_US    1900 // 1900

int servoPos = 1500; // gripper start position 1500
byte  servoIncrement = 50; // 50

int buttonState = 0;
int buttonState2 = 0;


void setup() {
  // put your setup code here, to run once:
Serial.begin(9600);

// lumen setup

lumen1.attach(lumen1Pin);
lumen1.writeMicroseconds(1100); // lumen off signal

lumen2.attach(lumen2Pin);
lumen2.writeMicroseconds(1100);

// gripper setup
gripper.attach(GRIPPER_PWM_PIN);
pinMode(OPEN, INPUT_PULLUP);
pinMode(CLOSE, INPUT_PULLUP);






}

void loop() {
  // put your main code here, to run repeatedly:


// lumen loop
lumen1Value = analogRead(pot1);
lumen1Value = map(lumen1Value, 0, 1023, 0, 180); //change to 1100, 1900
lumen1.write(lumen1Value);

lumen2Value = analogRead(pot2);
lumen2Value = map(lumen2Value, 0, 1023, 0, 180); //change to 1100, 1900
lumen2.write(lumen2Value);

// gripper loop



buttonState = digitalRead(OPEN);
buttonState2 = digitalRead(CLOSE);

if (buttonState == LOW)
{
  Serial.println("opening gripper");
  servoPos = servoPos - servoIncrement;
  if (servoPos <= OPEN_GRIPPER_US) servoPos = OPEN_GRIPPER_US;
  gripper.writeMicroseconds(servoPos);
}


if (buttonState2 == LOW)
{
  Serial.println("closing gripper");
  servoPos = servoPos + servoIncrement;
  if (servoPos >= CLOSE_GRIPPER_US) servoPos = CLOSE_GRIPPER_US;
  gripper.writeMicroseconds(servoPos);
}



delay(100);

}