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){
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;
angle2go = (angle2go*180)/Math.PI;
double P = 0.5;
while (opModeIsActive()){
yaw = robotOrientation.getYaw(AngleUnit.RADIANS);
double currentPos = yaw;
double error = currentPos-lastPos;
lastPos = currentPos;
command += P * error;
double error = yaw-currentpos;
double command = (P * error)/Math.PI;
command = clamp(command,.6);
lm.setPower(command);
rm.setPower(-command);
}
}
double clamp(double value, double max) {
return Math.max(-max, Math.min(max, value));
}
@Override
public void runOpMode() {