#include <AccelStepper.h>

const int dir_pino = 2;
const int step_pino = 3;

#define drive_motor 1
AccelStepper motor_1(drive_motor, step_pino, dir_pino);
#define  SIX 13
#define Red 11
#define Green 10
#define Blue 9

//Variavel de controle da A tivacao da ferramenta
bool Ativa_Ferramenta = LOW;

void setup() 
{
  pinMode(SIX, INPUT_PULLUP);
  pinMode(Red, OUTPUT);
  pinMode(Green, OUTPUT);
  pinMode(Blue, OUTPUT);
 motor_1.setMaxSpeed(1000);
 //Taxa de Aceleração-Passos por segundos ao quadrado
 motor_1.setAcceleration(50);
 Serial.begin(9600);
 Serial.println("Iniciando Homing");
 Homing(); // chama funcao toda vez que arduino é reiniciado
}

void loop() 
{
float voltas = motor_1.currentPosition() /200;
float posicao = voltas * 4;

if(motor_1.distanceToGo() !=0)
{
  Serial.print("Posição: ");
  Serial.println(posicao);
   Serial.print("mm");
  Serial.print("- Voltas: ");
  Serial.println(voltas);
}
char letra = Serial.read();
if(letra == 'A' || letra == 'a')
{
  motor_1.moveTo(1000);
  Ativa_Ferramenta = !Ativa_Ferramenta;
}
if(letra == 'B' || letra == 'b')
{
Serial.println("Posição 1");
  motor_1.moveTo(1000);
  Ativa_Ferramenta = !Ativa_Ferramenta;
}
if(letra == 'C' || letra == 'c')
{
Serial.println("Posição 2");
  motor_1.moveTo(1000);
   Ativa_Ferramenta = !Ativa_Ferramenta;
}
if(letra == 'D' || letra == 'd')
{
Serial.println("Posição 3");
  motor_1.moveTo(3000);
     Ativa_Ferramenta = !Ativa_Ferramenta;
}
 if(motor_1.distanceToGo() == 0 &&  Ativa_Ferramenta == HIGH)
 {
  Serial.println("Ferramenta iniciada");
  Ferramenta();
 
 }
  motor_1.run();
}
void Homing()
{
  // Seta o motor em sentido NTI-HOrio e prepara para que ele rode na velocidade
  motor_1.setSpeed(-500);
  // Enquanto o inivio de curso nao estiver acionado
  while(digitalRead(SIX) == HIGH)
  {
  motor_1.runSpeed(); // Roda o motor
  }
  //Zera velocidade
  motor_1.setSpeed(0);
  //Zera posicao atul do motor de passo
  motor_1.setCurrentPosition(0); //Gustavo
  Serial.println("Homing Finalizado!");
   Serial.println("Selecione uma posição");
}

void Ferramenta()
{//vermelho
 analogWrite(Red, 255);
 analogWrite(Green, 0); 
 analogWrite(Blue, 0); 
 delay(500); 
 //verde
 analogWrite(Red, 0);
 analogWrite(Green, 255); 
 analogWrite(Blue, 0); 
 delay(500);
 //blue
 analogWrite(Red, 0);
 analogWrite(Green, 0); 
 analogWrite(Blue, 255); 
 delay(500);
 //cigano
 analogWrite(Red, 255);
 analogWrite(Green, 0); 
 analogWrite(Blue, 255); 
 delay(500);
 // roxo
 analogWrite(Red, 0);
 analogWrite(Green, 255); 
 analogWrite(Blue, 255); 
 delay(500);
 //branco
 analogWrite(Red, 255);
 analogWrite(Green, 255); 
 analogWrite(Blue, 255); 
 delay(500);
 analogWrite(Red, 0);
 analogWrite(Green, 0); 
 analogWrite(Blue, 0); 
 Serial.println("Ferramenta Concluida");
 Ativa_Ferramenta = !Ativa_Ferramenta;
 Homing();// TODA VEZ QUE A FERRAMENTA É CONCLUIDA
}