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

Merge pull request #22 from Logan-Nash/master

Browse Source 
Fixed loop times on OTOS localizer. Updated the pinpoint to the lates…
This commit is contained in:
Logan Nash
2024-12-10 10:42:11 -05:00
committed by GitHub
parent d34d56d567 f4942715d9
commit a2d896b67c
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
*/
public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){
if (xEncoder == EncoderDirection.FORWARD){
writeInt(Register.DEVICE_CONTROL,1<<5);
}
if (xEncoder == EncoderDirection.REVERSED) {
writeInt(Register.DEVICE_CONTROL,1<<4);
}
if (yEncoder == EncoderDirection.FORWARD){
writeInt(Register.DEVICE_CONTROL,1<<3);
}
if (yEncoder == EncoderDirection.REVERSED){
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
public Pose getPose() {
SparkFunOTOS.Pose2D rawPose = otos.getPosition();
Pose pose = new Pose(rawPose.x, rawPose.y, rawPose.h);
Pose pose = new Pose(otosPose.x, otosPose.y, otosPose.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()));
}
/**