/*
  Motor Test - with AccelStepper
  Run the BigEasy motor driver board
*/

#include <AccelStepper.h>

//Pins
int ms1 = 12;
int st = 5;
int dr = 4;

// Define a stepper and the pins it will use
AccelStepper stepper(AccelStepper::DRIVER, st, dr);

void setup() {
  // all interface pins to output
  pinMode(ms1, OUTPUT);
  
  // set options --
  // half stepping
  digitalWrite(ms1, HIGH);

  // set up stepper
  stepper.setMaxSpeed(3000);
  stepper.setAcceleration(1000);
  stepper.setMinPulseWidth(200);
  
  Serial.begin(115200);
}

void loop() {
  
  char chr, args, step_hi, step_lo, time_hi, time_lo, i;
  word accel;
  static short steps = 0;

  if(Serial.available() > 0) {
    chr = Serial.read();
    
    if (chr == 'G') {
      stepper.move(steps);
      // should be able to interleave running the motor and receiving
      // commands in the future
      while(stepper.distanceToGo() != 0) {
        stepper.run();
      } 
      Serial.write(0x55);
    } else if (chr == 'Y' || chr == 'y') {
      while(Serial.available()==0);
      args = Serial.read();
      while(Serial.available()==0);
      step_hi = Serial.read();
      while(Serial.available()==0);
      step_lo = Serial.read();
      steps = (((unsigned char) step_hi) << 8) | ((unsigned char) step_lo);
      if(chr == 'y') {
        steps = -steps;
      }
    } else if (chr == 'T') {
      while(Serial.available()==0);
      args = Serial.read();
      while(Serial.available()==0);
      time_hi = Serial.read();
      while(Serial.available()==0);
      time_lo = Serial.read();
      accel = (((unsigned char) time_hi) << 8) | ((unsigned char) time_lo);
      stepper.setAcceleration(accel);
    } else {
      while(Serial.available()==0);
      args = Serial.read();
      for (i=0;i<args;++i) {
        while(Serial.available()==0);
        chr = Serial.read();
      }
    }
  }
}