/**
* Thread that periodicaly updates motors' speed acording to the line position contains the
* regulator loop itself
*/
public void run() {
// Remember the starting time
long tm = System.currentTimeMillis();
listenSocket();
while (Thread.currentThread() == timer) {
// ***************************Versão original********************************
// double position = sensor.GetLinePosition();
//
// if (Math.abs(position) > 1) { // if robot is off line, start turning
// if (position < 0) {
// motorController.setSpeed(0, speed);
// } else {
// motorController.setSpeed(speed, 0);
// }
// sumLinePositions = 0;
// } else {
//
//
// out.println(position + " " + sumLinePositions + " " + previousLinePosition);
// double drive = -1;
// try {
// drive = Double.parseDouble(in.readLine());
// } catch (Exception ex) {
// has_client = false;
// break;
// }
// drive = drive * speed;
//
// double l, r;
//
// if (drive < 0) {
// l = Math.min(-drive, speed);
// r = speed;
// } else {
// l = speed;
// r = Math.min(drive, speed);
// }
// motorController.setSpeed(l, r);
//
// previousLinePosition = position;
// sumLinePositions += position;
// }
// ***********************************************************************
double positionl = sensor.GetLinePosition();
double positionr = 0.5 - positionl;
System.out.println("Left Sensor " + positionl);
System.out.println("Rigth Sensor " + positionr);
out.println(positionl);
out.println(positionr);
if (Math.abs(positionl) == Double.POSITIVE_INFINITY) {
accummulator += 1000;
} else {
accummulator += Math.abs(positionl - positionr);
}
counter += 1;
mae = accummulator / counter;
System.out.println("Mean Absolute Error " + mae);
double l, r;
try {
r = Double.parseDouble(in.readLine());
l = Double.parseDouble(in.readLine());
} catch (Exception ex) {
has_client = false;
break;
}
motorController.setSpeed(l, r);
try {
tm += delay;
Thread.sleep(Math.max(0, tm - System.currentTimeMillis()));
} catch (InterruptedException e) {
break;
}
}
try {
System.out.println("Finishing simulation...");
server.close();
} catch (IOException ex) {
Logger.getLogger(RegulatorServer.class.getName()).log(Level.SEVERE, null, ex);
}
}
@Override
public void run() {
// TODO Auto-generated method stub
while (!Button.ENTER.isDown()) ;
LCD.clear();
if (menuSelection == 1) {
LCD.drawString("Task 1-2 Selected", 0, 0);
} else if (menuSelection == 2) {
LCD.drawString("Task 3 Selected", 0, 0);
}
ultrasonicSensor.wakeUp();
display.wakeUp();
LCD.drawString("Threads Awake", 0, 1);
leftTunedSpeed = leftSpeed;
rightTunedSpeed = rightSpeed;
// switchGains();
LCD.drawString("Running...", 0, 2);
try {
Thread.sleep(50);
while (!Button.ESCAPE.isDown()) {
Thread.sleep(timeStep);
// if(menuSelection == 1)
// {
// if(Math.abs(lineError) > 200)
// {
// counter++;
// if(counter > 10)
// {
// switchGains();
// counter = 0;
// }
// }
// if(state == 0)
// {
// Sound.beep();
// }
// }
// else
// {
// counter++;
// if(counter > 50)
// {
// state = 0;
// switchGains();
// }
// }
// state = 0;
// switchGains();
// state = 0;
// else
// {
// counter = 0;
// }
rangeReading = ultrasonicSensor.getPulseLenght() / 1000;
// LCD.drawString("" + echoValue, 0, 4);
rangePower =
(rangeFinderPID.pid(echoTarget, rangeReading, (double) timeStep / 1000))
/ 40; // blah/40
position = IRArray.calculatePosition();
lineError =
10 * linePID.pid(0, position, (double) timeStep / 1000) / (3 * IR_MAX_ERROR); // 360
readings = IRArray.poleSensor();
if (menuSelection == 1) // Task 1-2: Line and Block Stop
{
if (rangeReading < 600) {
leftTunedSpeed = (int) (setPower - lineError - rangePower);
rightTunedSpeed = (int) (setPower + lineError - rangePower);
} else {
leftTunedSpeed = (int) (setPower - lineError);
rightTunedSpeed = (int) (setPower + lineError);
}
// if(echoValue < 300)
// {
// leftTunedSpeed /= 5;
// rightTunedSpeed /= 5;
// }
if (rangeReading < 400) {
stop = true;
} else {
stop = false;
}
} else if (menuSelection == 2) // Task 3: Platooning and Redline Stop
{
rangeFinderPID.capI(100);
// if(echoValue < echoTarget)
// {
// leftTunedSpeed = (int)(set - curveError - echoError)/5;
// rightTunedSpeed = (int)(set + curveError - echoError)/5;
// }
// else
// {
leftTunedSpeed = (int) (setPower - lineError - rangePower);
rightTunedSpeed = (int) (setPower + lineError - rangePower);
// }
if (readings[0] < 800
&& readings[0] > 400
&& readings[2] < 800
&& readings[2] > 400
&& readings[1] < 800
&& readings[1] > 400) {
stop = true;
}
}
if (stop) {
motors.stopMotors();
LCD.drawString("Stopped", 0, 3);
} else {
if (menuSelection == 1) motors.updateMotors(leftTunedSpeed, rightTunedSpeed);
else {
if (leftTunedSpeed < 0 && rightTunedSpeed < 0) {
motors.leftMotor.motor.backward();
motors.rightMotor.motor.backward();
leftTunedSpeed = setPower - leftTunedSpeed;
rightTunedSpeed = setPower - rightTunedSpeed;
} else {
motors.leftMotor.motor.forward();
motors.rightMotor.motor.forward();
}
motors.updateMotors(leftTunedSpeed, rightTunedSpeed);
}
}
}
} catch (InterruptedException e) {
// TODO Auto-generated catch block
e.printStackTrace();
}
}
