Skip to content

Commit

Permalink
why did 12.1 work before ?
Browse files Browse the repository at this point in the history
  • Loading branch information
Yxhej committed Feb 3, 2025
1 parent 107e1f8 commit 5cfb014
Show file tree
Hide file tree
Showing 7 changed files with 14 additions and 228 deletions.
213 changes: 1 addition & 212 deletions simgui.json
Original file line number Diff line number Diff line change
@@ -1,237 +1,26 @@
{
"HALProvider": {
"Addressable LEDs": {
"0": {
"columns": 64
},
"window": {
"visible": true
}
},
"Other Devices": {
"window": {
"visible": false
}
}
},
"NTProvider": {
"types": {
"/FMSInfo": "FMSInfo",
"/Faults/Active Faults": "Alerts",
"/Faults/Total Faults": "Alerts",
<<<<<<< HEAD
"/Robot/arm/armCanvas": "Mechanism2d",
"/Robot/arm/fb": "ProfiledPIDController",
=======
>>>>>>> main
"/Robot/autos": "String Chooser",
"/Robot/drive/field2d": "Field2d",
"/Robot/drive/rotationController": "PIDController",
"/Robot/drive/translationController": "ProfiledPIDController",
"/Robot/elevator/measurement/mech": "Mechanism2d",
"/Robot/elevator/pid": "ProfiledPIDController",
"/Robot/elevator/setpoint/mech": "Mechanism2d",
"/SmartDashboard/Alerts": "Alerts",
"/SmartDashboard/Scheduler": "Scheduler",
"/SmartDashboard/VisionSystemSim-main/Sim Field": "Field2d",
<<<<<<< HEAD
=======
"/SmartDashboard/drive dynamic backward": "Command",
"/SmartDashboard/drive dynamic forward": "Command",
"/SmartDashboard/drive quasistatic backward": "Command",
"/SmartDashboard/drive quasistatic forward": "Command",
"/SmartDashboard/pivot dynamic backward": "Command",
"/SmartDashboard/pivot dynamic forward": "Command",
"/SmartDashboard/pivot quasistatic backward": "Command",
"/SmartDashboard/pivot quasistatic forward": "Command",
>>>>>>> main
"/SmartDashboard/rotation dynamic backward": "Command",
"/SmartDashboard/rotation dynamic forward": "Command",
"/SmartDashboard/rotation quasistatic backward": "Command",
"/SmartDashboard/rotation quasistatic forward": "Command",
"/SmartDashboard/translation dynamic backward": "Command",
"/SmartDashboard/translation dynamic forward": "Command",
"/SmartDashboard/translation quasistatic backward": "Command",
"/SmartDashboard/translation quasistatic forward": "Command",
"/SmartDashboard/turn dynamic backward": "Command",
"/SmartDashboard/turn dynamic forward": "Command",
"/SmartDashboard/turn quasistatic backward": "Command",
"/SmartDashboard/turn quasistatic forward": "Command"
},
"windows": {
"/Robot/autos": {
"window": {
"visible": true
}
},
"/Robot/drive/field2d": {
"bottom": 1476,
"height": 8.210550308227539,
"left": 150,
"module- FL": {
"style": "Line"
},
"module- RR": {
"style": "Line"
},
"module-FR": {
"style": "Line"
},
"module-RL": {
"style": "Line"
},
"right": 2961,
"top": 79,
"width": 16.541748046875,
"window": {
"visible": true
}
},
"/Robot/elevator/measurement/mech": {
"window": {
"visible": true
}
},
"/Robot/elevator/setpoint/mech": {
"window": {
"visible": true
}
}
}
},
"NetworkTables": {
"Retained Values": {
"open": false
},
"transitory": {
"Robot": {
"drive": {
"Pose2d##v_/Robot/drive/getPose": {
"Rotation2d##v_rotation": {
"open": true
},
"open": true
},
"Pose2d##v_/Robot/drive/pose": {
"Rotation2d##v_rotation": {
"open": true
},
"Translation2d##v_translation": {
"open": true
},
"open": true
},
"SwerveModulePosition[]##v_/Robot/drive/getModulePositions": {
"SwerveModulePosition##v_[1]": {
"Rotation2d##v_angle": {
"open": true
}
}
},
"SwerveModuleState[]##v_/Robot/drive/getModuleSetpoints": {
"SwerveModuleState##v_[0]": {
"Rotation2d##v_angle": {
"open": true
}
},
"SwerveModuleState##v_[1]": {
"open": true
},
"SwerveModuleState##v_[2]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"SwerveModuleState##v_[3]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"SwerveModuleState[]##v_/Robot/drive/getModuleStates": {
"SwerveModuleState##v_[0]": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"frontRight": {
"SwerveModulePosition##v_/Robot/drive/frontRight/position": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"SwerveModuleState##v_/Robot/drive/frontRight/state": {
"Rotation2d##v_angle": {
"open": true
},
"open": true
},
"open": true
},
"open": true
},
"vision": {
"open": true
}
},
"Shuffleboard": {
"open": true
},
"SmartDashboard": {
"open": true
}
"/SmartDashboard/translation quasistatic forward": "Command"
}
},
"NetworkTables Info": {
"visible": true
},
"Plot": {
"Plot <0>": {
"plots": [
{
"backgroundColor": [
0.0,
0.0,
0.0,
0.8500000238418579
],
<<<<<<< HEAD
"height": 0,
=======
"height": 405,
>>>>>>> main
"series": [
{
"color": [
0.2980392277240753,
0.44705885648727417,
0.6901960968971252,
1.0
],
"id": "NT:/Robot/elevator/positionSetpoint"
},
{
"color": [
0.8666667342185974,
0.5176470875740051,
0.32156863808631897,
1.0
],
"id": "NT:/Robot/elevator/position"
}
]
}
],
"window": {
"name": "Elevator"
}
}
}
}
1 change: 0 additions & 1 deletion src/main/java/org/sciborgs1155/robot/drive/Drive.java
Original file line number Diff line number Diff line change
Expand Up @@ -133,7 +133,6 @@ public static Drive create() {
ANGULAR_OFFSETS.get(3),
"RR",
true));

} else {
return new Drive(
new NoGyro(),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -119,9 +119,7 @@ public static final class FF {
}

static final class Turning {
public static final double GEARING = 12.1; // 0.4 / (12.1 * 1.3);

public static final double POSITION_FACTOR = GEARING / (2 * Math.PI);
public static final double GEARING = 1; // 0.4 / (12.1 * 1.3);

public static final Current CURRENT_LIMIT = Amps.of(20);

Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/sciborgs1155/robot/drive/ModuleIO.java
Original file line number Diff line number Diff line change
Expand Up @@ -86,7 +86,7 @@ public interface ModuleIO extends Logged, AutoCloseable {
*
* @param angle The angle setpoint.
*/
void setTurnSetpoint(double angle);
void setTurnSetpoint(Rotation2d angle);

/**
* Updates controllers based on an optimized desired state and actuates the module accordingly.
Expand Down
2 changes: 1 addition & 1 deletion src/main/java/org/sciborgs1155/robot/drive/NoModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ public void resetEncoders() {}
public void setDriveSetpoint(double velocity) {}

@Override
public void setTurnSetpoint(double angle) {}
public void setTurnSetpoint(Rotation2d angle) {}

@Override
public void updateSetpoint(SwerveModuleState setpoint, ControlMode mode) {}
Expand Down
6 changes: 3 additions & 3 deletions src/main/java/org/sciborgs1155/robot/drive/SimModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -104,8 +104,8 @@ public void setDriveSetpoint(double velocity) {
}

@Override
public void setTurnSetpoint(double setpoint) {
setTurnVoltage(turnFeedback.calculate(rotation().getRadians(), setpoint));
public void setTurnSetpoint(Rotation2d setpoint) {
setTurnVoltage(turnFeedback.calculate(rotation().getRadians(), setpoint.getRadians()));
}

@Override
Expand All @@ -121,7 +121,7 @@ public void updateSetpoint(SwerveModuleState setpoint, ControlMode mode) {
setDriveSetpoint(setpoint.speedMetersPerSecond);
}

setTurnSetpoint(setpoint.angle.getRadians());
setTurnSetpoint(setpoint.angle);
this.setpoint = setpoint;
}

Expand Down
14 changes: 7 additions & 7 deletions src/main/java/org/sciborgs1155/robot/drive/TalonModule.java
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ public class TalonModule implements ModuleIO {
private final StatusSignal<Angle> turnPos;

private final VelocityVoltage velocityOut = new VelocityVoltage(0);
private final PositionVoltage radiansOut = new PositionVoltage(0);
private final PositionVoltage rotationsIn = new PositionVoltage(0);

private final SimpleMotorFeedforward driveFF;

Expand Down Expand Up @@ -86,7 +86,7 @@ public TalonModule(
talonTurnConfig.MotorOutput.Inverted =
invert ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive;

talonTurnConfig.Feedback.SensorToMechanismRatio = Turning.POSITION_FACTOR;
talonTurnConfig.Feedback.SensorToMechanismRatio = Turning.GEARING;
talonTurnConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder;
talonTurnConfig.Feedback.FeedbackRemoteSensorID = sensorID;

Expand All @@ -108,8 +108,8 @@ public TalonModule(
if (success.isOK()) break;
}

register(turnMotor);
register(driveMotor);
register(turnMotor);

TalonUtils.addMotor(driveMotor);
TalonUtils.addMotor(turnMotor);
Expand Down Expand Up @@ -179,8 +179,8 @@ public void setDriveSetpoint(double velocity) {
}

@Override
public void setTurnSetpoint(double angle) {
turnMotor.setControl(radiansOut.withPosition(angle).withSlot(0));
public void setTurnSetpoint(Rotation2d angle) {
turnMotor.setControl(rotationsIn.withPosition(angle.getRotations()).withSlot(0));
}

@Override
Expand All @@ -195,15 +195,15 @@ public void updateSetpoint(SwerveModuleState setpoint, ControlMode mode) {
setDriveSetpoint(setpoint.speedMetersPerSecond);
}

setTurnSetpoint(setpoint.angle.getRotations());
setTurnSetpoint(setpoint.angle);
this.setpoint = setpoint;
}

@Override
public void updateInputs(Rotation2d angle, double voltage) {
setpoint.angle = angle;
setDriveVoltage(voltage);
setTurnSetpoint(angle.getRotations());
setTurnSetpoint(angle);
}

@Override
Expand Down

0 comments on commit 5cfb014

Please sign in to comment.