// run method (required for Thread)
public void run() {
long updateStart, updateEnd;
double distL, distR, deltaD, deltaT, dX, dY;
while (true) {
updateStart = System.currentTimeMillis();
// put (some of) your odometer code here
nowTachoL = leftMotor.getTachoCount(); // get tacho counts
nowTachoR = rightMotor.getTachoCount();
distL = Math.PI * WR * (nowTachoL - lastTachoL) / 180; // compute
// wheel
distR = Math.PI * WR * (nowTachoR - lastTachoR) / 180; // displacements
lastTachoL = nowTachoL; // save tacho counts for next iteration
lastTachoR = nowTachoR;
deltaD = 0.5 * (distL + distR); // compute vehicle displacement
deltaT = (distL - distR) / WB; // compute change in heading
synchronized (lock) {
// don't use the variables x, y, or theta anywhere but here!
theta += deltaT;
if (theta >= Math.PI * 2) {
theta = theta - Math.PI * 2;
}
dX = deltaD * Math.sin(theta); // compute X component of
// displacement
dY = deltaD * Math.cos(theta); // compute Y component of
// displacement
x = x + dX;
y = y + dY;
thetaDeg = theta * 180 / Math.PI;
}
// this ensures that the odometer only runs once every period
updateEnd = System.currentTimeMillis();
if (updateEnd - updateStart < ODOMETER_PERIOD) {
try {
Thread.sleep(ODOMETER_PERIOD - (updateEnd - updateStart));
} catch (InterruptedException e) {
// there is nothing to be done here because it is not
// expected that the odometer will be interrupted by
// another thread
}
}
}
}
public static void frente(int distancia) {
motorA.synchronizeWith(new RegulatedMotor[] {motorB});
motorA.startSynchronization();
motorA.rotate(odometria(distancia), true);
motorB.rotate(odometria(distancia), true);
motorC.stop();
motorA.endSynchronization();
motorA.waitComplete();
motorB.waitComplete();
}
// run method (required for Thread)
public void run() {
long updateStart, updateEnd;
// reset the tacho count, and set the initial tacho counts to the last_tacho_X variables
leftMotor.resetTachoCount();
rightMotor.resetTachoCount();
last_tacho_L = leftMotor.getTachoCount();
last_tacho_R = rightMotor.getTachoCount();
while (true) {
updateStart = System.currentTimeMillis();
int current_tacho_L = leftMotor.getTachoCount();
int current_tacho_R = rightMotor.getTachoCount();
// calculating the distance each wheel has travelled
double distanceL = Math.PI * wheelRadius * (current_tacho_L - last_tacho_L) / 180;
double distanceR = Math.PI * wheelRadius * (current_tacho_R - last_tacho_R) / 180;
// updating the last tacho counts
last_tacho_L = current_tacho_L;
last_tacho_R = current_tacho_R;
// calculating the change in angle and change in absolute displacement
double deltaD = 0.5 * (distanceL + distanceR);
double deltaT = (distanceR - distanceL) / axleLength;
synchronized (lock) {
// updating the locations of the variables
theta += deltaT;
// maintain the bounds of theta
if (theta < 0) theta += 2 * Math.PI;
if (theta > 2 * Math.PI) theta -= 2 * Math.PI;
setDistanceTravelled(getDistanceTravelled() + deltaD);
setX(x + deltaD * Math.cos(theta));
setY(y + deltaD * Math.sin(theta));
}
// this ensures that the odometer only runs once every period
updateEnd = System.currentTimeMillis();
if (updateEnd - updateStart < ODOMETER_PERIOD) {
try {
Thread.sleep(ODOMETER_PERIOD - (updateEnd - updateStart));
} catch (InterruptedException e) {
// there is nothing to be done here because it is not
// expected that the odometer will be interrupted by
// another thread
}
}
}
}
public static void main(String[] args) {
motorA = new EV3LargeRegulatedMotor(MotorPort.A);
motorB = new EV3LargeRegulatedMotor(MotorPort.B);
motorC = new EV3LargeRegulatedMotor(MotorPort.C);
// motorA.setSpeed(200);//UMB
// motorB.setSpeed(200.16f);//UMB
motorA.setAcceleration(50);
motorB.setAcceleration(50);
motorA.setSpeed(200);
motorB.setSpeed(201.5f);
frente(100);
parar();
resolucao_primeira();
levantar_garra();
motorA.setAcceleration(20);
motorB.setAcceleration(20);
frente(20);
baixar_garra();
frente(20);
levantar_garra();
frente(24);
motorA.setAcceleration(50);
motorB.setAcceleration(50);
frente(-10);
baixar_garra();
frente(-54);
/*
primeira_Prova();
parar();
resolucao_primeira();
segunda_Prova();
*/
}
public void avoidObstacle(double pX, double pY) {
rightMotor.stop();
leftMotor.stop();
leftUltraSoundMotor.rotate(50, true);
leftMotor.rotate(NavigatorUtility.convertAngle(wheelRadius, axleLength, 90), true);
rightMotor.rotate(-NavigatorUtility.convertAngle(wheelRadius, axleLength, 90), false);
double currentX;
double currentY;
do {
currentX = odometer.getX();
currentY = odometer.getY();
wallFollowerController.processUSData(ultraSonicPoller.getLeftUltraSoundSensorDistance());
} while (Math.abs(
NavigatorUtility.calculateAngleError(pX - currentX, pY - currentY, odometer.getTheta())
* 180
/ Math.PI)
> wallFollowingAngleError);
leftUltraSoundMotor.rotate(-50, false);
}
public void run() {
leftMotor.setAcceleration(2000);
rightMotor.setAcceleration(2000);
distMid = getFilteredData();
sensorMotor.rotate(45);
Delay.msDelay(500);
distLeft = getFilteredData();
sensorMotor.rotate(-90);
Delay.msDelay(500);
distRight = getFilteredData();
sensorMotor.rotate(45);
if (distLeft > distMid && distLeft > distRight) {
select = 0;
} else if (distRight >= distMid && distRight >= distLeft) {
select = 2; // turn right
}
if (select == 0) {
leftMotor.setSpeed(ROTATE_SPEED);
rightMotor.setSpeed(ROTATE_SPEED);
sensorMotor.setAcceleration(1000);
angleSave = odometer.getAng();
leftMotor.rotate(-convertAngle(2.2, 15, 89.8), true);
rightMotor.rotate(convertAngle(2.2, 15, 89.8), false);
sensorMotor.rotate(-85);
while (safe == false || collide() == false) {
if (getFilteredData() > 100 && filterControl < FILTER_OUT) {
// bad value, do not set the distance var, however do increment the filter value
filterControl++;
} else if (getFilteredData() > 100) {
// set distance to this.distance
distance = 100;
} else {
// distance went below 100, therefore reset everything.
filterControl = 0;
distance = getFilteredData();
}
perror = distance - bandCenter;
amperror = perror * proportion;
if (distance > 60) {
// if distance >80, the robot is about to do a U-turn
leftMotor.setSpeed(maxSpeed);
rightMotor.setSpeed(motorStraight);
// right wheel will speed up in order to turn left
leftMotor.forward();
rightMotor.forward();
}
/* else if(distance<8){
// if distance<8, the robot is going to hit the wall
leftMotor.setSpeed(motorStraight+50);
rightMotor.setSpeed(motorStraight+50);
leftMotor.forward();
rightMotor.backward();
// right motor rolls backwards to respond.
}
*/
else {
// general proportion error correction
rightMotor.setSpeed(motorStraight);
if ((motorStraight + amperror) > maxSpeed) {
leftMotor.setSpeed(maxSpeed);
}
// left wheel remains constant speed
else {
leftMotor.setSpeed((int) (motorStraight + amperror));
}
// right wheel will continuously correcting its speed according to errors.
leftMotor.forward();
rightMotor.forward();
}
if (CloseTo(rightDegreeHelper(angleSave, odometer.getAng()), -90)) {
sensorMotor.rotate(85);
safe = true;
}
}
}
if (select == 2) {
leftMotor.setSpeed(ROTATE_SPEED);
rightMotor.setSpeed(ROTATE_SPEED);
sensorMotor.setAcceleration(1000);
angleSave = odometer.getAng();
leftMotor.rotate(convertAngle(2.2, 15, 89.8), true);
rightMotor.rotate(-convertAngle(2.2, 15, 89.8), false);
sensorMotor.rotate(85);
while (safe == false || collide() == false) {
if (getFilteredData() > 100 && filterControl < FILTER_OUT) {
// bad value, do not set the distance var, however do increment the filter value
filterControl++;
} else if (getFilteredData() > 100) {
// set distance to this.distance
distance = 100;
} else {
// distance went below 100, therefore reset everything.
filterControl = 0;
distance = getFilteredData();
}
perror = distance - bandCenter;
amperror = perror * proportion;
if (distance > 60) {
// if distance >80, the robot is about to do a U-turn
leftMotor.setSpeed(motorStraight);
rightMotor.setSpeed(maxSpeed);
// right wheel will speed up in order to turn left
leftMotor.forward();
rightMotor.forward();
}
/* else if(distance<10){
// if distance<8, the robot is going to hit the wall
rightMotor.setSpeed(motorStraight+50);
rightMotor.setSpeed(motorStraight+50);
rightMotor.forward();
leftMotor.backward();
// right motor rolls backwards to respond.
}
*/
else {
// general proportion error correction
leftMotor.setSpeed(motorStraight);
if ((motorStraight + amperror) > maxSpeed) {
rightMotor.setSpeed(maxSpeed);
}
// left wheel remains constant speed
else {
rightMotor.setSpeed((int) (motorStraight + amperror));
}
// right wheel will continuously correcting its speed according to errors.
leftMotor.forward();
rightMotor.forward();
}
if (CloseTo(leftDegreeHelper(angleSave, odometer.getAng()), 90)) {
sensorMotor.rotate(-85);
safe = true;
}
}
}
if (collide() == true) {
leftMotor.stop(true);
rightMotor.stop(false);
leftMotor.rotate(convertDistance(2.2, -10), true);
rightMotor.rotate(convertDistance(2.2, -10), false);
}
}
public static void levantar_garra() {
motorC.setSpeed(50);
motorC.rotate(50, false);
motorC.stop();
}
public static void baixar_garra() {
motorC.setSpeed(50);
motorC.rotate(-50, false);
motorC.stop();
}
public static void resolucao_primeira() {
motorC.setSpeed(30);
// motorC.rotate(-15, false);
motorC.rotate(-30, false);
motorC.stop();
}
public static void parar() {
motorA.stop(true);
motorB.stop(true);
}