#include <Servo.

h>
#include <Keypad.h>
#include <Ultrasonic.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h>

//servo
const int scrap_pin = 41; //
Servo scrap;

//ultrasonic
const int trig_pin_top = 50;
const int echo_pin_top = 48;

const int trig_pin_right = 46;

const int echo_pin_right = 44;

const int trig_pin_left = 38;

const int echo_pin_left = 36;

Ultrasonic ultrasonic_top(trig_pin_top,echo_pin_top);
Ultrasonic ultrasonic_right(trig_pin_right,echo_pin_right);
Ultrasonic ultrasonic_left(trig_pin_left,echo_pin_left);

//keypad
const byte rows = 4; //four rows
const byte cols = 3; //three columns
char keys[rows][cols] = {
{'1','2','3'},
{'4','5','6'},
{'7','8','9'},
{'#','0','*'}
};
byte rowPins[rows] = {22, 24, 26, 28}; //connect to the row pinouts of the keypad
byte colPins[cols] = {31, 33, 35}; //connect to the column pinouts of the keypad
Keypad keypad = Keypad( makeKeymap(keys), rowPins, colPins, rows, cols );

// Parameters
const int drive_distance = 70; // cm
const int turn_distance = 23.5; // 30cm * pi /4
const int left_motor_speed = 200; // 0-255
const int right_motor_speed = 200; // 0-255
const int motor_offset = 5; // Diff. when driving straight, when the speed is
different
const int wheel_d = 103; // Wheel diameter (mm)
const float wheel_c = PI * wheel_d; // Wheel circumference (mm) = 323.5
const int counts_per_rev = 12592.8; // (36 holes + 36 black) * (174.9:1 gearbox)
* (2 falling/rising edges) = 25185.6

// Pins
const int encoder_leftA_pin = 18; // Motor A
const int encoder_rightA_pin = 2; // Motor B

const int left_motor_pin1 = 8;

const int left_motor_pin2 = 9;
const int left_pwm_pin = 10;
const int right_motor_pin1 = 5;
const int right_motor_pin2 = 6;
const int right_pwm_pin = 7;

const int pump = 40;

const int brush = 42;

// Globals
char encoder_leftA, encoder_leftB;
char encoder_rightA, encoder_rightB;

unsigned long enc_l = 0;

unsigned long enc_r = 0;

char key;
String tileLength, startDirection, operation;
float val;
int c=1;
boolean flag = false;

Adafruit_PCD8544 display = Adafruit_PCD8544(7, 6, 5, 4, 3);

const int lcd_light_pin = 11;

void setup() {
//servo pin
scrap.attach(scrap_pin);

pinMode(lcd_light_pin,OUTPUT);
digitalWrite(lcd_light_pin,LOW);

// Set up pins
pinMode(encoder_leftA_pin, INPUT);
pinMode(encoder_rightA_pin, INPUT);

pinMode(right_pwm_pin, OUTPUT);
pinMode(right_motor_pin1, OUTPUT);
pinMode(right_motor_pin2, OUTPUT);

pinMode(left_pwm_pin, OUTPUT);
pinMode(left_motor_pin1, OUTPUT);
pinMode(left_motor_pin2, OUTPUT);

pinMode(pump, OUTPUT);
pinMode(brush, OUTPUT);

display.begin();
display.setContrast(50);

digitalWrite(brush,LOW);
digitalWrite(pump,LOW);
scrap.write(90);

//length
while(1){
if (flag){
 break;
}else {
enterLength();
}
}

//direction left or right

while(1){
if (!flag){
break;
}else {
enterDirection();
}
}

//roba or brush
while(1){
if (flag){
break;
}else {
robaOrBrush();
}
}

// text display tests

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);

display.println("Length:");
display.print(" ");
display.println(val);

display.println("Start at:");
display.print(" ");
display.println(startDirection);

display.println("operation");
display.print(" ");
display.println(operation);

display.display();
delay(2000);

// Set up interrupts
attachInterrupt(digitalPinToInterrupt(encoder_leftA_pin), countLeft, CHANGE);
attachInterrupt(digitalPinToInterrupt(encoder_rightA_pin), countRight, CHANGE);

// Drive straight
delay(1000);

void loop() {

if (startDirection == "LEFT"){
driveTillWall();
turnRight();
 driveHorizontal();
turnRight();

driveTillWall();
turnLeft();
driveHorizontal();
turnLeft();

}else if (startDirection == "RIGHT"){

driveTillWall();
turnLeft();
driveHorizontal();
turnLeft();

driveTillWall();
turnRight();
driveHorizontal();
turnRight();
}

void driveStraight(float dist, int l_m_speed, int r_m_speed, char dir) {

char d = dir;
unsigned long num_ticks_l;
unsigned long num_ticks_r;

// Set initial motor speed

int speed_left = l_m_speed;
int speed_right = r_m_speed;

// Used to determine which way to turn to adjust

unsigned long diff_l;
unsigned long diff_r;

// Reset encoder counts

enc_l = 0;
enc_r = 0;

// Remember previous encoder counts

unsigned long enc_l_prev = enc_l;
unsigned long enc_r_prev = enc_r;

// Calculate target number of ticks

float num_rev = (dist * 10) / wheel_c; // Convert to mm
unsigned long target_count = num_rev * counts_per_rev;

// Drive until one of the encoders reaches desired count

while ( (enc_l < target_count) && (enc_r < target_count) ) {

// Sample number of encoder ticks

num_ticks_l = enc_l;
num_ticks_r = enc_r;

// Drive
drive(speed_left, speed_right, d);
// // Number of ticks counted since last time
diff_l = num_ticks_l - enc_l_prev;
diff_r = num_ticks_r - enc_r_prev;

// Store current tick counter for next time

enc_l_prev = num_ticks_l;
enc_r_prev = num_ticks_r;

// If left is faster, slow it down and speed up right

if ( diff_l > diff_r ) {
speed_left -= motor_offset;
speed_right += motor_offset;

// If right is faster, slow it down and speed up left

if ( diff_l < diff_r ) {
speed_left += motor_offset;
speed_right -= motor_offset;

// Brief pause to let motors respond

delay(20);
}

// Brake
brake();
}

void drive(int power_a, int power_b, char direc) {

// Constrain power to between -255 and 255

power_a = constrain(power_a, -255, 255);
power_b = constrain(power_b, -255, 255);

if ( direc == 'l' ) {
digitalWrite(right_motor_pin1, HIGH);
digitalWrite(right_motor_pin2, LOW);
digitalWrite(left_motor_pin1, LOW);
digitalWrite(left_motor_pin2, HIGH);

} else if ( direc == 'r' ){

digitalWrite(right_motor_pin1, LOW);
digitalWrite(right_motor_pin2, HIGH);
digitalWrite(left_motor_pin1, HIGH);
digitalWrite(left_motor_pin2, LOW);

}else {
digitalWrite(right_motor_pin1, HIGH);
digitalWrite(right_motor_pin2, LOW);
digitalWrite(left_motor_pin1, HIGH);
digitalWrite(left_motor_pin2, LOW);

}
 // Set speed
analogWrite(left_pwm_pin, abs(power_a));
analogWrite(right_pwm_pin, abs(power_b));
}

void brake() {
digitalWrite(right_motor_pin1, LOW);
digitalWrite(right_motor_pin2, LOW);
digitalWrite(left_motor_pin1, LOW);
digitalWrite(left_motor_pin2, LOW);
analogWrite(right_pwm_pin, 0);
analogWrite(left_pwm_pin, 0);
}

void countLeft() {
enc_l++;

void countRight() {
enc_r++;

void enterLength(){

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Distance?");
display.println("#:to continue");
display.display();
delay(2000);

//keypad
while(1){
key = keypad.getKey();
if (key){
if (key!='#'){
tileLength += key;
display.print(key);
display.display();
}
else break;
}
}
display.println();
display.println("Save?");
display.println("#:yes, *:no");
display.display();

while(1){
key = keypad.getKey();
if (key){
if (key == '#'){
flag = true;
break;
 }
else if (key == '.'){
flag = false;
break;
}
}
}

val = tileLength.toFloat();

void enterDirection(){

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Direction?");
display.println("1:left");
display.println("2:right");
display.display();
delay(2000);

while(1){
key = keypad.getKey();
if (key){
if (key == '1' || key == '2'){
if (key == '1')startDirection = "LEFT";
else if(key == '2') startDirection = "RIGHT";
flag = false;
break;
}
}
}

void robaOrBrush(){

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Select:");
display.println("1:roba");
display.println("2:brush");
display.display();
delay(2000);

while(1){
key = keypad.getKey();
if (key){
if (key == '1' || key == '2'){
if (key == '1') operation = "Roba";
else if(key == '2') operation = "Brush";
flag = true;
 break;
}
}
}

void turnRight(){

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Turning?");
display.println("Right");
display.display();
delay(2000);

if (ultrasonic_right.read() > (val+25)){

driveStraight(turn_distance, left_motor_speed, right_motor_speed, 'r');//turn
right
delay(2000);
}else finish();

void turnLeft(){

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Turning?");
display.println("Left");
display.display();
delay(2000);

if (ultrasonic_left.read() > (val+25)){

driveStraight(turn_distance, left_motor_speed, right_motor_speed, 'l');
delay(2000);
}else finish();

void driveHorizontal(){

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Driving?");
display.println("Horezontal");
display.display();
delay(2000);

driveStraight(val, left_motor_speed, right_motor_speed, 's');

delay(2000);
}

void driveTillWall(){

display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("Moving?");
display.println("Forward");
display.display();
delay(2000);

if (operation == "Roba"){
digitalWrite(pump,HIGH);
scrap.write(45);
digitalWrite(brush,LOW);
}else if (operation == "Brush"){
digitalWrite(brush,HIGH);
digitalWrite(pump,LOW);
scrap.write(90);
}

while(ultrasonic_top.read() > 25){

driveStraight(drive_distance, left_motor_speed, right_motor_speed, 's');//turn
right
delay(2000);

if (operation == "Roba"){
digitalWrite(pump,LOW);
scrap.write(90);
digitalWrite(brush,LOW);
}else if (operation == "Brush"){
digitalWrite(brush,LOW);
digitalWrite(pump,LOW);
scrap.write(90);
}

void finish(){
display.clearDisplay();
display.setTextSize(1);
display.setTextColor(BLACK);
display.setCursor(0,0);
display.println("FINISHED");
display.display();
delay(2000);

while(1);
}