/*
11 August 2023, Jonah Sokoloff, Arduino Rotation Program

This is built to work with the Adafruit assembled Motor Shield for Arduino v2
It won't work with v1.x motor shields! Only for the v2's with built in PWM
control

For use with the Adafruit Motor Shield v2
---->   http://www.adafruit.com/products/1438

Essentially same wiring, except with pins 8 and 9 instead of 2 and 3 and without LED:
https://learn.adafruit.com/adafruit-arduino-lesson-6-digital-inputs

Serial commands require a new line character at the end. Simply set serial
monitor to "new line." Serial commands are the following:

<commandNum>f - move forward by commandNum number of steps. ie, "100f" will move forward
              100 steps.
              
<commandNum>b - same as <commandNum>f but backwards

w - move forward by one step

s - move backward by one step

<motorSpeed>v - set speed of motor to motorSpeed.

*/
#include <Wire.h>
#include <Adafruit_MotorShield.h>

int buttonPin1 = 8; //assign manual real button to uno pin 
int buttonPin2 = 9; //assign manual real button to uno pin

int stepCount = 0;
int stepDelay = 10;
int commandDelay = 500;
int motorSpeed = 20;

String command = "";

// Create the motor shield object with the default I2C address
Adafruit_MotorShield AFMS = Adafruit_MotorShield();
// Or, create it with a different I2C address (say for stacking)
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61);

// Connect a stepper motor with 200 steps per revolution (1.8 degree)
// to motor port #2 (M3 and M4)
Adafruit_StepperMotor *myMotor = AFMS.getStepper(200, 2);

void setup() {
  Serial.begin(9600);           // set up Serial library at 9600 bps
  pinMode(buttonPin1, INPUT_PULLUP); // in project box, manual control 
  pinMode(buttonPin2, INPUT_PULLUP); // in project box, manual control

  AFMS.begin();  // create with the default frequency 1.6KHz
  myMotor->setSpeed(motorSpeed);  // eg.. myMotor->setSpeed(10);  is = 10 rpm   
}

void loop() {
  if (digitalRead(buttonPin1) == LOW) {
    myMotor->step(1, FORWARD, MICROSTEP); 
    stepCount++;
    delay(stepDelay);
    Serial.println(stepCount);
  }
 
  if (digitalRead(buttonPin2) == LOW) {
    myMotor->step(1, BACKWARD, MICROSTEP);
    stepCount++;
    delay(stepDelay);
    Serial.println(stepCount);
  }
  
  if (Serial.available() > 0) {
    char newbyte = Serial.read();

    if (newbyte != '\n') {
      command.concat(newbyte);
    }
    
    if (newbyte == '\n')
    {
      int numChars = command.length();
      int commandNum = 0;
      
      for (int i=0; i<numChars; i++) {
        if (isDigit(command.charAt(i)) ) {
          int stepNum = command.charAt(i) - '0';
          commandNum += stepNum * pow(10, numChars - i - 2);
        }
        else {
          if (command[i] == 'b' & commandNum <= 200) {
            myMotor->step(commandNum, BACKWARD, MICROSTEP); //advances by commandNum
            stepCount -= commandNum;
            delay(commandDelay); //probably unnecessary, but might avoid double commands
            Serial.print("B \n");
          }
          if (command[i] == 'f' & commandNum <= 200) {
            myMotor->step(commandNum, FORWARD, MICROSTEP); 
            stepCount += commandNum;
            delay(commandDelay);
            Serial.print("F \n");
          }
          if (command[i] == 'w') {
            myMotor->step(1, FORWARD, MICROSTEP); //advance it one step.
            stepCount--;
            delay(stepDelay);
            Serial.print("W \n");
          }
          if (command[i] == 's') {
            myMotor->step(1, BACKWARD, MICROSTEP);
            stepCount--;
            delay(stepDelay);
            Serial.print("S \n" );
          }
          if (command[i] == 'v') {
            motorSpeed = commandNum;
            myMotor->setSpeed(motorSpeed); 
            delay(commandDelay);
            Serial.print("V \n");
          }
          else {
            command = "";
            break;
          }
        }
      }
    } 
  }
}