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

rebuilt all paths, flipped odometry in auto (janky)

Browse Source 
Mostly untested
This commit is contained in:
ryan123rudder
2020-07-13 14:54:42 -06:00
parent a62a91f5f3
commit 560dda1f58
22 changed files with 90 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
PathWeaver/Groups/Test Auto
+1
View File
@@ -0,0 +1 @@
DriveOffLineForward
PathWeaver/Groups/Unnamed
+1
View File
@@ -0,0 +1 @@
Unnamed_0.path
PathWeaver/Paths/SixBallMid0
+1 -1
View File
@@ -1,3 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
3.2,-2.4,0.2,2.5,true,
3.2,-2.4,-0.0023621783990721568,2.1247596755680735,true,
5.006107200045366,-1.3,2.0,0.0,true,
PathWeaver/Paths/Unnamed.path
+3
View File
@@ -0,0 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
0.0,0.0,3.048,0.0,true,
3.048,-3.048,0.0,-3.048,true,
PathWeaver/Paths/Unnamed_0.path
+3
View File
@@ -0,0 +1,3 @@
X,Y,Tangent X,Tangent Y,Fixed Theta,Name
3.1500465221681786,-2.464440098338352,-0.011897824858186468,1.76087807901169,true,
7.825891691435707,-1.3460445616687653,3.4265735591578856,0.059489124290935,true,
PathWeaver/pathweaver.json
+2 -3
View File
@@ -1,8 +1,7 @@
{
"lengthUnit": "Meter",
"exportUnit": "Always Meters",
"maxVelocity": 1.7,
"maxAcceleration": 2.7,
"maxVelocity": 1.5,
"maxAcceleration": 2.5,
"wheelBase": 0.648,
"gameName": "Infinite Recharge",
"outputDir": ".."
src/main/deploy/paths/DriveOffLineBackward.wpilib.json
+1 -1
View File
@@ -1 +1 @@
[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
[{"time":0.0,"velocity":0.0,"acceleration":2.5000000000000004,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3162277660168379,"velocity":0.7905694150420949,"acceleration":2.5000000000000004,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.44721359549995787,"velocity":1.118033988749895,"acceleration":-2.5000000000000004,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5781994249830779,"velocity":0.7905694150420949,"acceleration":-2.5000000000000004,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8944271909999157,"velocity":0.0,"acceleration":-2.5000000000000004,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
src/main/deploy/paths/DriveOffLineForward.wpilib.json
+1 -1
View File
@@ -1 +1 @@
[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3042903097250923,"velocity":0.8215838362577492,"acceleration":2.7,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.43033148291193524,"velocity":1.161895003862225,"acceleration":-2.7,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5563726560987782,"velocity":0.8215838362577492,"acceleration":-2.7,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8606629658238705,"velocity":0.0,"acceleration":-2.7,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
[{"time":0.0,"velocity":0.0,"acceleration":2.5000000000000004,"pose":{"translation":{"x":3.2,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3162277660168379,"velocity":0.7905694150420949,"acceleration":2.5000000000000004,"pose":{"translation":{"x":3.325,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.44721359549995787,"velocity":1.118033988749895,"acceleration":-2.5000000000000004,"pose":{"translation":{"x":3.45,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5781994249830779,"velocity":0.7905694150420949,"acceleration":-2.5000000000000004,"pose":{"translation":{"x":3.575,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8944271909999157,"velocity":0.0,"acceleration":-2.5000000000000004,"pose":{"translation":{"x":3.7,"y":-2.3},"rotation":{"radians":0.0}},"curvature":0.0}]
src/main/deploy/paths/EightBallMid0.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMid1.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMid2.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/EightBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/FiveBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/SixBallMid0.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/SixBallMid1.wpilib.json
+1 -1
View File
@@ -1 +1 @@
[{"time":0.0,"velocity":0.0,"acceleration":2.7,"pose":{"translation":{"x":5.006,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.2656061620411004,"velocity":0.717136637510971,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.101237954974175,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3755418079977478,"velocity":1.013962881593919,"acceleration":2.699999999999999,"pose":{"translation":{"x":5.1963927268981935,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.45975256128584724,"velocity":1.2413319154717875,"acceleration":2.6999999999999997,"pose":{"translation":{"x":5.291352763772011,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5305335412553954,"velocity":1.4324405613895677,"acceleration":2.7000000000000006,"pose":{"translation":{"x":5.385978881835937,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5926137417782101,"velocity":1.6000571028011674,"acceleration":1.7647018169830884,"pose":{"translation":{"x":5.480107913374901,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6492481806601815,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.573556354522705,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7036997445093501,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.666124013066292,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7575077699115317,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.757597656250001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.810541300693782,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.847754658579826,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8626660013101702,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":5.936366649627686,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9137463026089846,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.023203161835671,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9636475475999504,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.108035278320313,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0122381372214386,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.190639280676843,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0593916761076771,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.270800296783448,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1049891183559657,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.3483159486055385,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1489209132938847,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.423000000000001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1910891512465098,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.494686004519464,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2314097093036216,"velocity":1.7,"acceleration":0.0,"pose":{"translation":{"x":6.563230953216554,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2698143970869191,"velocity":1.7,"acceleration":-1.1179894261480032,"pose":{"translation":{"x":6.628518922448159,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3067004902763237,"velocity":1.6587617378423356,"acceleration":-2.700000000000003,"pose":{"translation":{"x":6.690464721679689,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3795667014519033,"velocity":1.4620229676682703,"acceleration":-2.700000000000001,"pose":{"translation":{"x":6.804164600372316,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4531237596123885,"velocity":1.26341891063496,"acceleration":-2.7000000000000015,"pose":{"translation":{"x":6.904402343750003,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.528510297293001,"velocity":1.0598752588973066,"acceleration":-2.7,"pose":{"translation":{"x":6.9919748954772984,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6089801534753305,"velocity":0.8426066472050169,"acceleration":-2.6999999999999997,"pose":{"translation":{"x":7.0685211181640675,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7044131404571534,"velocity":0.5849375823540952,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.136638523101813,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.9210566894771886,"velocity":0.0,"acceleration":-2.7000000000000006,"pose":{"translation":{"x":7.200000000000008,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0}]
[{"time":0.0,"velocity":0.0,"acceleration":2.5,"pose":{"translation":{"x":5.006,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.2760260204751348,"velocity":0.690065051187837,"acceleration":2.5000000000000004,"pose":{"translation":{"x":5.101237954974175,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.3902744950910252,"velocity":0.975686237727563,"acceleration":2.5000000000000004,"pose":{"translation":{"x":5.1963927268981935,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.47778887703420686,"velocity":1.1944721925855173,"acceleration":2.5000000000000013,"pose":{"translation":{"x":5.291352763772011,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.5513466291442704,"velocity":1.3783665728606762,"acceleration":1.8597110004015631,"pose":{"translation":{"x":5.385978881835937,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6167511071051255,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":5.480107913374901,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.6790500678703282,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":5.573556354522705,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.7407618402327193,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":5.666124013066292,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8017442690218584,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":5.757597656250001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.8618489372417422,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":5.847754658579826,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9209235979403155,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":5.936366649627686,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":0.9788146060789717,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.023203161835671,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0353693504020665,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.108035278320313,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.0904386853064199,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.190639280676843,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1438793627108235,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.270800296783448,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.1955564639255505,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.3483159486055385,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2453458315218588,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.423000000000001,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.2931365012015006,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.494686004519464,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3388331336662271,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.563230953216554,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.3823584464872976,"velocity":1.5,"acceleration":0.0,"pose":{"translation":{"x":6.628518922448159,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.4236556459749838,"velocity":1.5,"acceleration":-1.1909555444367483,"pose":{"translation":{"x":6.690464721679689,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.5018850508093828,"velocity":1.4068322565744855,"acceleration":-2.500000000000001,"pose":{"translation":{"x":6.804164600372316,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.5783277880029385,"velocity":1.2157254135905964,"acceleration":-2.5,"pose":{"translation":{"x":6.904402343750003,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.6566717760846543,"velocity":1.019865443386307,"acceleration":-2.5,"pose":{"translation":{"x":6.9919748954772984,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.7402985037159875,"velocity":0.8107986243079742,"acceleration":-2.4999999999999996,"pose":{"translation":{"x":7.0685211181640675,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":1.8394753730183333,"velocity":0.5628564510521096,"acceleration":-2.5000000000000004,"pose":{"translation":{"x":7.136638523101813,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0},{"time":2.064617953439177,"velocity":0.0,"acceleration":-2.5000000000000004,"pose":{"translation":{"x":7.200000000000008,"y":-1.3},"rotation":{"radians":0.0}},"curvature":0.0}]
src/main/deploy/paths/SixBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/TenBallMidComplete.wpilib.json
+1 -1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Unnamed.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/deploy/paths/Unnamed_0.wpilib.json
+1
View File
File diff suppressed because one or more lines are too long
src/main/java/frc4388/robot/RobotContainer.java
+9 -4
View File
@@ -16,6 +16,7 @@ import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
@@ -316,7 +317,9 @@ public class RobotContainer {
//resetOdometry(new Pose2d(0, 0, new Rotation2d(180)));
String[] sixBallAutoMiddlePaths = new String[]{
"SixBallMidComplete"
"SixBallMid0",
"SixBallMid1"
//"Unnamed_0"
};
m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths));
@@ -369,12 +372,12 @@ public class RobotContainer {
try {
SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto);
//return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_driveOffLineForward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_driveOffLinfeBackward.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
//return m_fiveBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
//return m_tenBallAutoMiddle.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0));
} catch (Exception e) {
System.err.println("ERROR");
@@ -382,6 +385,7 @@ public class RobotContainer {
return new InstantCommand();
}
TrajectoryConfig getTrajectoryConfig() {
return new TrajectoryConfig(
DriveConstants.MAX_SPEED_METERS_PER_SECOND,
@@ -432,7 +436,8 @@ public class RobotContainer {
SmartDashboard.putString("trajectoryPath Initial", trajectoryPath.toString());
Trajectory trajectory = TrajectoryUtil.fromPathweaverJson(trajectoryPath);
initialTrajectory = trajectory;
//Transform2d transform = m_robotDrive.getPose().minus(trajectory.getInitialPose());
initialTrajectory = trajectory;//.transformBy(transform);
RamseteCommand ramseteCommand = getRamseteCommand(trajectory.relativeTo(initialTrajectory.getInitialPose()));
ramseteCommands[0] = ramseteCommand;
src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java
+19 -2
View File
@@ -10,8 +10,10 @@ package frc4388.robot.commands.auto;
import java.nio.file.Path;
import edu.wpi.first.wpilibj.Filesystem;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
import frc4388.robot.RobotContainer;
@@ -27,9 +29,24 @@ public class SixBallAutoMiddle extends SequentialCommandGroup {
public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
/* TODO
* Flip line 41 to true, and test positive and negative directions with smart dash
* If those match to the picture on Ryan's phone, figure out what else could be wrong
* If they don't match, make them match
* Sincerly, Past Ryan and Keenan
*/
addCommands(
paths[0]
new InstantCommand(() -> drive.getPose(), drive),
new InstantCommand(() -> drive.SetHeading(true), drive),
new InstantCommand(() -> drive.updatePosition(), drive),
new InstantCommand(() -> drive.setOdometry(drive.savedOdometry), drive),
paths[0],
paths[1],
new InstantCommand(() -> drive.getPose(), drive),
new InstantCommand(() -> drive.SetHeading(false), drive),
new InstantCommand(() -> drive.updatePosition(), drive),
new InstantCommand(() -> drive.setOdometry(drive.savedOdometry), drive)
);
}
}
src/main/java/frc4388/robot/subsystems/Drive.java
+38 -9
View File
@@ -162,7 +162,7 @@ public class Drive extends SubsystemBase {
m_leftBackMotor.config_kI(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kI, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.config_kD(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kD, DriveConstants.DRIVE_TIMEOUT_MS);
m_leftBackMotor.configClosedLoopPeakOutput(DriveConstants.SLOT_VELOCITY, m_gainsVelocityBack.m_kPeakOutput, DriveConstants.DRIVE_TIMEOUT_MS);
/* Reset Sensors for WPI_TalonFXs */
resetEncoders();
@@ -339,9 +339,15 @@ public class Drive extends SubsystemBase {
m_totalRightDistanceInches += ticksToInches(m_currentRightPosTicks - m_lastRightPosTicks);
m_totalLeftDistanceInches += ticksToInches(m_currentLeftPosTicks - m_lastLeftPosTicks);
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
if (m_isFront) {
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_rightFrontMotor)),
-inchesToMeters(getDistanceInches(m_leftFrontMotor)));
} else {
m_odometry.update(Rotation2d.fromDegrees( getHeading()),
inchesToMeters(getDistanceInches(m_leftFrontMotor)),
-inchesToMeters(getDistanceInches(m_rightFrontMotor)));
}
}
/**
@@ -461,8 +467,18 @@ public class Drive extends SubsystemBase {
// m_currentAngleYaw+(360));
// double targetGyro = (m_kinematicsTargetAngle / 360) *
// DriveConstants.TICKS_PER_GYRO_REV;
double moveVelLeft = inchesToTicks(metersToInches(leftSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
double moveVelRight = inchesToTicks(metersToInches(rightSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
double moveVelRight;
double moveVelLeft;
if (m_isFront)
{
moveVelRight = -inchesToTicks(metersToInches(leftSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
moveVelLeft = -inchesToTicks(metersToInches(rightSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
}
else
{
moveVelLeft = inchesToTicks(metersToInches(leftSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
moveVelRight = inchesToTicks(metersToInches(rightSpeed)) / DriveConstants.SECONDS_TO_TICK_TIME;
}
// SmartDashboard.putNumber("Move Vel Left", moveVelLeft);
// SmartDashboard.putNumber("Move Vel Right", moveVelRight);
@@ -480,6 +496,12 @@ public class Drive extends SubsystemBase {
m_driveTrain.feedWatchdog();
}
boolean m_isFront;
public void SetHeading (boolean isFront)
{
m_isFront = isFront;
}
/**
* Selects a song to play!
*
@@ -613,7 +635,14 @@ public class Drive extends SubsystemBase {
* @return The robot's heading in degrees, from -180 to 180
*/
public double getHeading() {
return Math.IEEEremainder(getGyroYaw(), 360);
if (m_isFront)
{
return Math.IEEEremainder(getGyroYaw()+180, 360);
}
else
{
return Math.IEEEremainder(getGyroYaw(), 360);
}
}
/**
@@ -627,13 +656,15 @@ public class Drive extends SubsystemBase {
return turnRate;
}
public Pose2d savedOdometry;
/**
* Returns the currently-estimated pose of the robot.
*
* @return The pose.
*/
public Pose2d getPose() {
return m_odometry.getPoseMeters();
savedOdometry = m_odometry.getPoseMeters();
return savedOdometry;
}
/**
@@ -910,7 +941,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());
SmartDashboard.putNumber("Odometry Heading", getHeading());
//SmartDashboard.putNumber("Time Seconds", m_currentTimeSec);
//SmartDashboard.putNumber("Delta Time", m_deltaTimeMs);
@@ -926,6 +957,4 @@ public class Drive extends SubsystemBase {
// e.printStackTrace(System.err);
}
}
}