#include <ozone.h>

MR2X5A myMotor(0);
int norSpeed = 1024;  //200~1024
int fastSpeed = 1024; //200~1024

int speed = 0;

//DeadZone is 50.
// the setup function runs once when you press reset or power the board
void setup() {
	Serial.begin(9600);


	while (!Serial);
	Serial.println("Motor test START");
	Serial.println("Enter the word. w:formwod, s:backword");
	Serial.println("a:turnLeft, d:turnRight, r:brake, t:DZtest, q:Stop");
	Serial.println("-----------------------------------------------------");
}

// the loop function runs over and over again until power down or reset
void loop() {
	if (Serial.available() > 0) {
		int SRR = Serial.read();
		switch (SRR)
		{
		case 119://w
			delay(5);
			forword();
			Serial.println("Forword");
			break;

		case 115://s
			delay(5);
			backword();
			Serial.println("Backword");
			break;

		case 97://a
			turnLeft();
			Serial.println("turnLeft");
			break;

		case 100://d
			turnRight();
			Serial.println("turnRight");
			break;

		case 116://t
		

		case 113://q
			stop();
			Serial.println("Stop");
			break;
		case 'r':
			brake();
			Serial.println("Brake");
			break;
		}//switch
	}//if
}

void forword() {
//  analogWrite(ML1, norSpeed);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, norSpeed);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR, HIGH);
  myMotor.ForwardA(norSpeed);
  myMotor.ForwardB(norSpeed);
}

void backword() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, norSpeed);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, norSpeed);
//  digitalWrite(ENR, HIGH);
  myMotor.BackwardA(norSpeed);
  myMotor.BackwardB(norSpeed);
}

void turnLeft() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, norSpeed);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.ForwardA(norSpeed);
  myMotor.StopB();
}

void turnRight() {
//  analogWrite(ML1, norSpeed);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.StopA();
  myMotor.ForwardB(norSpeed);
}


void stop() {
//  digitalWrite(ML1, LOW);
//  digitalWrite(ML2, LOW);
//  digitalWrite(ENL,HIGH);
//  digitalWrite(MR1, LOW);
//  digitalWrite(MR2, LOW);
//  digitalWrite(ENR,HIGH);
  myMotor.StopA();
  myMotor.StopB();
}

void brake() {
//  digitalWrite(ML1, HIGH);
//  digitalWrite(ML2, HIGH);
//  digitalWrite(ENL,HIGH);
//  digitalWrite(MR1, HIGH);
//  digitalWrite(MR2, HIGH);
//  digitalWrite(ENR,HIGH);
  myMotor.BrakeA();
  myMotor.BrakeB();
}

void situTurnLeft() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, norSpeed);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, norSpeed);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.ForwardA(norSpeed);
  myMotor.BackwardB(norSpeed);
}

void situTurnRight() {
//  analogWrite(ML1, norSpeed);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, norSpeed);
//  digitalWrite(ENR,HIGH);
  myMotor.BackwardA(norSpeed);
  myMotor.ForwardB(norSpeed);
}

void f_forword() {
//  analogWrite(ML1, fastSpeed);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, fastSpeed);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.ForwardA(fastSpeed);
  myMotor.ForwardB(fastSpeed);
}

void f_backword() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, fastSpeed);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, fastSpeed);
//  digitalWrite(ENR,HIGH);
  myMotor.BackwardA(fastSpeed);
  myMotor.BackwardB(fastSpeed);
}

void f_turnLeft() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, fastSpeed);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.ForwardA(fastSpeed);
  myMotor.StopB();
}

void f_turnRight() {
//  analogWrite(ML1, fastSpeed);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.StopA();
  myMotor.ForwardB(fastSpeed);
}

void f_situTurnLeft() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, fastSpeed);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, fastSpeed);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.ForwardA(fastSpeed);
  myMotor.BackwardB(fastSpeed);
}

void f_situTurnRight() {
//  analogWrite(ML1, fastSpeed);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, fastSpeed);
//  digitalWrite(ENR,HIGH);
  myMotor.BackwardA(fastSpeed);
  myMotor.ForwardB(fastSpeed);
}

void backTurnLeft() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, norSpeed);
//  digitalWrite(ENR,HIGH);
  myMotor.BackwardA(norSpeed);
  myMotor.StopB();
}

void backTurnRight() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, norSpeed);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.StopA();
  myMotor.BackwardB(norSpeed);
}

void f_backTurnLeft() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, 0);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, fastSpeed);
//  digitalWrite(ENR,HIGH);
myMotor.BackwardA(fastSpeed);
myMotor.StopB();
}

void f_backTurnRight() {
//  analogWrite(ML1, 0);
//  analogWrite(ML2, fastSpeed);
//  digitalWrite(ENL,HIGH);
//  analogWrite(MR1, 0);
//  analogWrite(MR2, 0);
//  digitalWrite(ENR,HIGH);
  myMotor.StopA();
  myMotor.BackwardB(fastSpeed);
}
