Merge branch 'master' of github.com:GZod01/ftc2024-robotcode
This commit is contained in:
commit
942c69b18f
2 changed files with 28 additions and 2 deletions
|
@ -45,14 +45,14 @@ public class ftc2024_autonome extends LinearOpMode {
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
if (mode){
|
if (mode){
|
||||||
//mode Elina
|
//mode Elina
|
||||||
while (opModeIsActive() && (runtime.seconds() <= 3.0)) {
|
while (opModeIsActive() && (runtime.seconds() <= 41e-2*Math.PI/4/speed)) {
|
||||||
lm.setPower(1);
|
lm.setPower(1);
|
||||||
rm.setPower(-1);
|
rm.setPower(-1);
|
||||||
telemetry.addData("Leg 1", runtime.seconds());
|
telemetry.addData("Leg 1", runtime.seconds());
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
}
|
}
|
||||||
runtime.reset();
|
runtime.reset();
|
||||||
while (opModeIsActive() && (runtime.seconds() <= 10.0)) {
|
while (opModeIsActive() && (runtime.seconds() <= 121.92e-2/speed)) {
|
||||||
lm.setPower(1);
|
lm.setPower(1);
|
||||||
rm.setPower(1);
|
rm.setPower(1);
|
||||||
telemetry.addData("Leg 2", runtime.seconds());
|
telemetry.addData("Leg 2", runtime.seconds());
|
||||||
|
|
|
@ -26,6 +26,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||||
public class Werobot_FTC2024_carlike extends LinearOpMode {
|
public class Werobot_FTC2024_carlike extends LinearOpMode {
|
||||||
private DcMotor rm;
|
private DcMotor rm;
|
||||||
private DcMotor lm;
|
private DcMotor lm;
|
||||||
|
private DcMotor moissoneuse;
|
||||||
private IMU imu;
|
private IMU imu;
|
||||||
private double helloexp(double t){
|
private double helloexp(double t){
|
||||||
return (Math.exp(5*t)-1)/(Math.exp(5)-1);
|
return (Math.exp(5*t)-1)/(Math.exp(5)-1);
|
||||||
|
@ -38,6 +39,7 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
|
||||||
double t2;
|
double t2;
|
||||||
double t3;
|
double t3;
|
||||||
String mode = "normal";
|
String mode = "normal";
|
||||||
|
boolean already_b = false;
|
||||||
boolean already_a = false;
|
boolean already_a = false;
|
||||||
telemetry.addData("Status", "Initialized");
|
telemetry.addData("Status", "Initialized");
|
||||||
telemetry.update();
|
telemetry.update();
|
||||||
|
@ -127,6 +129,30 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
|
||||||
lm.setPower(lpower);
|
lm.setPower(lpower);
|
||||||
rm.setPower(rpower);
|
rm.setPower(rpower);
|
||||||
|
|
||||||
|
if(gamepad1.b && !already_b){
|
||||||
|
already_b = !already_b;
|
||||||
|
if(moissoneuse.getPower == 1){
|
||||||
|
moissoneuse.setPower(0);
|
||||||
|
}else{
|
||||||
|
moissoneuse.setPower(1);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
if(!gamepad1.b && already_b){
|
||||||
|
already_b = !already_b;
|
||||||
|
}
|
||||||
|
if(gamepad1.a && !already_a){
|
||||||
|
already_a = !already_a;
|
||||||
|
if(moissoneuse.getPower == -1){
|
||||||
|
moissoneuse.setPower(0);
|
||||||
|
}else{
|
||||||
|
moissoneuse.setPower(-1);
|
||||||
|
}
|
||||||
|
if(!gamepad1.a && already_a){
|
||||||
|
already_a = !already_a;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
telemetry.addData("x",x);
|
telemetry.addData("x",x);
|
||||||
telemetry.addData("y",y);
|
telemetry.addData("y",y);
|
||||||
telemetry.addData("lpow",lpower);
|
telemetry.addData("lpow",lpower);
|
||||||
|
|
Loading…
Reference in a new issue