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

Revert "Added Trajectory Command"

Browse Source 
This reverts commit 0e5427cb60.
This commit is contained in:
Aarav Shah
2020-02-13 17:17:01 -07:00
parent 0e5427cb60
commit 9ae3ff611f
2 changed files with 1 additions and 64 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+1 -58
View File
@@ -7,24 +7,13 @@
package frc4388.robot;
import java.util.List;
import com.ctre.phoenix.motorcontrol.NeutralMode;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.controller.PIDController;
import edu.wpi.first.wpilibj.controller.RamseteController;
import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
import edu.wpi.first.wpilibj.geometry.Pose2d;
import edu.wpi.first.wpilibj.geometry.Rotation2d;
import edu.wpi.first.wpilibj.geometry.Translation2d;
import edu.wpi.first.wpilibj.trajectory.Trajectory;
import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
import edu.wpi.first.wpilibj2.command.Command;
import edu.wpi.first.wpilibj2.command.InstantCommand;
import edu.wpi.first.wpilibj2.command.RamseteCommand;
import edu.wpi.first.wpilibj2.command.RunCommand;
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
import frc4388.robot.Constants.*;
@@ -132,53 +121,7 @@ public class RobotContainer {
*/
public Command getAutonomousCommand() {
// no auto
var autoVoltageConstraint =
new DifferentialDriveVoltageConstraint(
new SimpleMotorFeedforward( DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
10);
TrajectoryConfig config =
new TrajectoryConfig( DriveConstants.kMaxSpeedMetersPerSecond,
DriveConstants.kMaxAccelerationMetersPerSecondSquared)
.setKinematics(DriveConstants.kDriveKinematics)
.addConstraint(autoVoltageConstraint);
Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
new Pose2d(0, 0, new Rotation2d(0)),
List.of(
new Translation2d(1, 1),
new Translation2d(2, -1)
),
new Pose2d(3, 0, new Rotation2d(0)),
config
);
RamseteCommand ramseteCommand = new RamseteCommand(
exampleTrajectory,
m_robotDrive::getPose,
new RamseteController(DriveConstants.kRamseteB, DriveConstants.kRamseteZeta),
new SimpleMotorFeedforward( DriveConstants.ksVolts,
DriveConstants.kvVoltSecondsPerMeter,
DriveConstants.kaVoltSecondsSquaredPerMeter),
DriveConstants.kDriveKinematics,
m_robotDrive::getWheelSpeeds,
new PIDController(DriveConstants.kPDriveVel, 0, 0),
new PIDController(DriveConstants.kPDriveVel, 0, 0),
m_robotDrive::tankDriveVolts,
m_robotDrive
);
return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
return new InstantCommand();
}
/**
src/main/java/frc4388/robot/subsystems/Drive.java
-6
View File
@@ -508,10 +508,4 @@ public class Drive extends SubsystemBase {
public double ticksToInches(double ticks) {
return ticks * DriveConstants.INCHES_PER_TICK;
}
public void tankDriveVolts(double leftVolts, double rightVolts) {
m_leftFrontMotor.setVoltage(leftVolts);
m_rightFrontMotor.setVoltage(-rightVolts);
m_driveTrain.feedWatchdog();
}
}