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 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(); }