diff --git a/FTC2024WeRobotControl.java b/FTC2024WeRobotControl.java index 55f0ceb..5d6ef89 100644 --- a/FTC2024WeRobotControl.java +++ b/FTC2024WeRobotControl.java @@ -58,6 +58,7 @@ public class FTC2024WeRobotControl { this.timer = new ElapsedTime(); } +<<<<<<< HEAD public void boxElv() { Parent.lmelevator.setVelocity(600); Parent.rmelevator.setVelocity(600); @@ -75,6 +76,44 @@ public class FTC2024WeRobotControl { * @param motor_speed = (optional) double between 0 and 1; motor power; default * to 1 */ +======= + /* + * return a metre/sec speed + * + * @param motor_speed = (optional) double between 0 and 1; motor power; default + * to 1 + */ + + public void boxElv(){ + Parent.lmelevator.setVelocity(600); + Parent.rmelevator.setVelocity(600); + Parent.lmelevator.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.lmelevator.setTargetPosition(0); + Parent.rmelevator.setTargetPosition(0); + Parent.rotation.setTargetPosition(800); + Parent.lmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + Parent.rmelevator.setMode(DcMotor.RunMode.RUN_TO_POSITION); + Parent.rotation.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + } + + public double getSpeedFromMotorSpeed(double motor_speed) { + double speed_tour_par_minutes = this.tour_par_minutes * motor_speed; + double speed = (speed_tour_par_minutes / 60) * this.wheel_perimeter; + return speed; + } + + +>>>>>>> 1a4f84df926fc16dbce849ff988c4823ffffb3e1 public double getSpeedFromMotorSpeed(double motor_speed) { double speed_tour_par_minutes = this.tour_par_minutes * motor_speed; diff --git a/ftc2024_autonome_api.java b/ftc2024_autonome_api.java index 88a8201..7638547 100644 --- a/ftc2024_autonome_api.java +++ b/ftc2024_autonome_api.java @@ -41,8 +41,8 @@ public class Ftc2024_autonome_api extends LinearOpMode { rm = hardwareMap.get(DcMotor.class, "brm"); harvestmotor = hardwareMap.get(DcMotor.class, "moissonneuse"); rotation = hardwareMap.get(DcMotorEx.class, "elvRot"); - lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); - rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); + lmelevator = hardwareMap.get(DcMotorEx.class, "ltrselv"); + rmelevator = hardwareMap.get(DcMotorEx.class, "rtrselv"); rm.setDirection(DcMotor.Direction.REVERSE); rotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); @@ -106,6 +106,7 @@ public class Ftc2024_autonome_api extends LinearOpMode { robot.boxElv(); robot.forward(2); robot.rotate(90); + robot.posBasse(); robot.forward(3.5); robot.harvest(-1); robot.backward(0.5);