package leJosExercises;

import lejos.nxt.*;
public class ExerciseLejos
{
// wheelCircumference is 113,09
// 1 cm is 6,28
// double pi=3.14
// double circumferece = 12.45
public static void main(String[] args) throws InterruptedException
{
int pwrLeft, pwrRight;
MotorPort mLeft, mRight;
mLeft = MotorPort.C;
mRight = MotorPort.B;
pwrLeft = 75;
pwrRight = 75;
int slowLeft = 57;
// prepare the robot
mLeft.resetTachoCount();
mRight.resetTachoCount();
// math calculations
double pi = 3.14;
double circumference = 17.58;
double circle = (2.0 * pi * 11.5);
int tachoPerCm = (int) (360 / circumference);
// int tachoNinetyDegrees = (int) ((90.0 * circumference) / 360.0);
int tachoNinetyDegrees = (int) (((circle / 4) / circumference));
int quarterCircle = tachoPerCm * (int) (circle / 4);
int cycle;
int tachoPerDegree = quarterCircle/90;
int bigAngle = tachoPerDegree*135;
int smallAngle = tachoPerDegree*90;
// Start the program
LCD.clear();
LCD.drawString("Press any key", 0, 0);
Button.waitForAnyPress();
LCD.clear();
// walk 90 cm
boolean finish = false;
boolean turn = false, reset = false;
byte actions = 0;
while (!finish)
{
if (!turn)
{
if (actions % 2 == 0)
cycle = tachoPerCm * 90;
else
cycle = tachoPerCm * 60;
if ((mLeft.getTachoCount() < cycle)
&& (mRight.getTachoCount() < cycle))
{
mLeft.controlMotor(pwrLeft, 1);
mRight.controlMotor(pwrRight, 1);
}

else
{
mLeft.controlMotor(0, 3);
mRight.controlMotor(0, 3);
turn = true;
actions++;
reset = true;
}
}
if (turn)
{
if (reset)
{
mLeft.resetTachoCount();
mRight.resetTachoCount();
reset = false;
}
if (mLeft.getTachoCount() < quarterCircle)
{
mLeft.controlMotor(pwrLeft, 1);
}
else
{
mLeft.controlMotor(0, 3);
turn = false;
}
}
if (actions == 4)
finish = true;
}
}
}