//stepper controller for timelapse motion
//for use with two A3967 chips on Sparkfun EasyDriver breakout boards
//Trevor Shannon 2011
//http://trevorshp.com/creations/timelapse_panning.htm
//
//be aware:  this code is poorly commented and might not work!  It
//worked fine for me, but use at your own risk.

#define FULL 1
#define HALF 2 
#define QUARTER 4
#define EIGHTH 8
#define YAW 0
#define PITCH 1
#define FORWARDS 0
#define BACKWARDS 1

const int dir[2] = {2,3};
const int stepp[2] = {4,5};
const int ms1 = 7;
const int ms2 = 8;

float ratio[2] = {10.18, 4.0}; //gear ratios of the two axes, yaw and pitch.
const int revSteps = 200; //number of steps in one full revolution of the motor.  (200 steps = 1.8 deg/step)
const int stepDelay = 2; //time to wait for the rotor to move into position.

boolean goYo = true;

void setup(){
  Serial.begin(9600);
  pinMode(dir[YAW], OUTPUT);
  pinMode(dir[PITCH], OUTPUT);
  pinMode(stepp[YAW], OUTPUT);
  pinMode(stepp[PITCH], OUTPUT);
  pinMode(ms1, OUTPUT);
  pinMode(ms2, OUTPUT);
  
  digitalWrite(dir[YAW], 1);
  digitalWrite(dir[PITCH], 1);
  digitalWrite(stepp[YAW], 0);
  digitalWrite(stepp[PITCH], 0);
  setMode(EIGHTH); //default to eighth step.
  
  
}

void loop(){
  while (goYo){
    move(60, 5000, 0, 0, EIGHTH);
    goYo = false;
  }

}

void move(int Ydeg, float Ytime, int Pdeg, float Ptime, int mode){ //time in seconds
  setMode(mode);
  setDir(Ydeg, YAW);
  setDir(Pdeg, PITCH);
  
  int YtotalSteps = totalSteps(Ydeg, YAW, mode);
  int PtotalSteps = totalSteps(Pdeg, PITCH, mode);
  int YwaitTime = waitTime(YtotalSteps, Ytime);
  int PwaitTime = waitTime(PtotalSteps, Ptime);
  
 Serial.println(YwaitTime);
 Serial.println(PwaitTime);
  
  int Ysteps = 0;
  int Psteps = 0;
  
  float Ytimer = 0;
  float Ptimer = 0;
  
  while (!(Ysteps >= YtotalSteps && Psteps >= PtotalSteps)){
    if (millis()-Ytimer > YwaitTime && Ysteps < YtotalSteps){  
      Ytimer = millis();
      takeStep(YAW);
      Ysteps++;      
    }
      
    if (millis()-Ptimer > PwaitTime && Psteps < PtotalSteps){  
      Ptimer = millis();
      takeStep(PITCH);
      Psteps++;  
    }  
  }
}

void takeStep(int axis){
  digitalWrite(stepp[axis], 1);
  digitalWrite(stepp[axis], 0);
}

void setDir(int deg, int axis){
  digitalWrite(dir[axis], deg<0);
}

int totalSteps(int deg, int axis, int mode){
  if(deg<0){
   deg*= -1;
  } 
  return (revSteps*mode*ratio[axis]*deg)/360.0;
}
  
float waitTime(int totalSteps, float totalTime){
  int wait;
  if (totalSteps == 0 ){
    wait = 0;
    return wait;
  }
  wait = (totalTime*(1000))/totalSteps;
  if (wait < (stepDelay+1)){
    wait = stepDelay+1; //need to set a maximum speed.
  }
  return wait;
}

void setMode(int mode){
  switch(mode){
    case FULL:
      digitalWrite(ms1, 0);
      digitalWrite(ms2, 0);
      break;
    case HALF:
      digitalWrite(ms1, 1);
      digitalWrite(ms2, 0);
      break;
    case QUARTER:
      digitalWrite(ms1, 0);
      digitalWrite(ms2, 1);
      break;
    case EIGHTH:
      digitalWrite(ms1, 1);
      digitalWrite(ms2, 1);
      break;
  }
}