werobot/ftc2024-robotcode
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

hello

Browse source
This commit is contained in:
GZod01 2024-02-11 17:12:29 +01:00
parent 101512a7dd
commit 648f8a813f
1 changed files with 25 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

26
ftc2024-autonome.java
View file

@ -32,11 +32,34 @@ public class ftc2024_autonome extends LinearOpMode {
private DcMotor lm; private DcMotor lm;
private IMU imu; private IMU imu;
private ElapsedTime runtime = new ElapsedTime(); private ElapsedTime runtime = new ElapsedTime();
YawPitchRollAngles robotOrientation;
public double time_for_dist(double speed, double dist){ public double time_for_dist(double speed, double dist){
return (double) (dist/speed); return (double) (dist/speed);
} }
/*
* @param double angle2go => degrees
*/
public double runPid(double angle2go){
robotOrientation = imu.getRobotYawPitchRollAngles();
angle2go = (angle2go*180)/Math.PI
double yaw = robotOrientation.getYaw(AngleUnit.RADIANS);
double lastPos;
// command -1.0<command<1.0
double command = (angle2go-yaw)/2*Math.PI;
while (opModeIsActive()){
yaw = robotOrientation.getYaw(AngleUnit.RADIANS);
double currentPos = yaw;
double error = currentPos-lastPos;
lastPos = currentPos;
command += P * error;
lm.setPower(command);
rm.setPower(-command);
}
}
@Override @Override
public void runOpMode() { public void runOpMode() {
@ -64,7 +87,6 @@ public class ftc2024_autonome extends LinearOpMode {
double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second double speed = (tour_par_minute/60)*wheel_perimeter;//dist per second
boolean mode = true; boolean mode = true;
YawPitchRollAngles robotOrientation;
robotOrientation = imu.getRobotYawPitchRollAngles(); robotOrientation = imu.getRobotYawPitchRollAngles();
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES); double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
@ -106,6 +128,8 @@ public class ftc2024_autonome extends LinearOpMode {
rm.setPower((Math.abs(Yaw-90.0)/90)*0.5); rm.setPower((Math.abs(Yaw-90.0)/90)*0.5);
lm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5); lm.setPower(-(Math.abs(Yaw-90.0)/90)*0.5);
} }
}
}
if(false){ if(false){
double[][] operations = { double[][] operations = {