/*************************************
      MICRODEV.GR
      Motor console 
      (C) PANOS PAPAZOGLOU 2023
**************************************/

#include <Servo.h>
#include <Stepper.h>
//Set DC motor pins
int enA=6;
int in1=4;
int in2=5;

//Set servo pins
int pin9g=2;
int pin946=3;

//Declare servo objects
Servo servo9g;
Servo servo946;   

void setup() {
  //Attach servo motors
  servo9g.attach(pin9g);
  servo946.attach(pin946);

  init_dc_motor();
}

void loop() {
  
//Read pot A5 and convert to range 0-180
int pot9g=analogRead(5);
int pos9g=map(pot9g,0,1023,0,180);

//Read pot A4 and convert to range 0-180
int pot946=analogRead(4);
int pos946=map(pot946,0,1023,0,180);

//Update servo 9g
servo9g.write(pos9g);

//Update heavy servo (MG946R)
servo946.write(pos946);

//Control DC motor
int potdc=analogRead(3);
update_dc_motor(potdc);


//Control stepper motor
int potspeed=analogRead(1);
int speed=map(potspeed,0,1023,50,5);
if ((potspeed<400) || (potspeed>600))
{
int potsteps=analogRead(2);
int valsteps=map(potsteps,0,1023,100,2000);

int potspeed=analogRead(1);
int speed=map(potspeed,0,1023,50,5);

Stepper sst = Stepper(valsteps, 8, 10, 9, 11);

sst.setSpeed(speed);
//Set direction
if (potsteps<512) valsteps=valsteps*(-1);
sst.step(valsteps);
}
delay(15);
}

//Rotate DC motor forward/backward
void move_dc(int dir)
{
  if (dir>0)
  {
    digitalWrite(in1,LOW);
    digitalWrite(in2,HIGH);
  }
  else
  {
    digitalWrite(in1,HIGH);
    digitalWrite(in2,LOW);
  }


}

//Initialize DC motor (stop position)
void init_dc_motor()
{
  digitalWrite(in1,LOW);
  digitalWrite(in2,LOW);
}

//Update DC motor operation
void update_dc_motor(int potval)
{
  int speed=0;
  int dir=0;

  //Set left direction for ADC values 0-400
  if ((potval>=0) && (potval<=400))
  {
    speed=map(potval,0,400,0,255);
    dir=-1;
  }
  else

  //Stop motor for pot middle position (ADC 400-600)
  if ((potval>400) && (potval<=600))
  {
    dir=0;
  }
  else

  //Set right direction for ADC values 600-1023
    if ((potval>600) && (potval<=1023))
  {
    speed=map(potval,601,1023,0,255);
    dir=1;
  }

  //Set DC motor speed
  analogWrite(enA,speed);

  //Set motor direction
  if (dir==-1) set_dir(HIGH,LOW);
  else
  if (dir==0) set_dir(LOW,LOW);
  else
  if (dir==1) set_dir(LOW,HIGH);

}

//Change direction with IN1 and IN2 signals
void set_dir(int i1, int i2)
{
  digitalWrite(in1,i1);
  digitalWrite(in2,i2);

}