//Deze code werkt alleen maar in de UnoArduSim

// bibliotheek voor LCD aan te sturen die communiceren via I2C
#include <LiquidCrystal_I2C.h>

// Het aanmaken van een object van type LiquidCrystal_I2C
// Bij het aanmaken van het object wordt het adres en de grootte (hoeveel karakters) van de LCD ook ingetseld
LiquidCrystal_I2C lcd(0x20, 16, 2);

//TK = Tegen de klok
//K = Met de klok

int TK_TK_button = 3;
int K_K_button = 4;
int TK_K_button = 5;
int K_TK_button = 6;

int PWM_M1 = 10;
int PWM_M2 = 11;
int Dir_M1 = 12;
int Dir_M2 = 13;

int Speed_pot = A0; // Deze wordt verbonden met de potentiometer
int Speed_read = 0;
int Speed_write = 0;

void setup()
{
	pinMode(TK_TK_button, INPUT);
	pinMode(K_K_button, INPUT);
	pinMode(TK_K_button, INPUT);
	pinMode(K_TK_button, INPUT);

	pinMode(PWM_M1, OUTPUT);
	pinMode(PWM_M2, OUTPUT);
	pinMode(Dir_M1, OUTPUT);
	pinMode(Dir_M2, OUTPUT);

	// initialiseer de LCD
	lcd.init();
	// Het backlight opzetten
	lcd.backlight();
}

void loop()
{

	// Achteruit
	if (digitalRead(TK_TK_button) == HIGH) {
		Speed_read = analogRead(Speed_pot);
		Speed_write = map(Speed_read, 0, 1024, 0, 255);
		digitalWrite(Dir_M1, HIGH);
		digitalWrite(Dir_M2, HIGH);
		analogWrite(PWM_M1, Speed_write);
		analogWrite(PWM_M2, Speed_write);


    // de instructies die nodig zijn om "achteruit rijden"

		lcd.setCursor(0, 0);
		lcd.print("achteruit");
		lcd.setCursor(0, 1);
		lcd.print("rijden");

	}

	else {

		// Vooruit
		if (digitalRead(K_K_button) == HIGH) {
			Speed_read = analogRead(Speed_pot);
			Speed_write = map(Speed_read, 0, 1024, 0, 255);
			digitalWrite(Dir_M1, LOW);
			digitalWrite(Dir_M2, LOW);

			analogWrite(PWM_M1, Speed_write);
			analogWrite(PWM_M2, Speed_write);

			lcd.setCursor(0, 0);
			lcd.print("voorruit");
			lcd.setCursor(0, 1);
			lcd.print("rijden");
		}

		else {

			//Links
			if (digitalRead(TK_K_button) == HIGH) {
				Speed_read = analogRead(Speed_pot);
				Speed_write = map(Speed_read, 0, 1024, 0, 255);
				digitalWrite(Dir_M1, HIGH);
				digitalWrite(Dir_M2, LOW);
				analogWrite(PWM_M1, Speed_write);
				analogWrite(PWM_M2, Speed_write);

				lcd.setCursor(0, 0);
				lcd.print("links");
				lcd.setCursor(0, 1);
				lcd.print("draaien");
			}

			else {

				// Rechts
				if (digitalRead(K_TK_button) == HIGH) {
					Speed_read = analogRead(Speed_pot);
					Speed_write = map(Speed_read, 0, 1024, 0, 255);
					digitalWrite(Dir_M1, LOW);
					digitalWrite(Dir_M2, HIGH);
					analogWrite(PWM_M1, Speed_write);
					analogWrite(PWM_M2, Speed_write);

					lcd.setCursor(0, 0);
					lcd.print("rechts");
					lcd.setCursor(0, 1);
					lcd.print("draaien");
				}
				else {
					Speed_write = 0;
					digitalWrite(Dir_M1, LOW);
					digitalWrite(Dir_M2, LOW);
					analogWrite(PWM_M1, Speed_write);
					analogWrite(PWM_M2, Speed_write);

         //Als er niet op een drukknop wordt gedrukt moet natuurlijk niets op de LCD display getoond worden
         // Daar zorgt voor volgende instructie voor

					lcd.clear();
				}

			}
		}
	}
}