diff --git a/networktables.json b/networktables.json index 67911c5d..d63b31e2 100644 --- a/networktables.json +++ b/networktables.json @@ -1,581 +1,54 @@ [ { - "name": "/Preferences/Arm/Acceleration Time Secs", - "type": "double", - "value": 0.125, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/Wrist Max Vel DegsS", - "type": "double", - "value": 360.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/Arm Max Vel DegsS", - "type": "double", - "value": 360.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kp", - "type": "double", - "value": 0.08, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kV", - "type": "double", - "value": 0.06, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kS", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.kp", - "type": "double", - "value": 47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kp", - "type": "double", - "value": 0.08, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kV", - "type": "double", - "value": 0.06, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kS", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.kp", - "type": "double", - "value": 47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kp", - "type": "double", - "value": 0.08, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kV", - "type": "double", - "value": 0.06, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kS", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.kp", - "type": "double", - "value": 47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kp", - "type": "double", - "value": 0.08, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kV", - "type": "double", - "value": 0.06, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kS", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kp", - "type": "double", - "value": 47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Theta Alignment.mm.kp", - "type": "double", - "value": 0.04, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Theta Alignment.mm.ki", - "type": "double", - "value": 0.01, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Theta Alignment.mm.kd", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left PID.mm.kp", - "type": "double", - "value": 0.015, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left PID.mm.kD", - "type": "double", - "value": 0.00675, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left PID.mm.kS", - "type": "double", - "value": 0.21963, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left PID.mm.kV", - "type": "double", - "value": 0.114541, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right PID.mm.kp", - "type": "double", - "value": 0.015, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right PID.mm.kD", - "type": "double", - "value": 0.00675, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right PID.mm.kS", - "type": "double", - "value": 0.21963, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right PID.mm.kV", - "type": "double", - "value": 0.114541, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/WristLowerLimit", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/WristUpperLimit", - "type": "double", - "value": 180.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/ArmLowerLimit", - "type": "double", - "value": -6.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/ArmUpperLimit", - "type": "double", - "value": 180.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/Wrist Gap", - "type": "double", - "value": 20.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kp", - "type": "double", - "value": 350.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.ki", - "type": "double", - "value": 10.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kD", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kS", - "type": "double", - "value": 0.375, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kV", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kG", - "type": "double", - "value": 0.375, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kp", - "type": "double", - "value": 350.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.ki", - "type": "double", - "value": 12.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kD", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kS", - "type": "double", - "value": 0.375, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kV", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kG", - "type": "double", - "value": 0.36, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.kp", - "type": "double", - "value": 12.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.kD", + "name": "/Preferences/Drive/Turning Speed", "type": "double", - "value": 0.0, + "value": 0.75, "properties": { "persistent": true } }, { - "name": "/Preferences/Climber/Left Climber.mm.kS", - "type": "double", - "value": 0.0, + "name": "/Preferences/Drive/Hold Heading", + "type": "boolean", + "value": false, "properties": { "persistent": true } }, { - "name": "/Preferences/Climber/Left Climber.mm.kV", - "type": "double", - "value": 0.0, + "name": "/Preferences/Drive/Use David Drive", + "type": "boolean", + "value": false, "properties": { "persistent": true } }, { - "name": "/Preferences/Climber/Left Climber.mm.kG", + "name": "/Preferences/Drive/Theta Alignment.mm.kp", "type": "double", - "value": 0.0, + "value": 0.02, "properties": { "persistent": true } }, { - "name": "/Preferences/Climber/Right Climber.mm.kp", + "name": "/Preferences/Drive/Theta Alignment.mm.ki", "type": "double", - "value": 12.0, + "value": 0.01, "properties": { "persistent": true } }, { - "name": "/Preferences/Climber/Right Climber.mm.ki", + "name": "/Preferences/Drive/Theta Alignment.mm.kd", "type": "double", - "value": 0.0, + "value": 0.004, "properties": { "persistent": true } }, { - "name": "/Preferences/Climber/Right Climber.mm.kD", + "name": "/Preferences/Wrist Angle Offset In", "type": "double", "value": 0.0, "properties": { @@ -583,25 +56,25 @@ } }, { - "name": "/Preferences/Climber/Right Climber.mm.kS", + "name": "/Preferences/Wrist Angle Degs offset", "type": "double", - "value": 0.0, + "value": 1.6, "properties": { "persistent": true } }, { - "name": "/Preferences/Climber/Right Climber.mm.kV", + "name": "/Preferences/ArmPoses/Pass SetpointArm Angle", "type": "double", - "value": 0.0, + "value": 45.0, "properties": { "persistent": true } }, { - "name": "/Preferences/Climber/Right Climber.mm.kG", + "name": "/Preferences/ArmPoses/Pass SetpointWrist Angle", "type": "double", - "value": 0.0, + "value": 55.0, "properties": { "persistent": true } @@ -622,198 +95,22 @@ "persistent": true } }, - { - "name": "/Preferences/ArmPoses/StowArm Angle", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/StowWrist Angle", - "type": "double", - "value": 35.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/IntakeArm Angle", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/IntakeWrist Angle", - "type": "double", - "value": 35.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/AmpArm Angle", - "type": "double", - "value": 94.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/AmpWrist Angle", - "type": "double", - "value": 145.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left RPM", - "type": "double", - "value": 3750.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right RPM", - "type": "double", - "value": 4500.0, - "properties": { - "persistent": true - } - }, { "name": "/Preferences/ArmPoses/ShooterTestingArm Angle", "type": "double", - "value": 45.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/ShooterTestingWrist Angle", - "type": "double", - "value": 50.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Small.mm.kp", - "type": "double", - "value": 0.035, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Small.mm.ki", - "type": "double", - "value": 0.001, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Small.mm.kd", - "type": "double", - "value": 0.004, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Fast.mm.kp", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Fast.mm.ki", - "type": "double", "value": 0.0, "properties": { "persistent": true } }, { - "name": "/Preferences/Drive/Aimbot Fast.mm.kd", - "type": "double", - "value": 0.002, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Acceleration Compensation", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/Pass SetpointArm Angle", - "type": "double", - "value": 45.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/Pass SetpointWrist Angle", + "name": "/Preferences/ArmPoses/ShooterTestingWrist Angle", "type": "double", "value": 55.0, "properties": { "persistent": true } }, - { - "name": "/Preferences/Drive/Turning Speed", - "type": "double", - "value": 0.75, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Hold Heading", - "type": "boolean", - "value": false, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Use David Drive", - "type": "boolean", - "value": false, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Wrist Angle Offset In", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Wrist Angle Degs offset", - "type": "double", - "value": 1.6, - "properties": { - "persistent": true - } - }, { "name": "/Preferences/Wrist Angle", "type": "double", diff --git a/networktables.json.bck b/networktables.json.bck index 4885dccb..fc9a85d2 100644 --- a/networktables.json.bck +++ b/networktables.json.bck @@ -1,779 +1,4 @@ [ - { - "name": "/Preferences/Arm/Acceleration Time Secs", - "type": "double", - "value": 0.125, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/Wrist Max Vel DegsS", - "type": "double", - "value": 360.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/Arm Max Vel DegsS", - "type": "double", - "value": 360.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kp", - "type": "double", - "value": 0.08, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kV", - "type": "double", - "value": 0.06, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Drive Pid Property.mm.kS", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.kp", - "type": "double", - "value": 47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module0/Turn Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kp", - "type": "double", - "value": 0.08, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kV", - "type": "double", - "value": 0.06, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Drive Pid Property.mm.kS", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.kp", - "type": "double", - "value": 47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module1/Turn Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kp", - "type": "double", - "value": 0.08, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kV", - "type": "double", - "value": 0.06, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Drive Pid Property.mm.kS", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.kp", - "type": "double", - "value": 47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module2/Turn Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kp", - "type": "double", - "value": 0.08, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kV", - "type": "double", - "value": 0.06, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Drive Pid Property.mm.kS", - "type": "double", - "value": 0.35, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kp", - "type": "double", - "value": 47.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Module3/Turn Pid Property.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Theta Alignment.mm.kp", - "type": "double", - "value": 0.04, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Theta Alignment.mm.ki", - "type": "double", - "value": 0.01, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Theta Alignment.mm.kd", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left PID.mm.kp", - "type": "double", - "value": 0.015, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left PID.mm.kD", - "type": "double", - "value": 0.00675, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left PID.mm.kS", - "type": "double", - "value": 0.21963, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left PID.mm.kV", - "type": "double", - "value": 0.114541, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right PID.mm.kp", - "type": "double", - "value": 0.015, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right PID.mm.kD", - "type": "double", - "value": 0.00675, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right PID.mm.kS", - "type": "double", - "value": 0.21963, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right PID.mm.kV", - "type": "double", - "value": 0.114541, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/WristLowerLimit", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/WristUpperLimit", - "type": "double", - "value": 180.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/ArmLowerLimit", - "type": "double", - "value": -6.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/ArmUpperLimit", - "type": "double", - "value": 180.0, - "properties": { - "persistent": true, - "retained": true - } - }, - { - "name": "/Preferences/Arm/Wrist Gap", - "type": "double", - "value": 20.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kp", - "type": "double", - "value": 350.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.ki", - "type": "double", - "value": 10.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kD", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kS", - "type": "double", - "value": 0.375, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kV", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Arm PID.mm.kG", - "type": "double", - "value": 0.375, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kp", - "type": "double", - "value": 350.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.ki", - "type": "double", - "value": 12.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kD", - "type": "double", - "value": 0.5, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kS", - "type": "double", - "value": 0.375, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kV", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Arm/Wrist PID.mm.kG", - "type": "double", - "value": 0.36, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.kp", - "type": "double", - "value": 12.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.kS", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.kV", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Left Climber.mm.kG", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Right Climber.mm.kp", - "type": "double", - "value": 12.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Right Climber.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Right Climber.mm.kD", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Right Climber.mm.kS", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Right Climber.mm.kV", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Climber/Right Climber.mm.kG", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/Amp IntermediateArm Angle", - "type": "double", - "value": 60.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/Amp IntermediateWrist Angle", - "type": "double", - "value": 145.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/StowArm Angle", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/StowWrist Angle", - "type": "double", - "value": 35.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/IntakeArm Angle", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/IntakeWrist Angle", - "type": "double", - "value": 35.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/AmpArm Angle", - "type": "double", - "value": 94.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/AmpWrist Angle", - "type": "double", - "value": 145.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Left RPM", - "type": "double", - "value": 3750.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Right RPM", - "type": "double", - "value": 4500.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/ShooterTestingArm Angle", - "type": "double", - "value": 45.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/ShooterTestingWrist Angle", - "type": "double", - "value": 50.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Small.mm.kp", - "type": "double", - "value": 0.035, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Small.mm.ki", - "type": "double", - "value": 0.001, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Small.mm.kd", - "type": "double", - "value": 0.004, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Fast.mm.kp", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Fast.mm.ki", - "type": "double", - "value": 0.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Drive/Aimbot Fast.mm.kd", - "type": "double", - "value": 0.002, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/Shooter/Acceleration Compensation", - "type": "double", - "value": 0.1, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/Pass SetpointArm Angle", - "type": "double", - "value": 45.0, - "properties": { - "persistent": true - } - }, - { - "name": "/Preferences/ArmPoses/Pass SetpointWrist Angle", - "type": "double", - "value": 55.0, - "properties": { - "persistent": true - } - }, { "name": "/Preferences/Drive/Turning Speed", "type": "double", diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index fdc8efce..e9075253 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -36,6 +36,7 @@ import edu.wpi.first.units.Voltage; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.DriverStation.Alliance; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.sysid.SysIdRoutineLog; @@ -248,6 +249,10 @@ public void periodic() { // process timestamp inputs m_timestampInputs.timestamps = m_timestampQueue.stream().mapToDouble(Double::valueOf).toArray(); + // fake sim inputs + if (m_timestampInputs.timestamps.length == 0) { + m_timestampInputs.timestamps = new double[] {Timer.getFPGATimestamp()}; + } Logger.processInputs("Drive/Odometry/Timestamps", m_timestampInputs); m_odometryLock.unlock(); @@ -283,6 +288,7 @@ public void periodic() { double velocity = (modulePositions[moduleIndex].distanceMeters - m_lastModulePositions[moduleIndex].distanceMeters) / dt; + double omega = modulePositions[moduleIndex].angle .minus(m_lastModulePositions[moduleIndex].angle) .getDegrees() @@ -292,12 +298,14 @@ public void periodic() { new SwerveModulePosition( modulePositions[moduleIndex].distanceMeters - m_lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle); + modulePositions[moduleIndex].angle.minus( + m_lastModulePositions[moduleIndex].angle)); m_lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - useUpdate = velocity <= DriveConstants.MAX_LINEAR_SPEED / 0.2 - && omega <= 90 / 0.2; // make not a floating number + // make sure our delta isn't too large + useUpdate = Math.abs(velocity) <= DriveConstants.MAX_LINEAR_SPEED * 5.0 + && Math.abs(omega) <= 1080 * 5.0; // make not a floating number } } @@ -306,13 +314,15 @@ public void periodic() { m_rawGyroRotation = gyroInputs.yawPosition; } else { Twist2d twist = kinematics.toTwist2d(moduleDeltas); - m_rawGyroRotation.plus(new Rotation2d(twist.dtheta)); + m_rawGyroRotation = m_rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } // apply gyro update if (useUpdate) { m_wpiPoseEstimator.updateWithTime(currentTimestamp, m_rawGyroRotation, modulePositions); } + m_lastTimestamp = currentTimestamp; + } // Stop moving when disabled diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java index 9e242ac4..8d62ebf9 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOSim.java @@ -32,7 +32,7 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; private final DCMotorSim driveSim = - new DCMotorSim(DCMotor.getKrakenX60(1), ModuleConstants.GearRatios.L3_KRAKEN.ratio, 0.025); + new DCMotorSim(DCMotor.getKrakenX60(1), ModuleConstants.GearRatios.L3.ratio, 0.025); private final DCMotorSim turnSim = new DCMotorSim(DCMotor.getFalcon500Foc(1), ModuleConstants.GearRatios.TURN.ratio, 0.004); @@ -71,15 +71,15 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) { inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; - inputs.odometryTimestamps = new double[] {Timer.getFPGATimestamp()}; inputs.odometryDrivePositionsMeters = new double[] {inputs.drivePositionMeters}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } @Override public void setDriveVelocityMPS(double mps) { - double rps = (mps / m_moduleConstants.WHEEL_CURCUMFERENCE_METERS()) * m_moduleConstants.DRIVE_GEAR_RATIO(); + double rps = (mps / m_moduleConstants.WHEEL_CURCUMFERENCE_METERS()) * ModuleConstants.GearRatios.L3.ratio; driveAppliedVolts = m_driveController.calculate(driveSim.getAngularVelocityRPM() / 60.0, rps); + driveAppliedVolts += rps * m_moduleConstants.DRIVE_KV(); driveAppliedVolts = MathUtil.clamp(driveAppliedVolts, -12.0, 12.0); driveSim.setInputVoltage(driveAppliedVolts); }