diff --git a/commands/chassis/SimpleSplines.java b/commands/chassis/SimpleSplines.java index 28443179..967e9aa7 100644 --- a/commands/chassis/SimpleSplines.java +++ b/commands/chassis/SimpleSplines.java @@ -23,21 +23,32 @@ public class SimpleSplines extends SequentialCommandGroup { public SimpleSplines(SplinesDrive robotDrive, Pose2d init_pos, List inter_points, Pose2d final_pos, double maxVoltage, Command nextCommand){ - super(new RamseteCommand( - TrajectoryGenerator.generateTrajectory(init_pos, inter_points, final_pos, new TrajectoryConfig(robotDrive.getAutoConstants().kMaxSpeedMetersPerSecond, - robotDrive.getAutoConstants().kMaxAccelerationMetersPerSecondSquared) - .setKinematics(robotDrive.getDriveConstants().kDriveKinematics) - .addConstraint(new DifferentialDriveVoltageConstraint( - new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts, - robotDrive.getDriveConstants().kvVoltSecondsPerMeter, - robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter), + super( + new RamseteCommand( + TrajectoryGenerator.generateTrajectory(init_pos, inter_points, final_pos, + new TrajectoryConfig( + robotDrive.getAutoConstants().kMaxSpeedMetersPerSecond, + robotDrive.getAutoConstants().kMaxAccelerationMetersPerSecondSquared + ).setKinematics(robotDrive.getDriveConstants().kDriveKinematics + ).addConstraint( + new DifferentialDriveVoltageConstraint( + new SimpleMotorFeedforward( + robotDrive.getDriveConstants().ksVolts, + robotDrive.getDriveConstants().kvVoltSecondsPerMeter, + robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter + ), robotDrive.getDriveConstants().kDriveKinematics, - maxVoltage))), + maxVoltage + ) + ) + ), robotDrive::getPose, new RamseteController(robotDrive.getAutoConstants().kRamseteB, robotDrive.getAutoConstants().kRamseteZeta), - new SimpleMotorFeedforward(robotDrive.getDriveConstants().ksVolts, - robotDrive.getDriveConstants().kvVoltSecondsPerMeter, - robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter), + new SimpleMotorFeedforward( + robotDrive.getDriveConstants().ksVolts, + robotDrive.getDriveConstants().kvVoltSecondsPerMeter, + robotDrive.getDriveConstants().kaVoltSecondsSquaredPerMeter + ), robotDrive.getDriveConstants().kDriveKinematics, robotDrive::getWheelSpeeds, new PIDController(robotDrive.getDriveConstants().kPDriveVel, 0, 0), diff --git a/subsystems/chassis/SplinesDrive.java b/subsystems/chassis/SplinesDrive.java index fcf37937..ddb17d0b 100644 --- a/subsystems/chassis/SplinesDrive.java +++ b/subsystems/chassis/SplinesDrive.java @@ -7,12 +7,10 @@ package org.usfirst.frc4904.standard.subsystems.chassis; -import javax.naming.InitialContext; - import com.ctre.phoenix.sensors.CANCoder; -import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; import org.usfirst.frc4904.standard.commands.chassis.SimpleSplines; +import org.usfirst.frc4904.standard.custom.sensors.CANTalonEncoder; import org.usfirst.frc4904.standard.custom.sensors.IMU; import org.usfirst.frc4904.standard.subsystems.chassis.TankDrive; import org.usfirst.frc4904.standard.subsystems.motor.Motor;