/*

        Robot arm test script 2.0          Dec12        alanesq@disroot.org
        
        Uses 2 stepper motors to move a simple 2 dimensional robotic arm
        
        the arm is positioned by giving an x,y coordinate and via the gift of inverse kinematics 
        this is translated to the required joint angles.

*/


#include <Servo.h>    // servo library


// define robot arm mengths
#define humerus 134                       // Humerus length (mm)
#define ulna 134                          // Ulna length (mm)


//stepper motors
  int motorSPin1 = 4;	// Blue   - 28BYJ48 pin 1
  int motorSPin2 = 5;	// Pink   - 28BYJ48 pin 2
  int motorSPin3 = 6;	// Yellow - 28BYJ48 pin 3
  int motorSPin4 = 7;	// Orange - 28BYJ48 pin 4

  int motorEPin1 = 8;	// Blue   - 28BYJ48 pin 1
  int motorEPin2 = 9;	// Pink   - 28BYJ48 pin 2
  int motorEPin3 = 10;	// Yellow - 28BYJ48 pin 3
  int motorEPin4 = 11;	// Orange - 28BYJ48 pin 4

  int motorSpeed = 1600;  //variable to set stepper speed
  int countsperrev = 4000;  // number of steps per full revolution
  int lookup[8] = {B01000, B01100, B00100, B00110, B00010, B00011, B00001, B01001};   // stepper coil sequence
  int count = 0;    // temp counter
  

// define global variables 
  int shoulderPos = 1000;       // current position of shoulder (in stepper motor steps)
  int elbowPos = 1000;          // current position of elbow (in stepper motor steps)
  int shoulderStep = 0;         // current stepper motor sequence step (0-7) 
  int elbowStep = 0;            // current stepper motor sequence step (0-7) 


// pre calculations
  float hum_sq = humerus * humerus;        // humurus length squared
  float uln_sq = ulna * ulna;              // ulna length squared

 

// ***************************************************************************


void setup() 
{ 
  //start serial connection
  Serial.begin(9600);
  Serial.println("Robot arm demo sketch v2.0");


  //declare the stepper motor pins as outputs
  pinMode(motorSPin1, OUTPUT);
  pinMode(motorSPin2, OUTPUT);
  pinMode(motorSPin3, OUTPUT);
  pinMode(motorSPin4, OUTPUT);

  pinMode(motorEPin1, OUTPUT);
  pinMode(motorEPin2, OUTPUT);
  pinMode(motorEPin3, OUTPUT);
  pinMode(motorEPin4, OUTPUT);


  resetArm();     // reset arm to it's start position

} 
 
 
// ***************************************************************************


void loop() 
{ 

  // if serial data waiting then read input and move arm
    if(Serial.available()) moveArmFromSerial();
     
  delay (100);

}

  
  
//// move arm along a straight line - up down
//  while(1) {
//    for ( int i=30; i < 210; i = i + 20)  {
//      moveArm ( 140,i );
//      delay(300);
//    }
//    for ( int i=210; i > 30; i = i - 20)  {
//      moveArm ( 140,i );
//      delay(300);
//    }
//  }



//  // move arm along a straight line - left right
//  while(1) {
//    for ( int i=50; i < 200; i = i + 10)  {
//      moveArm ( i,140 );
//      delay(300);
//    }
//    for ( int i=220; i > 50; i = i - 10)  {
//      moveArm ( i,140 );
//      delay(300);
//    }
//  }




//  // move around in a circle
//  float h = 130.0;          // x coordinate of circle center
//  float k = 130.0;          // y coordinate of circle center
//  float r = 50.0;           // radius of circle
//  float cstep = 6.283 / 60; // steps   (2 pi divided by steps)
//  while(1) {
//    for ( float theta = 0; theta < 6.283; theta = theta + cstep)  {
//        float x = h + r*cos(theta);
//        float y = k + r*sin(theta);
//        moveArm( x,y);
//    }
//  }
   


// ***************************************************************************
//                                procedures
// ***************************************************************************


//  Move the arm to specified x,y position 


  void moveArm ( float x, float y) {
  
      // calculate required angles using inverse kinematics 
      Serial.println("Calculate angles:");
      
      // Work out the length of an imaginery line from the arms shoulder to the x,y position and call it B
      // using pythagoras theorem - length of b squared = x sqared + y squared
      float B = sqrt ( ( x * x ) + ( y * y ) );
      Serial.print("  B = ");     Serial.println( B );
  
      // Calculate the angle of the imaginary line from the x axis and call it QA
      float QA = atan2 ( y , x ); 
      Serial.print("  QA = ");     Serial.println( QA );
  
      // Calculate the angle from the imaginary line to the humerus (using cosine rule) and call it QB
      float B_sq = B * B;    // B squared
      float tvala = hum_sq - uln_sq + B_sq ;
      float tvalb = 2.0 * humerus * B;
      float QB = acos ( tvala / tvalb ); 
      Serial.print("  QB = ");     Serial.println( QB );
      
      // Calculate angle of shoulder by ading the two calculated angles together
      float angleS = QA + QB;
      Serial.print("  angleS = ");     Serial.println( angleS );
  
      // Calculate angle of elbow
      // this is done using the cosine rule 
      tvala = hum_sq + uln_sq - B_sq ;
      tvalb = 2.0 * humerus * ulna;
      float angleE = acos ( tvala / tvalb ); 
      Serial.print("  angleE = ");     Serial.println( angleE );
      
      
      // convert angles from radians to degrees
      angleE = angleE * RAD_TO_DEG;
      angleS = angleS * RAD_TO_DEG;
      
  
      Serial.print("moving arm to x=");  Serial.print(x);  Serial.print(" Y=");  Serial.println(y);
      Serial.print("  angleS in degrees = ");     Serial.println( angleS );
      Serial.print("  angleE in degrees = ");     Serial.println( angleE );
      moveMotors(angleS,angleE);    // move the arm
    
  }


// ***************************************************************************


// Move motors to given angles


  void moveMotors ( int s, int e ) {

    
    int starget = s * ( countsperrev / 360 );     // convert target angle to motor steps
    int etarget = e * ( countsperrev / 360 );     // convert target angle to motor steps
  
    while ( shoulderPos != starget || elbowPos != etarget ) {    // keep going until both angles are correct
  
        if ( shoulderPos < starget ) {
          moveStepper ( 0 , 1 );      
          delayMicroseconds(motorSpeed);
        }
        if ( shoulderPos > starget ) {
          moveStepper ( 0 , -1 );
          delayMicroseconds(motorSpeed);
        }
        if ( elbowPos < etarget ) {
          moveStepper ( 1 , 1 );
          delayMicroseconds(motorSpeed);
        }        
        if ( elbowPos > etarget ) {
          moveStepper ( 1 , -1 );
          delayMicroseconds(motorSpeed);
        }        
  
    }
    
    allOff();    // all motor coils off
    
  }


// ***************************************************************************


// move stepper motor one step
//   motor = which motor 0 or 1   (0=shoulder 1=elbow)
//   direct = which direction    (1=clockwise, -1=anticlockwise)


  void moveStepper(int motor, int direct)
  {

    if (motor == 0) {
      // shoulder
      shoulderStep = shoulderStep - direct;      // increment current stepper motor step (0-7) 
      if (shoulderStep < 0) shoulderStep = 7;
      if (shoulderStep > 7) shoulderStep = 0;
      shoulderPos = shoulderPos + direct;    // increment motor position flag
      digitalWrite(motorSPin1, bitRead(lookup[shoulderStep], 0));
      digitalWrite(motorSPin2, bitRead(lookup[shoulderStep], 1));
      digitalWrite(motorSPin3, bitRead(lookup[shoulderStep], 2));
      digitalWrite(motorSPin4, bitRead(lookup[shoulderStep], 3));
      // also move elbow so it stays in position relative to shoulder arm
      elbowStep = elbowStep + direct;      // increment current stepper motor step (0-7) 
      if (elbowStep < 0) elbowStep = 7;
      if (elbowStep > 7) elbowStep = 0;
      digitalWrite(motorEPin1, bitRead(lookup[elbowStep], 0));
      digitalWrite(motorEPin2, bitRead(lookup[elbowStep], 1));
      digitalWrite(motorEPin3, bitRead(lookup[elbowStep], 2));
      digitalWrite(motorEPin4, bitRead(lookup[elbowStep], 3));
          }
    else {
      // elbow
      elbowStep = elbowStep + direct;      // increment current stepper motor step (0-7) 
      if (elbowStep < 0) elbowStep = 7;
      if (elbowStep > 7) elbowStep = 0;
      elbowPos = elbowPos + direct;    // increment motor position flag
      digitalWrite(motorEPin1, bitRead(lookup[elbowStep], 0));
      digitalWrite(motorEPin2, bitRead(lookup[elbowStep], 1));
      digitalWrite(motorEPin3, bitRead(lookup[elbowStep], 2));
      digitalWrite(motorEPin4, bitRead(lookup[elbowStep], 3));
    }
  }


// ***************************************************************************


// all motor outputs off (make sure no coils left energized when not in use)

  void allOff() {

      digitalWrite(motorSPin1,0);
      digitalWrite(motorSPin2,0);
      digitalWrite(motorSPin3,0);
      digitalWrite(motorSPin4,0);
      digitalWrite(motorEPin1,0);
      digitalWrite(motorEPin2,0);
      digitalWrite(motorEPin3,0);
      digitalWrite(motorEPin4,0);

  }


// ***************************************************************************


// find arms position and move to default start location

  void resetArm() {
  
    // move elbow to stop then back to 90 degrees
    for ( int i=0; i < ( countsperrev / 2.5); i++) {
      moveStepper(1,-1);
      delayMicroseconds(motorSpeed);
    }
    for ( int i=0; i < ( countsperrev / 4.4); i++) {
      moveStepper(1,1);
      delayMicroseconds(motorSpeed);
    }
  
  
    // reset arm location flags
      shoulderPos = 1000;       // current position of shoulder (in stepper motor steps)
      elbowPos = 1000;          // current position of elbow (in stepper motor steps)

    allOff();   // turn all motor outputs off
    
  }


// ***************************************************************************


// read serial input and move arm if anything recived


  void moveArmFromSerial() {

    const int serStrLen = 30;
    char serInStr[ serStrLen ];  // array that will hold the serial input string
    int input1, input2;
    memset( serInStr, 0, sizeof(serInStr) ); // set it all to zero

    delay(10);  // wait a little for serial data
   
    int i = 0;
    while(Serial.available() && i<serStrLen ) {
      serInStr[i] = Serial.read();
      i++;
    }
    Serial.print ("Input = ");  Serial.println( serInStr );

    char* str = serInStr;      // Save the remaining string for inputs.
     
    input1 = atoi(str);        // Next command is input1, capture and save it.

    while( (*++str == ',') == 0 );    // find comma
    *++str;                           // jump past comma

    input2 = atoi(str);        // Next command is input2, capture and save it.
 
    delay(30);                 // Delay to settle the UART buffer.
     
    Serial.print("Moving arm to ");  Serial.print( input1 );  Serial.print( " , " );  Serial.println( input2 );
    moveArm(input1,input2);
    allOff();       // all motor outputs off

}


// ***************************************************************************
// end