/*
  Claw Machine PCB
  Zachary Hwang

  AVR128DB32 microcontroller
  3x A4988 stepper motors drivers
  Control claw machine's 3 stepper motors
  1x SG90 servo PWM
  5 direction input buttons
  3 limit switch inputs

  Coordinate System
  Origin - back left, over prize drop hole
  X-axis goes to the right
  Y-axis goes away from the front, forwards or "up"
*/

#include <Servo_DxCore.h>

// Define Constants

// Claw servo setup
Servo claw;
const int clawServoOpen = 100;
const int clawServoClosed = 40; // may need to adjust*

// Connections to A4988, 3x stepper motors
const int dirPinX = PIN_PA2;  // Direction X
const int stepPinX = PIN_PA3; // Step X
const int dirPinY = PIN_PA4;  // Direction Y
const int stepPinY = PIN_PA5; // Step Y
const int dirPinZ = PIN_PA6;  // Direction Z
const int stepPinZ = PIN_PA7; // Step Z

// Connections to claw servo; forgot to include in schematic, use pin PD5 janky solder
const int clawServoPin = PIN_PD5; // claw servo

// Connections to buttons
const int buttonXn = PIN_PC0; // x negative (left)
const int buttonXp = PIN_PC1; // x positive (right)
const int buttonYn = PIN_PC2; // y negative (back)
const int buttonYp = PIN_PC3; // y positive (forward)
const int buttonZ = PIN_PD1;  // activates z drop, grab, return home, release algorithm

const int limitSwitchX = PIN_PD2;
const int limitSwitchY = PIN_PD3;
const int limitSwitchZ = PIN_PF4;

const int STEPS_PER_REV = 200;// Motor steps per revolution, NOT CORRECT
const int PWMpulse = 1000;    // delay in microseconds for stepper PWM step pulse

// Declare variables for tracking button presses
int buttonStateXn = 0;
int buttonStateXp = 0;
int buttonStateYn = 0;
int buttonStateYp = 0;
int buttonStateZ = 0;
int limitSwitchStateX = 0;
int limitSwitchStateY = 0;
int limitSwitchStateZ = 0;

void setup() {
  Serial.begin(9600);
  
  // Set up pins as Outputs
  claw.attach(clawServoPin); // attach servo to PD5 pin on AVR128
  pinMode(dirPinX,OUTPUT);
  pinMode(stepPinX,OUTPUT);
  pinMode(dirPinY,OUTPUT);
  pinMode(stepPinY,OUTPUT); 
  pinMode(dirPinZ,OUTPUT);
  pinMode(stepPinZ,OUTPUT); 
  
  // Set up pins as Inputs
  pinMode(buttonXn,INPUT);
  pinMode(buttonXp,INPUT);
  pinMode(buttonYn,INPUT);
  pinMode(buttonYp,INPUT);
  pinMode(buttonZ,INPUT);
  pinMode(limitSwitchX,INPUT);
  pinMode(limitSwitchY,INPUT);
  pinMode(limitSwitchZ,INPUT);

  // initial buttons read
  buttonStateXn = digitalRead(buttonXn);
  buttonStateXp = digitalRead(buttonXp);
  buttonStateYn = digitalRead(buttonYn);
  buttonStateYp = digitalRead(buttonYp);  
  buttonStateZ = digitalRead(buttonZ);
  limitSwitchStateX = digitalRead(limitSwitchX);
  limitSwitchStateY = digitalRead(limitSwitchY); 
  limitSwitchStateZ = digitalRead(limitSwitchZ);

  // Set start position
//  claw.write(clawServoOpen);  // set initial claw position to open
//  goHome();                   // set initial crane position to home
}

void loop() {
  // read button states
  buttonStateXn = digitalRead(buttonXn);
  buttonStateXp = digitalRead(buttonXp);
  buttonStateYn = digitalRead(buttonYn);
  buttonStateYp = digitalRead(buttonYp);  
  buttonStateZ = digitalRead(buttonZ);
  limitSwitchStateX = digitalRead(limitSwitchX);
  limitSwitchStateY = digitalRead(limitSwitchY); 
  limitSwitchStateZ = digitalRead(limitSwitchZ);

  // move crane left on joystick left
  if (buttonStateXn == LOW) {
    if (limitSwitchStateX == HIGH) {
      goLeft();
      limitSwitchStateX = digitalRead(limitSwitchX);
    }
  }
  
  // move crane right on joystick right
  if (buttonStateXp == LOW) {
    goRight();
  }

  // move crane down/backwards on joystick down
  if (buttonStateYn == LOW) { 
    if (limitSwitchStateY == HIGH) {
      goDown();
      limitSwitchStateY = digitalRead(limitSwitchY);
    }
  }

  // move crane up/forwards on joystick up
  if (buttonStateYp == LOW) {
    goUp();
  }

  // activate grab algorithm on z button pressed
  if (buttonStateZ == LOW) {
    // open claw (should be open already, but run just in case)
    claw.write(clawServoOpen);

    // drop claw z for set number of steps
    for(int x = 0; x < 1000 ; x++) {
      digitalWrite(dirPinZ,HIGH);
      stepForward(stepPinZ);
    }
    delay(2000);
    
    // close claw
    claw.write(clawServoClosed);
    delay(1000);
    
    // move claw up until limit switch z hit
    while (limitSwitchStateZ == HIGH) {
      digitalWrite(dirPinZ,LOW);
      stepForward(stepPinZ);
      limitSwitchStateZ = digitalRead(limitSwitchZ);
    }
    
    // move claw off limit switch z a bit
    for(int x = 0; x < 100 ; x++) {
      digitalWrite(dirPinZ,HIGH);
      stepForward(stepPinZ);
    }
    delay(1000);
    goHome();
    claw.write(clawServoOpen);    
  }
}

void stepForward(int stepPin) {
  digitalWrite(stepPin,HIGH);
  delayMicroseconds(PWMpulse);
  digitalWrite(stepPin,LOW);
  delayMicroseconds(PWMpulse); 
}

void goLeft() {
  digitalWrite(dirPinX,HIGH);   // x motor HIGH goes left Xn (negative)
  stepForward(stepPinX);
}

void goRight() {
  digitalWrite(dirPinX,LOW);  // x motor LOW goes right Xp (positive)
  stepForward(stepPinX);
}

void goDown() {
  digitalWrite(dirPinY,LOW);  // y motor LOW goes "down" Yn (negative)
  stepForward(stepPinY);
}

void goUp() {
  digitalWrite(dirPinY,HIGH);  // y motor HIGH goes "up" Yp (positive)
  stepForward(stepPinY);
}

void goHome() {
  // move x and y stepper negative til limit switches hit
  while (limitSwitchStateX == HIGH || limitSwitchStateY == HIGH) {
    if (limitSwitchStateX == HIGH) {
      goLeft();
      limitSwitchStateX = digitalRead(limitSwitchX);
    }
    if (limitSwitchStateY == HIGH) {
      goDown();
      limitSwitchStateY = digitalRead(limitSwitchY);
    }
  }
  // move off limit switches a bit
  for(int x = 0; x < 50 ; x++) {
    goRight();
    goUp();
  }
}


/* old code

void stepBackward(int dirPin, int stepPin) {
  digitalWrite(dirPin,LOW);
  digitalWrite(stepPin,HIGH);
  delayMicroseconds(PWMpulse);
  digitalWrite(stepPin,LOW);
  delayMicroseconds(PWMpulse); 
}

void stepForward(int dirPin, int stepPin) {
  digitalWrite(dirPin,HIGH);
  digitalWrite(stepPin,HIGH);
  delayMicroseconds(PWMpulse);
  digitalWrite(stepPin,LOW);
  delayMicroseconds(PWMpulse); 
}

 */