Change autonomousMode

This commit is contained in:
Zelina974 2024-04-06 10:40:00 +02:00
parent 868b380755
commit 1a4f84df92
2 changed files with 17 additions and 4 deletions

View file

@ -70,12 +70,22 @@ public class FTC2024WeRobotControl {
Parent.rmelevator.setVelocity(600); Parent.rmelevator.setVelocity(600);
Parent.lmelevator.setTargetPosition(90); Parent.lmelevator.setTargetPosition(90);
Parent.rmelevator.setTargetPosition(90); Parent.rmelevator.setTargetPosition(90);
Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
public void posBasse(){
Parent.lmelevator.setVelocity(600);
Parent.rmelevator.setVelocity(600);
Parent.rotation.setVelocity(600); Parent.rotation.setVelocity(600);
Parent.rotation.setTargetPosition(-50); Parent.lmelevator.setTargetPosition(0);
Parent.rmelevator.setTargetPosition(0);
Parent.rotation.setTargetPosition(800);
Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION);
Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
}
public double getSpeedFromMotorSpeed(double motor_speed) { public double getSpeedFromMotorSpeed(double motor_speed) {
double speed_tour_par_minutes = this.tour_par_minutes * motor_speed; double speed_tour_par_minutes = this.tour_par_minutes * motor_speed;
@ -83,6 +93,8 @@ public class FTC2024WeRobotControl {
return speed; return speed;
} }
/* /*
* return the needed time for a distance * return the needed time for a distance
* *

View file

@ -41,8 +41,8 @@ public class Ftc2024_autonome_api extends LinearOpMode {
rm = hardwareMap.get(DcMotor.class, "brm"); rm = hardwareMap.get(DcMotor.class, "brm");
harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse"); harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse");
rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); rotation = hardwareMap.get(DcMotorEx.class, "elvRot");
lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv");
rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv");
rm.setDirection(DcMotor.Direction.REVERSE); rm.setDirection(DcMotor.Direction.REVERSE);
rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
@ -106,6 +106,7 @@ public class Ftc2024_autonome_api extends LinearOpMode {
robot.boxElv(); robot.boxElv();
robot.forward(2); robot.forward(2);
robot.rotate(90); robot.rotate(90);
robot.posBasse();
robot.forward(3.5); robot.forward(3.5);
robot.harvest(-1); robot.harvest(-1);
robot.backward(0.5); robot.backward(0.5);