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

feat:ftc2024-carlike.java

Browse source
This commit is contained in:
GZod01 2024-01-07 16:41:52 +01:00
parent 0064a62555
commit 1b69a283ea
1 changed files with 10 additions and 63 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

73
ftc2024-carlike.java
View file

@ -27,8 +27,6 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
private DcMotor rm; private DcMotor rm;
private DcMotor lm; private DcMotor lm;
private IMU imu; private IMU imu;
private Orientation lastAngles = new Orientation();
private double globalAngle, power = .30, correction;
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);
} }
@ -135,69 +133,18 @@ public class Werobot_FTC2024_carlike extends LinearOpMode {
telemetry.addData("ltrigg",t); telemetry.addData("ltrigg",t);
telemetry.addData("t2",t2); telemetry.addData("t2",t2);
telemetry.addData("mode",mode); telemetry.addData("mode",mode);
// Use gyro to drive in a straight line. // Create an object to receive the IMU angles
correction = checkDirection(); YawPitchRollAngles robotOrientation;
robotOrientation = imu.getRobotYawPitchRollAngles();
// Now use these simple methods to extract each angle
// (Java type double) from the object you just created:
double Yaw = robotOrientation.getYaw(AngleUnit.DEGREES);
double Pitch = robotOrientation.getPitch(AngleUnit.DEGREES);
double Roll = robotOrientation.getRoll(AngleUnit.DEGREES);
telemetry.addData("yaw",Yaw);
telemetry.addData("1 imu heading", lastAngles.firstAngle);
telemetry.addData("2 global heading", globalAngle);
telemetry.addData("3 correction", correction);
telemetry.update(); telemetry.update();
} }
} }
private void resetAngle()
{
lastAngles = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
globalAngle = 0;
}
/**
* Get current cumulative angle rotation from last reset.
* @return Angle in degrees. + = left, - = right.
*/
private double getAngle()
{
// We experimentally determined the Z axis is the axis we want to use for heading angle.
// We have to process the angle because the imu works in euler angles so the Z axis is
// returned as 0 to +180 or 0 to -180 rolling back to -179 or +179 when rotation passes
// 180 degrees. We detect this transition and track the total cumulative angle of rotation.
Orientation angles = imu.getRobotOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
double deltaAngle = angles.firstAngle - lastAngles.firstAngle;
if (deltaAngle < -180)
deltaAngle += 360;
else if (deltaAngle > 180)
deltaAngle -= 360;
globalAngle += deltaAngle;
lastAngles = angles;
return globalAngle;
}
/**
* See if we are moving in a straight line and if not return a power correction value.
* @return Power adjustment, + is adjust left - is adjust right.
*/
private double checkDirection()
{
// The gain value determines how sensitive the correction is to direction changes.
// You will have to experiment with your robot to get small smooth direction changes
// to stay on a straight line.
double correction, angle, gain = .10;
angle = getAngle();
if (angle == 0)
correction = 0; // no adjustment.
else
correction = -angle; // reverse sign of angle for correction.
correction = correction * gain;
return correction;
}
} }