Team4388/RiseOfRidgebotics2020
1
0
Fork 0
mirror of https://github.com/Team4388/RiseOfRidgebotics2020.git synced 2026-06-09 08:48:01 -06:00
Code Issues Packages Projects Releases Wiki Activity

Fixed heading in odometry, but position needs work

Browse Source
This commit is contained in:
Aarav Shah
2020-02-13 19:35:30 -07:00
parent a3a1e953fb
commit 03daeed506
2 changed files with 4 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/Robot.java
+1
View File
@@ -89,6 +89,7 @@ public class Robot extends TimedRobot {
// schedule the autonomous command (example)
if (m_autonomousCommand != null) {
m_autonomousCommand.schedule();
System.err.println("Auto Start");
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+3 -2
View File
@@ -77,7 +77,7 @@ public class Drive extends SubsystemBase {
m_pigeon.configFactoryDefault();
resetGyroYaw();
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()));
m_odometry = new DifferentialDriveOdometry(Rotation2d.fromDegrees(getHeading()), new Pose2d(0, 0, new Rotation2d()));
speedShift = new DoubleSolenoid(7,0,1);
@@ -263,6 +263,7 @@ public class Drive extends SubsystemBase {
//SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
SmartDashboard.putString("Odometry Values Meters", getPose().toString());
SmartDashboard.putNumber("Odometry Heading", getHeading());
} catch (Exception e) {
System.err.println("Error in the Drive Subsystem");
@@ -271,7 +272,7 @@ public class Drive extends SubsystemBase {
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftFrontMotor)),
inchesToMeters(getDistanceInches(m_rightFrontMotor)));
inchesToMeters(-getDistanceInches(m_rightFrontMotor)));
}
/**