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

helloworld

Browse source
This commit is contained in:
GZod01 2024-03-08 14:29:30 +01:00
parent f43822b675
commit 864306c9bf
1 changed files with 8 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

16
ftc2024-autonome.java
View file

@ -44,22 +44,22 @@ public class ftc2024_autonome extends LinearOpMode {
*/ */
public double runPid(double angle2go){ public double runPid(double angle2go){
robotOrientation = imu.getRobotYawPitchRollAngles(); robotOrientation = imu.getRobotYawPitchRollAngles();
angle2go = (angle2go*180)/Math.PI angle2go = (angle2go*180)/Math.PI;
double yaw = robotOrientation.getYaw(AngleUnit.RADIANS); double P = 0.5;
double lastPos;
// command -1.0<command<1.0
double command = (angle2go-yaw)/2*Math.PI;
while (opModeIsActive()){ while (opModeIsActive()){
yaw = robotOrientation.getYaw(AngleUnit.RADIANS); yaw = robotOrientation.getYaw(AngleUnit.RADIANS);
double currentPos = yaw; double currentPos = yaw;
double error = currentPos-lastPos; double error = yaw-currentpos;
lastPos = currentPos; double command = (P * error)/Math.PI;
command += P * error; command = clamp(command,.6);
lm.setPower(command); lm.setPower(command);
rm.setPower(-command); rm.setPower(-command);
} }
} }
double clamp(double value, double max) {
return Math.max(-max, Math.min(max, value));
}
@Override @Override
public void runOpMode() { public void runOpMode() {