/*
Program: Make a linear actuator go back and forth automatically
File: control_linear_actuator.ino
Description: This program makes the Actuonix L16 Actuator 100mm 150:1 6V RC Control 
  go back and forth.
Author: Addison Sears-Collins
Website: https://automaticaddison.com
Date: October 5, 2020
*/
 
#include <VarSpeedServo.h> 

//////////// Linear Actuator Variables ///////////////
// Create the object.
VarSpeedServo linearActuator; 

// Attach motor to digital pin on the Arduino
const int linearActuatorPin = 12;

// Set the home and extended positions of the linear actuator
const int linearActuatorHomePos = 0;
const int linearActuatorExtPos = 180;

// Stores the current position of the linear actuator
int linearActuatorPos = 0;

// The amount of time in milliseconds alloted to enable the linear actuator to move into position
// The more delay given, the more the linear actuator extends. 100ms, for example, will result
// in full extension of the linear actuator.
const int linearActuatorDelay = 50;
//////////// End Linear Actuator Variables ///////////////

void setup() {  

    // Attach the linear actuator to the pin 
    linearActuator.attach(linearActuatorPin);  
}
 
void loop() {  

  // Linear actuator goes to extended position and then back to home position
  moveLinearActuator();
}

void moveLinearActuator() {
  
  for (linearActuatorPos = linearActuatorHomePos; linearActuatorPos <= linearActuatorExtPos; linearActuatorPos += 1) { // goes from home position to extended position
    linearActuator.write(linearActuatorPos);       
    delay(linearActuatorDelay);  // wait for the actuator to reach the position
  }
  for (linearActuatorPos = linearActuatorExtPos; linearActuatorPos >= linearActuatorHomePos; linearActuatorPos -= 1) { // goes from extended position to home position
    linearActuator.write(linearActuatorPos);          
    delay(linearActuatorDelay);  // wait for the actuator to reach the position
  }  
}

/*
Program: Make an electric lifting magnet turn ON and OFF repeatedly.
File: control_electric_magnet_arduino_v1.ino
Description: This program makes an electric lifting magnet turn ON and OFF repeatedly.
Author: Addison Sears-Collins
Website: https://automaticaddison.com
Date: October 6, 2020
*/

const int magnetSignalPin = 10;    // select the pin that will control the magnet

void setup() {
  pinMode(magnetSignalPin, OUTPUT);    // sets digital pin 10 as output
}

void loop() {
  activateMagnet();
  delay(10000);            // waits for 10 seconds
  deactivateMagnet();
  delay(3000);            // wait for 3 seconds
}

void activateMagnet() {  
  digitalWrite(magnetSignalPin, HIGH); // sets the digital pin ON  
}

void deactivateMagnet() {  
  digitalWrite(magnetSignalPin, LOW); // sets the digital pin OFF  
}

/*
Program: Perform a pick and place task with a SCARA-style robotic 
  arm using inverse kinematics (graphical method)
File: pick_and_place_no_vision_v1.ino
Description: This program performs a pick and place task with a SCARA-style robotic arm. 
  No computer vision is used.
Author: Addison Sears-Collins
Website: https://automaticaddison.com
Date: October 9, 2020
*/
 
#include <VarSpeedServo.h> 

//////////// Servo Motor Variables ///////////////

// Define the number of servos
#define SERVOS 2

// Create the servo objects.
VarSpeedServo myservo[SERVOS]; 

// Speed of the servo motors
// Speed=1: Slowest
// Speed=255: Fastest.
const int servoSpeed = 50;

// Attach servos to digital pins on the Arduino
int servoPins[SERVOS] = {3,5};

//////////// End Servo Motor Variables ///////////////

//////////// Linear Actuator Variables ///////////////

// Create the linear servo object.
VarSpeedServo linearActuator; 

// Attach motor to digital pin on the Arduino
const int linearActuatorPin = 12;

// Set the home position of the linear actuator
const int linearActuatorHomePos = 0;

// Stores the current position of the linear actuator
int linearActuatorPos = 0;

// The amount of time in milliseconds alloted to enable the linear actuator to move into position
// The more delay given, the more the linear actuator extends. 100ms, for example, will result
// in full extension of the linear actuator. Tweak this input here until you get desired results.
const int linearActuatorDelay = 45;

//////////// End Linear Actuator Variables ///////////////

// Link lengths in centimeters
const double a1 = 4.85;
const double a2 = 5.80;
const double a3 = 5.15;
const double a4 = 6.0;

// Select the pin that will control the magnet
const int magnetSignalPin = 10;    

void setup() {

  // Move servos to the home position of 0 degrees
  setupServos(); 

  // Set magnet pin
  pinMode(magnetSignalPin, OUTPUT);    

  // Attach the linear actuator to the digital pin of the Arduino 
  linearActuator.attach(linearActuatorPin);  
  linearActuator.write(0); 
  delay(1000); 
}
 
void loop() {

  // The pick location in centimeters in the 
  // robot's base frame (pick up the object from here)
  double xPosPick = 4.0;
  double yPosPick = 10.0;

  // The place location in centimeters in the 
  // robot's base frame (place the object here)
  double xPosPlace = -4.0;
  double yPosPlace = 10.0;

  // Pick up an object from one location, and place it in another
  pickAndPlace(xPosPick, yPosPick, xPosPlace, yPosPlace);

  while(1) {
    // Return home
    returnHome();
  }
}     

/*  This method converts the desired angle for Servo 1 into a control angle
 *  for Servo 1. It assumes that the 0 degree position on the kinematic
 *  diagram for Servo 1 is actually 90 degrees on the actual servo.
 *  The angle range for Servo 1 on the kinematic diagram is
 *  -90 to 90 degrees, with 0 degrees being the center position. 
 *  The actual servo range for the physical motor
 *  is 0 to 180 degrees. We convert the desired angle 
 *  to a value within that range.
 */
int calcServo1Angle (int inputAngle) {
  
  int result;

  result = map(inputAngle, -90, 90, 0, 180);

  return result;
}  

/* Move servos to the home position */
void setupServos() {
  
  // Attach the servos to the servo object 
  // attach(pin, min, max  ) - Attaches to a pin 
  //   setting min and max values in microseconds
  //   default min is 544, max is 2400  
  // Alter these numbers until both servos have a 
  //   180 degree range.
  myservo[0].attach(servoPins[0], 544, 2475);  
  myservo[1].attach(servoPins[1], 500, 2475); 

  // Set initial servo positions to home position
  myservo[0].write(0, servoSpeed, true);  
  myservo[1].write(calcServo1Angle(0), servoSpeed, true);

  // Wait to let servos get into position
  delay(3000);
}

/* Move arm to the x and y position indicated by the parameters */
void moveArm(double xPos, double yPos) {

  // Define the inverse kinematics variables
  double r1 = 0.0;
  double phi_1 = 0.0;
  double phi_2 = 0.0;
  double phi_3 = 0.0;
  double theta_1 = 0.0;
  double theta_2 = 0.0;
  
  /* Calculate the inverse kinematics*/
  // (1) r1  = square_root((x_0_2)^2 + (y_0_2)^2) 
  // Value is in centimeters
  r1 = sqrt((xPos * xPos) + (yPos * yPos));

  // (2) ϕ1 = arccos((a4^2 - r1^2 - a2^2)/(-2*r1*a2))
  // The returned value is in the range [0, pi] radians. 
  phi_1 = acos(((a4 * a4) - (r1 * r1) - (a2 * a2))/(-2.0 * r1 * a2));

  // (3) ϕ2 = arctan((y_0_2) / (x_0_2)) 
  // The atan2() function computes the principal value of the 
  // arc tangent of y/x, using the signs of both arguments to 
  // determine the quadrant of the return value. 
  // The returned value is in the range [-pi, +pi] radians.
  phi_2 = atan2(yPos, xPos);

  // (4) θ1 =  ϕ1 - ϕ2
  // Value is in radians
  theta_1 = phi_2 - phi_1;

  // (5) ϕ3 = arccos((r1^2 - a2^2 - a4^2)/(-2*a2*a4))
  // Value is in radians
  phi_3 = acos(((r1 * r1) - (a2 * a2) - (a4 * a4))/(-2.0 * a2 * a4));

  //(6) θ2 = 180° - ϕ3 
  theta_2 = PI - phi_3;
    
  /* Convert the joint (servo) angles from radians to degrees*/
  theta_1 = theta_1 * RAD_TO_DEG; // Joint 1
  theta_2 = theta_2 * RAD_TO_DEG; // Joint 2

  /* Move the end effector to the desired x and y position */
  // Set Joint 1 (Servo 0) angle
  myservo[0].write(theta_1, servoSpeed, true); 

  // Set Joint 2 (Servo 1) angle
  myservo[1].write(calcServo1Angle(theta_2), servoSpeed, true); 
 
  // Wait
  delay(1000);
}

/* Activate the electric lifting magnet */
void activateMagnet() {  
  digitalWrite(magnetSignalPin, HIGH); // sets the digital pin ON  
}

/* Deactivate the electric lifting magnet */
void deactivateMagnet() {  
  digitalWrite(magnetSignalPin, LOW); // sets the digital pin OFF 
}

/* Move linear actuator down and then up */
void moveLinearActuator(int extension) {
  
  // goes from home position to extended position
  for (linearActuatorPos = linearActuatorHomePos; linearActuatorPos <= extension; linearActuatorPos += 1) { 
    linearActuator.write(linearActuatorPos); 
    delay(linearActuatorDelay);                       // wait for the actuator to reach the position
  }
  
  // goes from extended position to home position
  for (linearActuatorPos = extension; linearActuatorPos >= linearActuatorHomePos; linearActuatorPos -= 1) { 
    linearActuator.write(linearActuatorPos);          
    delay(linearActuatorDelay * 2);                          // wait for the actuator to reach the position
  }  
}

/* Pick up an object from one location, and place it in another */
void pickAndPlace(double xPosPick, double yPosPick, double xPosPlace, double yPosPlace) {
  
  // Move arm to the pick location
  moveArm(xPosPick, yPosPick);

  activateMagnet();

  // Pick up the object
  moveLinearActuator(180);

  // Move arm to the place location
  moveArm(xPosPlace, yPosPlace);

  deactivateMagnet();

  // Place the object (tweak this input here until you get desired results)
  moveLinearActuator(120);

  delay(1000); 
}

/* Return to the 0 degree home position */
void returnHome() {
  
  // Set servo positions to home position
  myservo[0].write(0, servoSpeed, true);  
  myservo[1].write(calcServo1Angle(0), servoSpeed, true);
}