SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

Fixed loop times on OTOS localizer. Updated the pinpoint to the latest driver

Browse Source
This commit is contained in:
Logan-Nash
2024-12-10 10:40:22 -05:00
parent d34d56d567
commit f4942715d9
2 changed files with 12 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/GoBildaPinpointDriver.java
View File

@ -314,9 +314,16 @@ public class GoBildaPinpointDriver extends I2cDeviceSynchDevice<I2cDeviceSynchSi
* @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left
*/ */
public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){ public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){
if (xEncoder == EncoderDirection.FORWARD){
writeInt(Register.DEVICE_CONTROL,1<<5);
}
if (xEncoder == EncoderDirection.REVERSED) { if (xEncoder == EncoderDirection.REVERSED) {
writeInt(Register.DEVICE_CONTROL,1<<4); writeInt(Register.DEVICE_CONTROL,1<<4);
} }
if (yEncoder == EncoderDirection.FORWARD){
writeInt(Register.DEVICE_CONTROL,1<<3);
}
if (yEncoder == EncoderDirection.REVERSED){ if (yEncoder == EncoderDirection.REVERSED){
writeInt(Register.DEVICE_CONTROL,1<<2); writeInt(Register.DEVICE_CONTROL,1<<2);
} }

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
View File

@ -108,10 +108,12 @@ public class OTOSLocalizer extends Localizer {
*/ */
@Override @Override
public Pose getPose() { public Pose getPose() {
SparkFunOTOS.Pose2D rawPose = otos.getPosition(); Pose pose = new Pose(otosPose.x, otosPose.y, otosPose.h);
Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h);
return MathFunctions.addPoses(startPose, MathFunctions.rotatePose(pose, startPose.getHeading(), false)); Vector vec = pose.getVector();
vec.rotateVector(startPose.getHeading());
return MathFunctions.addPoses(startPose, new Pose(vec.getXComponent(), vec.getYComponent(), pose.getHeading()));
} }
/** /**