Motor Tester

#include <AccelStepper.h>
#include <Servo.h>

//MoMoTestTest
//Revision 1
//Stepper motor tester
//180-degree servo motor tester
//Continuous servo tester
//Scott Mangiacotti
//Tucson, Arizona USA
//September 2018


//Global constants
const int GIVE_BACK = 5;  //number milliseconds delay at end of each scan to ensure not using 100% CPU

const int STEPPER_IN1 = 3;
const int STEPPER_IN2 = 5;
const int STEPPER_IN3 = 6;
const int STEPPER_IN4 = 9;
const int STEPS = 4095;
const int HALF_STEP = 8;

const int SERVO_PIN = 10;
const int CONT_SERVO_PIN = 11;

//Global variables
bool gStepperEnabled = false;
bool gStepperClockwiseTravel = true;
bool gServoEnabled = false;
bool gContServoEnabled = false;

Servo gServo;
Servo gContServo;
AccelStepper gAstepper(HALF_STEP, STEPPER_IN1, STEPPER_IN3, STEPPER_IN2, STEPPER_IN4);


//Run once
void setup()
{

  //Setup static stepper properties
  gAstepper.setMaxSpeed(1000.0);
  gAstepper.setAcceleration(100.0);
  gAstepper.setSpeed(200);

  //Initialize serial port
  Serial.begin(9600);

  //Post app data
  postAppData();
 
}


//Run continuous
void loop()
{


  //Process serial messages
  if (Serial.available() > 0)
  {
    processMessage();
    
  }

  //Run the motors until desired position achieved
  gAstepper.setSpeed(999);
  gAstepper.runSpeedToPosition();

  //Give some time back
  delay(GIVE_BACK);

}


//Read data from serial port and process message
//Format is: XXnnnn
//XX is a value between 1 - 32 and represents the command type or area (for example manual commands to the HOUR servo motor)
//nnnn is a value between 0-999 and represents the value for the target command type
//For example, 01180 is type 02 and value 180. It represents HOUR servo motor move to position 180 degrees
//See documentation for command definitions and value ranges
void processMessage()
{
  int iMessage;
  int iControlCode;
  int iControlValue;

  //Read the data in the serial port buffer
  iMessage = Serial.parseInt();
  Serial.print("Message received: ");
  Serial.println(iMessage);


  //Process the serial port message
  if (iMessage > 0)
  {
    iControlCode = iMessage/1000;
    Serial.println("-----");
    Serial.print("Control Code: ");
    Serial.println(iControlCode);
    
    iControlValue = iMessage % 1000;
    Serial.print("Control Value: ");
    Serial.println(iControlValue);
  }

  //Control and command codes
  if (iControlCode == 10)
  {
    if (iControlValue == 1)
    {
      postAppData();
      
    }
    else if (iControlValue == 2)
    {
      //Enable not necessary for stepper library
      gAstepper.move(1024);
      Serial.println("banana, banana, banana");
      
    }
    else if (iControlValue == 3)
    {
      gServoEnabled = !gServoEnabled;
      
      if (gServoEnabled == true)
      {
        gServo.attach(SERVO_PIN);
        Serial.println("180-degree servo enabled");
        
      }
      else
      {
        gServo.detach();
        Serial.println("180-degree servo disabled");
        
      }
      
    }
    else if (iControlValue == 4)
    {
      gContServoEnabled = !gContServoEnabled;
      
      if (gContServoEnabled == true)
      {
        gContServo.attach(CONT_SERVO_PIN);
        Serial.println("Continuous servo enabled");
        
      }
      else
      {
        gContServo.detach();
        Serial.println("Continuous servo disabled");
        
      }

    }
    else if (iControlValue == 5)
    {
      gStepperClockwiseTravel = !gStepperClockwiseTravel;
      Serial.print("gStepperClockwiseTravel = ");
      Serial.println(gStepperClockwiseTravel);
      
    }
    else
    {
      Serial.print("Invalid Control Value: ");
      Serial.println(iControlValue);
    }
  }


  if (iControlCode == 11)
  {
    //Stepper motor move command
    if (iControlValue >= 0 && iControlValue <= STEPS)
    {
      int banana;
      if (gStepperClockwiseTravel == true)
      {
        banana = iControlValue;
      }
      else
      {
        banana = iControlValue  * -1;
      }
      
      gAstepper.moveTo(banana);

      Serial.print("Stepper motor commanded to position: ");
      Serial.println(banana);
      
    }
    else
    {
      Serial.print("Invalid Control Value: ");
      Serial.println(iControlValue);
    }
  }

  if (iControlCode == 12)
  {
    //180-degree servo move command
    if (iControlValue >= 0 && iControlValue <= 180)
    {
      gServo.write(iControlValue);
      Serial.print("180-degree servo commanded to position: ");
      Serial.println(iControlValue);
      
    }
    else
    {
      Serial.print("Invalid Control Value: ");
      Serial.println(iControlValue);
    }
  }

  if (iControlCode == 13)
  {
    //Continuous servo move speed command
    if (iControlValue >= 0 && iControlValue <= 999)
    {
      gContServo.write(iControlValue);
      
    }
    else
    {
      Serial.print("Invalid Control Value: ");
      Serial.println(iControlValue);
    }
  }

  Serial.println("-----");

}


void postAppData()
{
//MoMoTestTest
//Revision 1
//Stepper motor tester
//180-degree servo motor tester
//Continuous servo tester
//Scott Mangiacotti
//Tucson, Arizona USA
//September 2018

  Serial.println("MoMoTestTest");
  Serial.println("Motor tester for Arduino");
  Serial.println("V1");
  Serial.println("By Scott Mangiacotti");
  Serial.println("Tucson, Arizona USA");
  Serial.println("September 2018");
  
}