/*
	BEE BOT 2 PROJECT

 Get direction from buttons pressed (UP DOWN LEFT RIGHT), store them
 into an array of 10 values and start movimg when SELECT is pressed
 
 We're using a LCD 16x02 SHIELD to get buttons (swiches) and show info

 When button SELECT is pressed (anytime) the program goes to the
 movement section and will give motors the movement based on the
 directions pressed.
 Finally program star again asking for directions
 */

#include <LiquidCrystal_I2C.h>
#include <AccelStepper.h>

#define btnNONE 0
#define btnUP 1
#define btnDOWN 2
#define btnLEFT 3
#define btnSELECT 4
#define btnRIGHT 5
#define btnUNKNOWN 6

const int steps_turn = 512; // Number of steps to make 1 complete turn of the 28BYJ48 stepper motor

int analogPin = 0;
int value0 = 0;		// variables to detect button pressing
int value1 = 0;
int direction[11] = {0,0,0,0,0,0,0,0,0,0};	// vector diretion to store 10 buttons

// Define some steppers and the pins the will use
AccelStepper stepper1; // Defaults to AccelStepper::FULL4WIRE (4 pins) on 2, 3, 4, 5
AccelStepper stepper2(AccelStepper::FULL4WIRE, 6, 7, 8, 9);

// Initialize LCD I2C
LiquidCrystal_I2C lcd (32, 16,2);

// define readkeypad function as an integer read from A0 with diferent values according to diferent resistance read (KEYPAD)
int readkeypad(){
      int adc_key_in = analogRead(analogPin); //
      int ret = btnUNKNOWN;

      if (adc_key_in < 50) ret = btnRIGHT;
      if ( (adc_key_in > 1000)) ret = btnNONE;
      if ( (adc_key_in > 100) && (adc_key_in < 190) ) ret = btnUP;
      if ( (adc_key_in > 250) && (adc_key_in < 400) ) ret = btnDOWN;
      if ( (adc_key_in > 450) && (adc_key_in < 600) ) ret = btnLEFT;
      if ( (adc_key_in > 700) && (adc_key_in < 800) ) ret = btnSELECT;

      return ret;
}


// Just for ease of displaying
const char *button[7] ={"None ", "Up ", "Down ", "Left ", "Select ", "Right ", "??????"};

const int waittime = 1000;   //espera entre fases

void setup()
{  
   
  
  	Serial.begin(9600);
  
    lcd.init();
  	lcd.backlight();
  	lcd.setCursor(0,0);
    lcd.print("STARTING...");
  
  	delay(700);
	lcd.clear();

    stepper1.setMaxSpeed(200.0);
    stepper1.setAcceleration(100.0);
    // stepper1.moveTo(24);
    
    stepper2.setMaxSpeed(200.0);
    stepper2.setAcceleration(100.0);
   // stepper2.moveTo(1000000);
    
}
 
void loop()
{
  // GET DIRECTION
  lcd.print("Enter directions");
  lcd.setCursor(0, 1);
  Serial.println("ENTER DIRECTION [max 10 movements]");
  
  int i = 0;
  value0=readkeypad();
  delay(300);
  value1=readkeypad();
    
  while (value1 != 4 & i<10){ //while not SELECT = GO & less than 10 directions

    if (value0 != value1){	//check if buton pressed from nothing
      if (value0 == 0){		//means a button has been pressed
        Serial.print("Direction num. ");
        Serial.print(i);
        
        direction[i]=value1;
        Serial.print(" is ");
        Serial.println(direction[i]);
        lcd.print(direction[i]);
        i = i+1;
      }
    }
    delay(100);
    value0 = value1;
    value1 = readkeypad();
  }
  
  Serial.println("Direction Acquired");
  Serial.print("DIRECTIONS ARE :");
  for (int j=0; j<10; j++){
    Serial.print(direction[j]); 
  }
  Serial.println();
  
  // START MOVEMENT

  int Pote= analogRead(A1);  // LEEMOS EL POT. EN A1 PARA DETERMINAR VELOCIDAD
  int speed = map(Pote,0,1023,0,255);

  Serial.print("START MOVEMENT...");
  lcd.clear();
  lcd.print("START MOVEMENT...");
  delay(500);
  Serial.print("Direction is : ");
  for (int j=0; j<10; j++){
  	Serial.print(direction[j]);
  }
  Serial.println();
  
  delay(500);
  
  i=0;

  while (i < 10){
    switch(direction[i]){
	  case 0:	// don't move
        Serial.println("NOTHING");
      	break;      
      case 1:	// UP move forward 2 motors
      	moveForward(steps_turn);
        Serial.println("FORWARD");
        lcd.clear();
 		lcd.print("FORWARD");
        delay(waittime);
   		break;
      case 2:	// DOWN move backward 2 motors
      	moveBackward(pinMotorA, speed);
   		moveBackward(pinMotorB, speed);
        Serial.println("BACKWARD");
        lcd.clear();
 		lcd.print("BACKWARD");
        delay(waittime);
      	break;
      case 3:	// LEFT move motor A forward & motor B backeard
        moveForward(pinMotorA, speed);
    	moveBackward(pinMotorB, speed);
        Serial.println("LEFT");
        lcd.clear();
 		lcd.print("LEFT");
      	delay(waittime);
   		break;
      case 5:	// LEFT move motor A forward & motor B backeard
        moveBackward(pinMotorA, speed);
    	moveForward(pinMotorB, speed);
        Serial.println("RIGHT");
        lcd.clear();
 		lcd.print("RIGHT");
      	delay(waittime);
   		break;
      default:
      	break;
    }
    i = i+1;
    fullStop(pinMotorA);
    fullStop(pinMotorB);
    delay(300);		// LET MOTORS STOP AFTER EVERY INSTRUCTION
  }
  lcd.clear();
  lcd.print("END OF MOVEMENT");
  delay(2000);
  lcd.clear();
  
}


void moveForward(const int steps_turn)
{
    stepper1.moveTo(512);
    stepper2.moveTo(512);
    stepper1.run();
    stepper2.run();
}
 
void moveBackward(const int pinMotor[3], int speed)
{
   digitalWrite(pinMotor[1], LOW);
   digitalWrite(pinMotor[2], HIGH);
 
   analogWrite(pinMotor[0], speed);
}
 
void fullStop(const int pinMotor[3])
{
   digitalWrite(pinMotor[1], LOW);
   digitalWrite(pinMotor[2], LOW);
 
   analogWrite(pinMotor[0], 0);
}