#include <DRV8835MotorShield.

h>

//**************Constante globale ******************

//sensor ultrasonic
#define TRIGGER_PIN 12
#define ECHO_PIN 11

//valori trimise la motor

#define MAX_DISTANCE 50
#define MOTOR_DIRECTION_FRONT 1
#define MOTOR_DIRECTION_BACK 0
#define LEFT_MOTOR 1
#define RIGHT_MOTOR 0

//valori primite

#define STOP 0
#define STRAIGHT_AHEAD 1
#define STRAIGHT_BACK 2
#define LEFT 3
#define RIGHT 4

//************** Variabile globale ****************

DRV8835MotorShield motors;

unsigned int LAST_COMMAND_DIRECTION =STOP;

unsigned int LAST_COMMAND_SPEED = 0;

boolean LEFT_MOTOR_DIRECTION = 1;
boolean RIGHT_MOTOR_DIRECTION = 1;

unsigned int LEFT_MOTOR_PWM = 0;

unsigned int RIGHT_MOTOR_PWM = 0;

void setup() {

Serial.begin(115200);
}

void loop() {

Read_Serial();
setDirection(LAST_COMMAND_DIRECTION);
control_motors();

void Read_Serial(){

char serialText = ' ';

char directia = 0;
 char viteza = 0;

if (Serial.available()){

serialText = Serial.read();
Serial.println(serialText);
directia = serialText>>4;
viteza = serialText & 0x0F;

//setarea directiei
if(directia>=0x00 && directia<=0x04){
LAST_COMMAND_DIRECTION = directia;
Serial.println(LAST_COMMAND_DIRECTION);
}

//setarea vitezei
if(viteza>=0x00 && viteza <=0x0A){
LAST_COMMAND_SPEED = viteza;
Serial.println(LAST_COMMAND_SPEED);
LAST_COMMAND_SPEED=LAST_COMMAND_SPEED*10;
}
}

void setDirection(unsigned int dir){

switch (dir){
case STRAIGHT_AHEAD: setMotorDirection(LEFT_MOTOR, MOTOR_DIRECTION_FRONT );
setMotorDirection(RIGHT_MOTOR, MOTOR_DIRECTION_FRONT );
setMotorPWM(LEFT_MOTOR,LAST_COMMAND_SPEED );
setMotorPWM(RIGHT_MOTOR,LAST_COMMAND_SPEED );
break;

case STRAIGHT_BACK: setMotorDirection(LEFT_MOTOR, MOTOR_DIRECTION_BACK );

setMotorDirection(RIGHT_MOTOR, MOTOR_DIRECTION_BACK );
setMotorPWM(LEFT_MOTOR,LAST_COMMAND_SPEED );
setMotorPWM(RIGHT_MOTOR,LAST_COMMAND_SPEED );
break;

case LEFT: setMotorDirection(LEFT_MOTOR, MOTOR_DIRECTION_BACK );

setMotorDirection(RIGHT_MOTOR, MOTOR_DIRECTION_FRONT );
setMotorPWM(LEFT_MOTOR,LAST_COMMAND_SPEED );
setMotorPWM(RIGHT_MOTOR,LAST_COMMAND_SPEED/2 );
break;

case RIGHT: setMotorDirection(LEFT_MOTOR, MOTOR_DIRECTION_FRONT );

setMotorDirection(RIGHT_MOTOR, MOTOR_DIRECTION_BACK );
setMotorPWM(LEFT_MOTOR,LAST_COMMAND_SPEED/2 );
setMotorPWM(RIGHT_MOTOR,LAST_COMMAND_SPEED );
break;
case STOP: setMotorDirection(LEFT_MOTOR, MOTOR_DIRECTION_FRONT );
setMotorDirection(RIGHT_MOTOR, MOTOR_DIRECTION_BACK );
setMotorPWM(LEFT_MOTOR,0);
setMotorPWM(RIGHT_MOTOR,0);
break;
}
}
void setMotorDirection(boolean motor , boolean dir){

if(motor==LEFT_MOTOR){
LEFT_MOTOR_DIRECTION = dir;
}
else{
RIGHT_MOTOR_DIRECTION = dir;
}
}

void setMotorPWM(boolean motor , unsigned int pwm){

if(motor==LEFT_MOTOR){
LEFT_MOTOR_PWM = pwm;

}
else{
RIGHT_MOTOR_PWM = pwm;
}

void control_motors(){

int sp1;
int sp2;
if(LEFT_MOTOR_DIRECTION == MOTOR_DIRECTION_FRONT){
sp1= map(LEFT_MOTOR_PWM,0 ,100,0,400);
}
if(RIGHT_MOTOR_DIRECTION == MOTOR_DIRECTION_FRONT){
sp2= map(RIGHT_MOTOR_PWM,0 ,100,0,400);
}
if(LEFT_MOTOR_DIRECTION == MOTOR_DIRECTION_BACK){
sp1 = map(LEFT_MOTOR_PWM,0 ,100,0,-400);
}
if(RIGHT_MOTOR_DIRECTION == MOTOR_DIRECTION_BACK){
sp2 = map(RIGHT_MOTOR_PWM,0 ,100,0,-400);
}

motors.setM1Speed(sp1);
motors.setM2Speed(sp2);
}