public void run() {
System.out.println("Watching stalls");
while (!Thread.interrupted()) {
index = 0;
for (Motor m : motors) {
newSpeed = m.getActualSpeed();
if (m.isRotating()) {
if (newSpeed < limit) {
if (oldSpeed[index] > newSpeed || wasStalled[index]) {
if (oldSpeed[index] > newSpeed) {
Sound.beep();
}
stalled = true;
m.flt();
wasStalled[index] = false;
}
wasStalled[index] = true;
}
} else {
oldSpeed[index] = 0;
}
oldSpeed[index] = newSpeed;
index++;
// TODO: this will miss a chunk abut once every 20-50 (actually more, this probably takes
// at least 10ms)
try {
Thread.sleep(95);
} catch (InterruptedException e) {
}
}
}
}
public void run() {
motor_left.resetTachoCount();
motor_left.regulateSpeed(true);
Movement.motor_left.smoothAcceleration(true);
int previousCommandCount = -1;
while (true) {
if (Movement.getCommandCount() == previousCommandCount) {
try {
Thread.sleep(10);
} catch (InterruptedException e) {
}
continue;
}
previousCommandCount = Movement.getCommandCount();
setToAngle(ControlCentre.getTargetSteeringAngleLeft());
int new_angle = getToAngle();
if (new_angle < 10) LCD.drawString(" ", 8, 1);
else if (new_angle < 100) LCD.drawString(" ", 9, 1);
LCD.drawString(Integer.toString(new_angle), 7, 1);
LCD.drawString("R", 11, 1);
int cur_angle = getCurrentSteeringAngle();
double delta = new_angle - cur_angle;
final double C = Movement.rotConstant;
double turn_angle = 0;
if (Math.abs(delta) < thresholdAngle / 2.0) {
continue;
} else if (Math.abs(delta) >= thresholdAngle / 2.0 && Math.abs(delta) < thresholdAngle) {
delta = thresholdAngle * delta / Math.abs(delta);
}
setCurrentSteeringAngle((int) (cur_angle + delta) % 360);
if (delta != 0 && Math.abs(delta) < 180) {
turn_angle = C * delta;
} else if (delta >= 180 && delta < 360) {
turn_angle = -C * (360 - delta);
} else if (delta <= -180) {
turn_angle = C * (360 + delta);
} else {
/* No turning needed */
continue;
}
motor_left.rotate((int) Math.round(turn_angle));
}
}
