WinchBot v1.5

//Project: WinchBot 1.5
//Homepage: www.HomoFaciens.de
//Author Norbert Heinz
//Version: 0.1
//Creation date: 04.12.2017
//
//compile with gcc WinchBot-1-5.c -o WinchBot-1-5 -I/usr/local/include -L/usr/local/lib -lwiringPi -lm -lpigpio -pthread -Wall
//For details see:
//https://www.homofaciens.de/technics-machines-winchbot-1-5_en.htm

#include <stdio.h>
#include <termios.h>
#include <sys/ioctl.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <sys/time.h>
#include <dirent.h>
#include <math.h>
#include <wiringPi.h>
#include <unistd.h>

#include <pigpio.h>
#include <signal.h>



//Base length of triangle in mm
#define BASELENGTH          895.0
//Initial cord length in mm
#define INITIAL_LENGTH       600.0
//Distance mount point cords to center of ball bearing mount at triangle
#define INITIAL_BAR_LENGTH   373.0
//Mount point cords on square tube
#define RADIUS_CORD_MOUNT     23.0
//Height from pivot point servo 4 to gripper
#define LEVER_S5 50.0
//Horizontal distance from pivot point servo 4 to tip of gripper
#define GRIPPER_LENGTH 60.0
//Horizontal offset from pivot point Servo 3 to centerline gripper
#define GRIPPER_OFFSET  5.0

//Time between two steps of platform motors
#define P_PAUSE 20000

//Time between two steps of WinchBot
#define STEP_PAUSE 500
//Pause needed for high signal on step input of motor driver
#define STEP_COMMAND_PAUSE 5

#define PI 3.1415927

//200 steps per turn, 7 teeth to 70 teeth
#define STEPS_PER_ANGLE  5.55555556

//Pin numbers WiringPi
#define M1_DIR          24            //Direction output motor
#define M1_STEP         25            //PWM output motor
#define M2_DIR          30            //Direction output motor
#define M2_STEP         21            //PWM output motor
#define M3_DIR          22            //Direction output motor
#define M3_STEP         23            //PWM output motor


#define ENABLE_MOTORS   26            //Motors disabled if pin LOW


#define MOTOR_P1_A   15
#define MOTOR_P1_B   16
#define MOTOR_P1_C    1
#define MOTOR_P1_D    4

#define MOTOR_P2_A   13
#define MOTOR_P2_B   14
#define MOTOR_P2_C   12
#define MOTOR_P2_D    3

#define FLASHLIGHT_1  5
#define FLASHLIGHT_2  6
#define FLASHLIGHT_3 10

//Pin numbers pigpio !!!

#define SERVO_3  16            //Servo arm rotate
#define SERVO_4  17            //Servo arm tilt
#define SERVO_5  27            //Servo gripper


#define SERVO_3_MIN  0
#define SERVO_3_MAX  180

#define SERVO_4_MIN  0
#define SERVO_4_MAX  180

#define SERVO_5_MIN  0
#define SERVO_5_MAX  180


int run = 1;
//12 teeth 80 teeth 2000 steps per revolution diameter drum 51.5mm
double StepsPermm = 13333.3 / 162.0;

double length1, length2, length3;


int Servo3 = 90;
int Servo4 = 90;
int Servo5 = 90;

#define SERVO_3_OFFSET  -30
#define SERVO_4_OFFSET  30
#define SERVO_5_OFFSET  0

long currentX = 0, currentY = 0, currentZ = 0;

double M1X, M1Y, M1Z;
double M2X, M2Y, M2Z;
double M3X, M3Y, M3Z;

int MotorP1Step = 0;
int MotorP2Step = 0;
double AngleP1 = 0.0;
double AngleP2 = 0.0;


//Catch term signal
void stop(int signum){
   run = 0;
}


//++++++++++++++++++++++++++++++ move servo 3 (arm rotate) ++++++++++++++++++++++++++++++++++++++++++++++++
void moveServo3(int direction){

  if((Servo3 < SERVO_3_MAX && direction > 0) || (Servo3 > SERVO_3_MIN && direction < 0)){
    Servo3 += direction;
    gpioServo(SERVO_3, (Servo3 + SERVO_3_OFFSET) * 1000 / 90 + 500);
  }
}

//++++++++++++++++++++++++++++++ move servo 4 (arm tilt) ++++++++++++++++++++++++++++++++++++++++++++++++++
void moveServo4(int direction){

  if((Servo4 < SERVO_4_MAX && direction > 0) || (Servo4 > SERVO_4_MIN && direction < 0)){
    Servo4 += direction;
    gpioServo(SERVO_4, (Servo4 + SERVO_4_OFFSET) * 1000 / 90 + 500);
  }
}

//++++++++++++++++++++++++++++++ move servo 5 (gripper) +++++++++++++++++++++++++++++++++++++++++++++++++++
void moveServo5(int direction){

  if((Servo5 < SERVO_5_MAX && direction > 0) || (Servo5 > SERVO_5_MIN && direction < 0)){
    Servo5 += direction;
    gpioServo(SERVO_5, (Servo5 + SERVO_5_OFFSET) * 1000 / 90 + 500);
  }
}

//++++++++++++++++++++++++++++++ move motor P1 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void motorP1(long steps, long uPause){
  long i;

  AngleP1 += steps;

  //printf("Moving motor1 for %ld steps\n", steps);


  for(i=0; i<abs(steps); i++){
    if(steps > 0){
      MotorP1Step++;
    }
    else{
      MotorP1Step--;
    }
    if(MotorP1Step > 3){
      MotorP1Step = 0;
    }
    if(MotorP1Step < 0){
      MotorP1Step = 3;
    }

    if(MotorP1Step == 0){
      digitalWrite(MOTOR_P1_A, 1);
      digitalWrite(MOTOR_P1_B, 0);
      digitalWrite(MOTOR_P1_C, 0);
      digitalWrite(MOTOR_P1_D, 0);
    }

    if(MotorP1Step == 1){
      digitalWrite(MOTOR_P1_A, 0);
      digitalWrite(MOTOR_P1_B, 0);
      digitalWrite(MOTOR_P1_C, 1);
      digitalWrite(MOTOR_P1_D, 0);
    }

    if(MotorP1Step == 2){
      digitalWrite(MOTOR_P1_A, 0);
      digitalWrite(MOTOR_P1_B, 1);
      digitalWrite(MOTOR_P1_C, 0);
      digitalWrite(MOTOR_P1_D, 0);
    }

    if(MotorP1Step == 3){
      digitalWrite(MOTOR_P1_A, 0);
      digitalWrite(MOTOR_P1_B, 0);
      digitalWrite(MOTOR_P1_C, 0);
      digitalWrite(MOTOR_P1_D, 1);
    }
    usleep(uPause);
  }

}

//++++++++++++++++++++++++++++++ move motor P2 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void motorP2(long steps, long uPause){
  long i;

  AngleP2 += steps;

  //printf("Moving motor1 for %ld steps\n", steps);


  for(i=0; i<abs(steps); i++){
    if(steps > 0){
      MotorP2Step++;
    }
    else{
      MotorP2Step--;
    }
    if(MotorP2Step > 3){
      MotorP2Step = 0;
    }
    if(MotorP2Step < 0){
      MotorP2Step = 3;
    }

    if(MotorP2Step == 0){
      digitalWrite(MOTOR_P2_A, 1);
      digitalWrite(MOTOR_P2_B, 0);
      digitalWrite(MOTOR_P2_C, 0);
      digitalWrite(MOTOR_P2_D, 0);
    }

    if(MotorP2Step == 1){
      digitalWrite(MOTOR_P2_A, 0);
      digitalWrite(MOTOR_P2_B, 0);
      digitalWrite(MOTOR_P2_C, 1);
      digitalWrite(MOTOR_P2_D, 0);
    }

    if(MotorP2Step == 2){
      digitalWrite(MOTOR_P2_A, 0);
      digitalWrite(MOTOR_P2_B, 1);
      digitalWrite(MOTOR_P2_C, 0);
      digitalWrite(MOTOR_P2_D, 0);
    }

    if(MotorP2Step == 3){
      digitalWrite(MOTOR_P2_A, 0);
      digitalWrite(MOTOR_P2_B, 0);
      digitalWrite(MOTOR_P2_C, 0);
      digitalWrite(MOTOR_P2_D, 1);
    }
    usleep(uPause);
  }

}

//++++++++++++++++++++++++++++++ move motor 1 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void motor1(long steps, long uPause){
  long i;
  length1 += steps;

  //printf("Moving motor1 for %ld steps\n", steps);

  if(steps < 0){
    digitalWrite(M1_DIR, 1);
  }
  else{
    digitalWrite(M1_DIR, 0);
  }

  if(steps != 0){
    for(i=0; i < labs(steps); i++){
      digitalWrite(M1_STEP, 1);
      usleep(STEP_COMMAND_PAUSE);
      digitalWrite(M1_STEP, 0);
      usleep(uPause);
    }
  }
}


//++++++++++++++++++++++++++++++ move motor 2 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void motor2(long steps, long uPause){
  long i;
  length2 += steps;

  //printf("Moving motor2 for %ld steps\n", steps);

  if(steps < 0){
    digitalWrite(M2_DIR, 1);
  }
  else{
    digitalWrite(M2_DIR, 0);
  }
  if(steps != 0){
    for(i=0; i < labs(steps); i++){
      digitalWrite(M2_STEP, 1);
      usleep(STEP_COMMAND_PAUSE);
      digitalWrite(M2_STEP, 0);
      usleep(uPause);
    }
  }
}

//++++++++++++++++++++++++++++++ move motor 3 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
void motor3(long steps, long uPause){
  long i;
  length3 += steps;

  //printf("Moving motor3 for %ld steps\n", steps);

  if(steps < 0){
    digitalWrite(M3_DIR, 1);
  }
  else{
    digitalWrite(M3_DIR, 0);
  }
  if(steps != 0){
    for(i=0; i < labs(steps); i++){
      digitalWrite(M3_STEP, 1);
      usleep(STEP_COMMAND_PAUSE);
      digitalWrite(M3_STEP, 0);
      usleep(uPause);
    }
  }
}


//++++++++++++++++++++++++++++++++++++++ MoveToCoordinates +++++++++++++++++++++++++++++++++++++++++++++++++++++
int MoveToCoordinates(long X, long Y, long Z, int Servo3New, int Servo4New, int Servo5New, long sleepTime){
  double length1New, length2New, length3New;
  double M1MountX, M1MountY, M1MountZ;
  double M2MountX, M2MountY, M2MountZ;
  double M3MountX, M3MountY, M3MountZ;
//  long offsetX, offsetY, offsetZ;
//  long tempX, tempY, tempZ;
  long stepDiff = 0;
//  double alpha = 90.0, beta = 90.0;
//  double alpha3;
//  double alpha4;
//  long barMountZ;


  double alpha, beta;
  double AngleP1New, AngleP2New;
  struct timeval tv;
  unsigned long uTimeStart, uTimeNow, longTemp;
  unsigned long uNowM1, uNowM2, uNowM3, uNowP1, uNowP2, uNowS3, uNowS4, uNowS5;
  unsigned long uDiffM1, uDiffM2, uDiffM3, uDiffP1, uDiffP2, uDiffS3, uDiffS4, uDiffS5;
  unsigned long uTimeDiff;


  printf("Moving to X=%ld, Y=%ld, Z=%ld, Servo3=%d, Servo4=%d, Servo5=%d\n", X, Y, Z, Servo3New, Servo4New, Servo5New);


  if(Servo3New > SERVO_3_MAX){
    Servo3New = SERVO_3_MAX;
  }
  if(Servo3New < SERVO_3_MIN){
    Servo3New = SERVO_3_MIN;
  }

  if(Servo4New > SERVO_4_MAX){
    Servo4New = SERVO_4_MAX;
  }
  if(Servo4New < SERVO_4_MIN){
    Servo4New = SERVO_4_MIN;
  }

  if(Servo5New > SERVO_5_MAX){
    Servo5New = SERVO_5_MAX;
  }
  if(Servo5New < SERVO_5_MIN){
    Servo5New = SERVO_5_MIN;
  }

/*
//  barMountZ = INITIAL_BAR_LENGTH * StepsPermm;

  offsetX = GRIPPER_OFFSET * StepsPermm;
  offsetY = -GRIPPER_LENGTH * StepsPermm;
  offsetZ = LEVER_S5 * StepsPermm;

  offsetX = 0;
  offsetY = 0;
  offsetZ = 0;


  //calculate X/Y/Z offset caused by rotation of servos 3 and 4
  alpha3 = Servo3New - 90;
  alpha4 = Servo4New - 90;

  tempZ = offsetZ * cos(alpha4 * PI / 180.0) - offsetY * sin(alpha4 * PI / 180.0);
  tempY = offsetY * cos(alpha4 * PI / 180.0) + offsetZ * sin(alpha4 * PI / 180.0);

  offsetY = tempY;

  tempX = offsetX * cos(alpha3 * PI / 180.0) - offsetY * sin(alpha3 * PI / 180.0);
  tempY = offsetY * cos(alpha3 * PI / 180.0) + offsetX * sin(alpha3 * PI / 180.0);

  tempX -= GRIPPER_OFFSET * StepsPermm;
  tempY += GRIPPER_LENGTH * StepsPermm;
  tempY = -tempY;
  tempZ -= LEVER_S5 * StepsPermm;

  offsetX = X + tempX;
  offsetY = Y + tempY;
  offsetZ = Z + tempZ;


  if(offsetY != 0){
    alpha = atan((double)(barMountZ - offsetZ) / (double)(offsetY)) * 180.0 / PI;
  }

  if(offsetX != 0){
    beta = atan((double)((barMountZ - offsetZ) / offsetX)) * 180.0 / PI;
  }




*/

  alpha = atan(Y/(INITIAL_BAR_LENGTH * StepsPermm - Z))*180 / PI;
  beta = atan(X/(INITIAL_BAR_LENGTH * StepsPermm - Z))*180 / PI;

  AngleP1New = round(alpha * STEPS_PER_ANGLE);
  AngleP2New = round(beta * STEPS_PER_ANGLE);



  M1MountX = X + RADIUS_CORD_MOUNT * StepsPermm * cos(30.0 * PI / 180.0) * cos(alpha) * cos(beta);
  M1MountY = Y - RADIUS_CORD_MOUNT * StepsPermm * sin(30.0 * PI / 180.0) * cos(alpha) * cos(beta);
  M1MountZ = Z - RADIUS_CORD_MOUNT * StepsPermm * sin(30.0 * PI / 180.0) * sin(alpha) * sin(beta);;

  M2MountX = X;
  M2MountY = Y + cos(alpha) * RADIUS_CORD_MOUNT * StepsPermm;
  M2MountZ = Z + sin(alpha) * RADIUS_CORD_MOUNT * StepsPermm;

  M3MountX = X - RADIUS_CORD_MOUNT * StepsPermm * cos(30.0 * PI / 180.0) * cos(alpha) * cos(beta);
  M3MountY = Y - RADIUS_CORD_MOUNT * StepsPermm * sin(30.0 * PI / 180.0) * cos(alpha) * cos(beta);
  M3MountZ = Z - RADIUS_CORD_MOUNT * StepsPermm * sin(30.0 * PI / 180.0) * sin(alpha) * sin(beta);;


  length1New = labs(sqrt(pow(M1X - M1MountX, 2.0) + pow(M1Y - M1MountY, 2.0) + pow(M1Z - M1MountZ, 2.0)));
  length2New = labs(sqrt(pow(M2X - M2MountX, 2.0) + pow(M2Y - M2MountY, 2.0) + pow(M2Z - M2MountZ, 2.0)));
  length3New = labs(sqrt(pow(M3X - M3MountX, 2.0) + pow(M3Y - M3MountY, 2.0) + pow(M3Z - M3MountZ, 2.0)));

  length1New = labs(sqrt(pow(M1X - X, 2.0) + pow(M1Y - Y, 2.0) + pow(M1Z - Z, 2.0)));
  length2New = labs(sqrt(pow(M2X - X, 2.0) + pow(M2Y - Y, 2.0) + pow(M2Z - Z, 2.0)));
  length3New = labs(sqrt(pow(M3X - X, 2.0) + pow(M3Y - Y, 2.0) + pow(M3Z - Z, 2.0)));

  stepDiff = abs(length1New - length1);
  if(abs(length2New - length2) > stepDiff){
    stepDiff = abs(length2New - length2);
  }
  if(abs(length3New - length3) > stepDiff){
    stepDiff = abs(length3New - length3);
  }
  if(abs(AngleP1New - AngleP1) > stepDiff){
    stepDiff = abs(AngleP1New - AngleP1);
  }
  if(abs(AngleP2New - AngleP2) > stepDiff){
    stepDiff = abs(AngleP2New - AngleP2);
  }
  if(abs(Servo3New - Servo3) > stepDiff){
    stepDiff = abs(Servo3New - Servo3);
  }
  if(abs(Servo4New - Servo4) > stepDiff){
    stepDiff = abs(Servo4New - Servo4);
  }
  if(abs(Servo5New - Servo5) > stepDiff){
    stepDiff = abs(Servo5New - Servo5);
  }

  printf("length1New - length1 = %lf, length2New - length2 = %lf, length3New - length3 = %lf\n", length1New - length1, length2New - length2, length3New - length3);



  gettimeofday(&tv,NULL);
  uTimeStart = 1000000 * tv.tv_sec + tv.tv_usec;
  longTemp = uTimeStart / 100000000;
  uTimeStart -= longTemp * 100000000;


  uTimeDiff = 0;

  if(abs(length1New - length1)!=0){
    uDiffM1 = stepDiff*STEP_PAUSE / abs(length1New - length1);
  }
  else{
    uDiffM1 = stepDiff*STEP_PAUSE +1000;
  }

  if(abs(length2New - length2)!=0){
    uDiffM2 = stepDiff*STEP_PAUSE / abs(length2New - length2);
  }
  else{
    uDiffM2 = stepDiff*STEP_PAUSE +1000;
  }

  if(abs(length3New - length3)!=0){
    uDiffM3 = stepDiff*STEP_PAUSE / abs(length3New - length3);
  }
  else{
    uDiffM3 = stepDiff*STEP_PAUSE +1000;
  }


  if(abs(AngleP1New - AngleP1)!=0){
    uDiffP1 = stepDiff*STEP_PAUSE / abs(AngleP1New - AngleP1);
  }
  else{
    uDiffP1 = stepDiff*STEP_PAUSE +1000;
  }

  if(abs(AngleP2New - AngleP2)!=0){
    uDiffP2 = stepDiff*STEP_PAUSE / abs(AngleP2New - AngleP2);
  }
  else{
    uDiffP2 = stepDiff*STEP_PAUSE +1000;
  }


  if(abs(Servo3New - Servo3)!=0){
    uDiffS3 = stepDiff*STEP_PAUSE / abs(Servo3New - Servo3);
  }
  else{
    uDiffS3 = stepDiff*STEP_PAUSE +1000;
  }

  if(abs(Servo4New - Servo4)!=0){
    uDiffS4 = stepDiff*STEP_PAUSE / abs(Servo4New - Servo4);
  }
  else{
    uDiffS4 = stepDiff*STEP_PAUSE +1000;
  }

  if(abs(Servo5New - Servo5)!=0){
    uDiffS5 = stepDiff*STEP_PAUSE / abs(Servo5New - Servo5);
  }
  else{
    uDiffS5 = stepDiff*STEP_PAUSE +1000;
  }



  uNowM1 = 0;
  uNowM2 = 0;
  uNowM3 = 0;

  uNowP1 = 0;
  uNowP2 = 0;

  uNowS3 = 0;
  uNowS4 = 0;
  uNowS5 = 0;

  while(uTimeDiff < stepDiff*STEP_PAUSE){
    gettimeofday(&tv,NULL);
    uTimeNow = 1000000 * tv.tv_sec + tv.tv_usec;
    longTemp = uTimeNow / 100000000;
    uTimeNow -= longTemp * 100000000;
    if(uTimeNow < uTimeStart){//Movement no longer than 100s to avoid overflow!
      uTimeNow += 100000000;
    }
    uTimeDiff = uTimeNow-uTimeStart;

    if(uTimeDiff - uNowM1 >= uDiffM1){
      if(length1New > length1){
        motor1(1, 0);
      }
      if(length1New < length1){
        motor1(-1, 0);
      }
      uNowM1+=uDiffM1;
    }

    if(uTimeDiff - uNowM2 >= uDiffM2){
      if(length2New > length2){
        motor2(1, 0);
      }
      if(length2New < length2){
        motor2(-1, 0);
      }
      uNowM2+=uDiffM2;
    }

    if(uTimeDiff - uNowM3 >= uDiffM3){
      if(length3New > length3){
        motor3(1, 0);
      }
      if(length3New < length3){
        motor3(-1, 0);
      }
      uNowM3+=uDiffM3;
    }

    if(uTimeDiff - uNowP1 >= uDiffP1){
      if(AngleP1New > AngleP1){
        motorP1(1, 0);
      }
      if(AngleP1New < AngleP1){
        motorP1(-1, 0);
      }
      uNowP1+=uDiffP1;
    }

    if(uTimeDiff - uNowP2 >= uDiffP2){
      if(AngleP2New > AngleP2){
        motorP2(1, 0);
      }
      if(AngleP2New < AngleP2){
        motorP2(-1, 0);
      }
      uNowP2+=uDiffP2;
    }

    if(uTimeDiff - uNowS3 >= uDiffS3){
      if(Servo3New > Servo3){
        moveServo3(1);
      }
      if(Servo3New < Servo3){
        moveServo3(-1);
      }
      uNowS3+=uDiffS3;
    }

    if(uTimeDiff - uNowS4 >= uDiffS4){
      if(Servo4New > Servo4){
        moveServo4(1);
      }
      if(Servo4New < Servo4){
        moveServo4(-1);
      }
      uNowS4+=uDiffS4;
    }

    if(uTimeDiff - uNowS5 >= uDiffS5){
      if(Servo5New > Servo5){
        moveServo5(1);
      }
      if(Servo5New < Servo5){
        moveServo5(-1);
      }
      uNowS5+=uDiffS5;
    }

  }


  while(length1New > length1){
    motor1(1, STEP_PAUSE);
    printf("length1New=%lf, length1=%lf\n", length1New, length1);
  }
  while(length1New < length1){
    motor1(-1, STEP_PAUSE);
    printf("length1New=%lf, length1=%lf\n", length1New, length1);
  }

  while(length2New > length2){
    motor2(1, STEP_PAUSE);
    printf("length2New=%lf, length2=%lf\n", length2New, length2);
  }
  while(length2New < length2){
    motor2(-1, STEP_PAUSE);
    printf("length2New=%lf, length2=%lf\n", length2New, length2);
  }

  while(length3New > length3){
    motor3(1, STEP_PAUSE);
    printf("length3New=%lf, length3=%lf\n", length3New, length3);
  }
  while(length3New < length3){
    motor3(-1, STEP_PAUSE);
    printf("length3New=%lf, length3=%lf\n", length3New, length3);
  }


  while(AngleP1New>AngleP1){
    motorP1(1, P_PAUSE);
    printf("AngleP1New=%lf, AngleP1=%lf\n", AngleP1New, AngleP1);
  }
  while(AngleP1New<AngleP1){
    motorP1(-1, P_PAUSE);
    printf("AngleP1New=%lf, AngleP1=%lf\n", AngleP1New, AngleP1);
  }

  while(AngleP2New>AngleP2){
    motorP2(1, P_PAUSE);
    printf("AngleP2New=%lf, AngleP2=%lf\n", AngleP2New, AngleP2);
  }
  while(AngleP2New<AngleP2){
    motorP2(-1, P_PAUSE);
    printf("AngleP2New=%lf, AngleP2=%lf\n", AngleP2New, AngleP2);
  }



  while(Servo3New>Servo3){
    moveServo3(1);
    printf("ServoNew3=%d, Servo3=%d\n", Servo3New, Servo3);
  }
  while(Servo3New<Servo3){
    moveServo3(-1);
    printf("ServoNew3=%d, Servo3=%d\n", Servo3New, Servo3);
  }

  while(Servo4New>Servo4){
    moveServo4(1);
    printf("ServoNew4=%d, Servo4=%d\n", Servo4New, Servo4);
  }
  while(Servo4New<Servo4){
    moveServo4(-1);
    printf("ServoNew4=%d, Servo4=%d\n", Servo4New, Servo4);
  }

  while(Servo5New>Servo5){
    moveServo5(1);
    printf("ServoNew5=%d, Servo5=%d\n", Servo5New, Servo5);
  }
  while(Servo5New<Servo5){
    moveServo5(-1);
    printf("ServoNew5=%d, Servo5=%d\n", Servo5New, Servo5);
  }




  currentX = X;
  currentY = Y;
  currentZ = Z;



  return 0;
}
//-------------------------------------- MoveToCoordinates -----------------------------------------------


//######################################################################
//################## Main ##############################################
//######################################################################

int main(int argc, char **argv){

  long i;
  long stepNumber = 0;
  FILE *commandsFile;
  FILE *ReplayFile;
  char commandsLine[1000];
  char TextLine[1000];
  char *pEnd;
  int stopPlot = 0;
  int coordReadNo = 0;
  long goToX, goToY, goToZ, goToS3, goToS4, goToS5;
  long sleepTime;
  int gripperOld = 90;

  if (wiringPiSetup () == -1){
    printf("Could not run wiringPiSetup!");
    exit(1);
  }

  if (gpioInitialise() < 0){
    printf("Could not run gpioInitialise!");
    exit(1);
  }

  gpioSetSignalFunc(SIGINT, stop);


  pinMode (M1_DIR, OUTPUT);
  pinMode (M1_STEP, OUTPUT);
  pinMode (M2_DIR, OUTPUT);
  pinMode (M2_STEP, OUTPUT);
  pinMode (M3_DIR, OUTPUT);
  pinMode (M3_STEP, OUTPUT);

  pinMode (ENABLE_MOTORS, OUTPUT);

  digitalWrite(ENABLE_MOTORS, 1);

  pinMode (MOTOR_P1_A, OUTPUT);
  pinMode (MOTOR_P1_B, OUTPUT);
  pinMode (MOTOR_P1_C, OUTPUT);
  pinMode (MOTOR_P1_D, OUTPUT);

  pinMode (MOTOR_P2_A, OUTPUT);
  pinMode (MOTOR_P2_B, OUTPUT);
  pinMode (MOTOR_P2_C, OUTPUT);
  pinMode (MOTOR_P2_D, OUTPUT);

  pinMode (FLASHLIGHT_1, OUTPUT);
  pinMode (FLASHLIGHT_2, OUTPUT);
  pinMode (FLASHLIGHT_3, OUTPUT);


  gpioServo(SERVO_3, (Servo3 + SERVO_3_OFFSET) * 1000 / 90 + 500);
  gpioServo(SERVO_4, (Servo4 + SERVO_4_OFFSET) * 1000 / 90 + 500);
  gpioServo(SERVO_5, (Servo5 + SERVO_5_OFFSET) * 1000 / 90 + 500);

/*
gpioServo(SERVO_5, Servo5 * 1000 / 90 + 500);
sleep(3);
gpioServo(SERVO_5, 45 * 1000 / 90 + 500);
sleep(3);
gpioServo(SERVO_5, 135 * 1000 / 90 + 500);
sleep(3);
digitalWrite(ENABLE_MOTORS, 0); gpioTerminate(); exit(0);//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
*/


  //Calculate XYZ coordinates of motors
  M1X = -BASELENGTH / 2.0;
  M1Y = BASELENGTH / 2.0 * tan(30.0 * PI / 180.0);

  M2X = 0.0;
  M2Y = -(BASELENGTH * cos(30.0 * PI / 180.0) - M1Y);

  M3X = BASELENGTH / 2.0;
  M3Y = BASELENGTH / 2.0 * tan(30.0 * PI / 180.0);

  M2Z = sqrt(pow(INITIAL_LENGTH, 2.0) - pow(M2Y + RADIUS_CORD_MOUNT, 2.0));
  M1Z = M2Z;
  M3Z = M2Z;

  M1X = M1X * StepsPermm;
  M1Y = M1Y * StepsPermm;
  M1Z = M1Z * StepsPermm;

  M2X = M2X * StepsPermm;
  M2Y = M2Y * StepsPermm;
  M2Z = M2Z * StepsPermm;

  M3X = M3X * StepsPermm;
  M3Y = M3Y * StepsPermm;
  M3Z = M3Z * StepsPermm;

  printf("\n\nKoordinates in steps:\n");
  printf("M1X = %lf, M1Y = %lf, M1Z = %lf\n", M1X, M1Y, M1Z);
  printf("M2X = %lf, M2Y = %lf, M2Z = %lf\n", M2X, M2Y, M2Z);
  printf("M3X = %lf, M3Y = %lf, M3Z = %lf\n", M3X, M3Y, M3Z);

  length1 = labs(INITIAL_LENGTH * StepsPermm);
  length2 = labs(INITIAL_LENGTH * StepsPermm);
  length3 = labs(INITIAL_LENGTH * StepsPermm);

  // Simple calculation
  length1 = labs(sqrt(pow(M1X, 2.0) + pow(M1Y, 2.0) + pow(M1Z, 2.0)));
  length2 = labs(sqrt(pow(M2X, 2.0) + pow(M2Y, 2.0) + pow(M2Z, 2.0)));
  length3 = labs(sqrt(pow(M3X, 2.0) + pow(M3Y, 2.0) + pow(M3Z, 2.0)));



  printf("length1 = %lf, length2 = %lf, length3 = %lf\n", length1, length2, length3);
/*
  motor1(1000);
  motor1(-1000);
  sleep(3);
  motor2(1000);
  motor2(-1000);
  sleep(3);
  motor3(1000);
  motor3(-1000);
  sleep(3);
*/

//  MoveToCoordinates(10000, 0, 0, Servo3, Servo4,Servo5);
//  sleep(3);
//  MoveToCoordinates(0, 0, 0, Servo3, Servo4,Servo5);
//  sleep(3);
//  digitalWrite(ENABLE_MOTORS, 0); gpioTerminate(); exit(0);//!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

printf("Moving P1 20\n");
for(i=0;i<20;i++){
  motorP1(1, P_PAUSE);
  usleep(20000);
}
printf("...done!\n");
sleep(2);
printf("Moving P1 -20\n");
for(i=0;i<20;i++){
  motorP1(-1, P_PAUSE);
  usleep(20000);
}
printf("...done!\n");

printf("Moving P2 20\n");
for(i=0;i<20;i++){
  motorP2(1, P_PAUSE);
  usleep(20000);
}
printf("...done!\n");
sleep(2);
printf("Moving P2 -20\n");
for(i=0;i<20;i++){
  motorP2(-1, P_PAUSE);
  usleep(20000);
}
printf("...done!\n");

  while(run){


    if(access("WinchBotCommands.txt", W_OK) != -1) {
      //sleep(1);
      if (!(commandsFile = fopen("WinchBotCommands.txt", "r"))){
        printf("Can't open WinchBotCommands.txt!\n");
        run = 0;
      }
      else{
        char a = 0;
        i = 0;
        commandsLine[0] = '\0';
        while((!(feof(commandsFile))) && a != 13 && i < 1000){
          fread(&a, 1, 1, commandsFile);
          commandsLine[i] = a;
          commandsLine[i+1] = '\0';
          i++;
          //printf("'%c'", a);
        }
        printf("commandsLine='%s'\n", commandsLine);

        //Store current coordinates in file
        if(strstr(commandsLine, "STORE")!=NULL){
          sprintf(TextLine, "echo \"% 7ld, % 7ld, % 7ld, % 7d, % 7d, % 7d,        0;\" >> coordinates.bot", currentX, currentY, currentZ, Servo3, Servo4, Servo5);
          system(TextLine);
        }

        //Store current coordinates in file
        if(strstr(commandsLine, "DELETEALL")!=NULL){
          system("rm coordinates.bot");
        }

        //Go to initial position
        if(strstr(commandsLine, "INIT")!=NULL){
          MoveToCoordinates(0, 0, 0, 90, 90, 90, 0);
        }

        //Replay file
        if(strstr(commandsLine, "REPLAY")!=NULL){
          if((ReplayFile=fopen("coordinates.bot","rb"))!=NULL){
            printf("Replaying coordinates.bot\n");
            //PlotStartTime = time(0);
            //coordinateCount = 0;
            stopPlot = 0;
            coordReadNo = 0;

            while(!(feof(ReplayFile)) && stopPlot == 0){
              fread(&a, 1, 1, ReplayFile);
              printf("%c", a);
              i=0;
              TextLine[0] = '\0';
              while(!(feof(ReplayFile)) && a !=' ' && a != ';' && a != '\"' && a != ',' && a != 10 && a!= 13){
                TextLine[i] = a;
                TextLine[i+1] = '\0';
                i++;
                fread(&a, 1, 1, ReplayFile);
                printf("%c", a);
              }
              if(a == '#'){
                while(!(feof(ReplayFile)) && a != 10 && a!= 13){
                  fread(&a, 1, 1, ReplayFile);
                  printf("%c", a);
                }
              }
              if(a == ',' || a == ';'){//Coordinates found
                coordReadNo++;
                if(coordReadNo == 1){
                  goToX = strtol(TextLine, &pEnd, 10);
                }
                if(coordReadNo == 2){
                  goToY = strtol(TextLine, &pEnd, 10);
                }
                if(coordReadNo == 3){
                  goToZ = strtol(TextLine, &pEnd, 10);
                }
                if(coordReadNo == 4){
                  goToS3 = strtol(TextLine, &pEnd, 10);
                }
                if(coordReadNo == 5){
                  goToS4 = strtol(TextLine, &pEnd, 10);
                }
                if(coordReadNo == 6){
                  goToS5 = strtol(TextLine, &pEnd, 10);
                }
                if(coordReadNo == 7){
...

This file has been truncated, please download it to see its full contents.