Team4388/2022NoWayHome
1
0
Fork 0
mirror of https://github.com/Team4388/2022NoWayHome.git synced 2026-06-09 00:38:05 -06:00
Code Issues Packages Projects Releases Wiki Activity

vel correction setup

Browse Source
This commit is contained in:
aarav18
2022-03-16 15:35:38 -06:00
parent 38459ba40c
commit 2f23aed7f1
5 changed files with 27 additions and 63 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/commands/ShooterCommands/Shoot.java
+1 -38
View File
@@ -96,7 +96,7 @@ public class Shoot extends CommandBase {
* Updates error for custom PID.
*/
public void updateError() {
m_targetPoint = new Pose2d(hTargetDistanceFromHub(), vTargetDistanceFromHub(), SwerveDriveConstants.HUB_POSE.getRotation());
m_targetPoint = SwerveDriveConstants.HUB_POSE;
m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, driveDummy.get());
// m_targetAngle = AimToCenter.angleToCenter(m_odoX, m_odoY, m_swerve.getRegGyro().getDegrees());
error = (m_targetAngle - turretDummy.get() + 360) % 360;
@@ -152,43 +152,6 @@ public class Shoot extends CommandBase {
output = kP * proportional + kI * integral + kD * derivative;
normOutput = output/360 * inverted;
}
// TODO: horizontal velocity correction
public double hTargetDistanceFromHub() {
double hVel = m_swerve.getChassisSpeeds()[0];
double velBeforeCorrection = m_boomBoom.getVelocity(m_distance);
double vDistanceFromHub = m_odoY;
double approxTravelTime = vDistanceFromHub / velBeforeCorrection;
double hTargetDistanceFromHub = hVel * approxTravelTime;
// return hTargetDistanceFromHub;
return 0.0; // this is for no velocity correction
}
public Pose2d findTargetPoint() {
// position vector and radius
Translation2d position = new Translation2d(m_odoX, m_odoY);
double radius = position.getNorm();
// equation of circle = x^2 + y^2 = m_distance^2
// derivative of circle = 2x + 2y * y' = 0 --> y' = -x/y
// velocity vector (x, y)
Translation2d cartesianVelocity = new Translation2d(m_swerve.getChassisSpeeds()[0], m_swerve.getChassisSpeeds()[1]);
// unit tangential vector
Translation2d tangential = new Translation2d(0, 0);
// velocity vector (tangential, radial)
Translation2d polarVelocity = new Translation2d(0, 0);
return SwerveDriveConstants.HUB_POSE;
}
// TODO: vertical velocity correction
public double vTargetDistanceFromHub() {
return 0.0; // this is for no velocity correction
}
// Called every time the scheduler runs while the command is scheduled.
@Override