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

GotoCoordinate Works, Trying to Implement TurnAngle

Browse Source
This commit is contained in:
Aarav Shah
2020-02-28 21:25:09 -07:00
parent a70983a4c2
commit 237eaeb7ef
3 changed files with 13 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/main/java/frc4388/robot/RobotContainer.java
+5 -5
View File
@@ -125,7 +125,7 @@ public class RobotContainer {
// Y driver test button
new JoystickButton(getDriverJoystick(), XboxController.Y_BUTTON)
.whenPressed(new GotoCoordinates(m_robotDrive, 12, 40));
.whenPressed(new GotoCoordinates(m_robotDrive, 12, 12, 0));
// X driver test button
new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON)
@@ -192,10 +192,10 @@ public class RobotContainer {
//Runs an Autonomous command group that would shoot our preloaded balls, pick up 3 more from the trench, and shoot those
//This assumes that we are positioned against the right wall with our shooter facing the target.
return new SequentialCommandGroup(new Wait(2, m_robotDrive),
return new SequentialCommandGroup(new Wait(0, m_robotDrive),
//add aim command
//add shooter command
new DriveStraightToPositionMM(m_robotDrive, 48.0),
//new DriveStraightToPositionMM(m_robotDrive, 48.0),
/*new ParallelCommandGroup(
new StorageIntakeGroup(m_robotIntake, m_robotStorage),
new DriveStraightToPositionMM(m_robotDrive, 36.0)),
@@ -207,8 +207,8 @@ public class RobotContainer {
//add shooter command
//Below this would be the picking up additional balls outside of those in the trench directly behind us
new TurnDegrees(m_robotDrive, -90),
new DriveStraightToPositionMM(m_robotDrive, 96.0));//,
//new GotoCoordinates(m_robotDrive, 36, 36),
new GotoCoordinates(m_robotDrive, 36, 36, 0));//,
//new StorageIntakeGroup(m_robotIntake, m_robotStorage),
//new TurnDegrees(m_robotDrive, 75),
//new DriveStraightToPositionMM(m_robotDrive, 18.0),
src/main/java/frc4388/robot/commands/GotoCoordinates.java
+6 -4
View File
@@ -20,13 +20,12 @@ public class GotoCoordinates extends SequentialCommandGroup {
double m_yTarget;
double m_currentAngle;
double m_hypotDist;
double m_lastAngle;
double m_endAngle;
/**
* Creates a new GotoPosition.
*/
public GotoCoordinates(Drive subsystem, double xTarget, double yTarget) {
public GotoCoordinates(Drive subsystem, double xTarget, double yTarget, double endAngle) {
// Add your commands in the super() call, e.g.
// super(new FooCommand(), new BarCommand());
m_drive = subsystem;
@@ -37,9 +36,12 @@ public class GotoCoordinates extends SequentialCommandGroup {
m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget));
m_currentAngle = calcAngle();
m_endAngle = endAngle;
addCommands(new TurnDegrees(m_drive, m_currentAngle), new DriveStraightToPositionMM(m_drive, m_hypotDist));
addCommands( new TurnDegrees(m_drive, m_currentAngle),
new DriveStraightToPositionMM(m_drive, m_hypotDist),
new TurnDegrees(m_drive, m_endAngle - m_currentAngle - 90));
}
public boolean isQuadrantThree() {
src/main/java/frc4388/robot/subsystems/Drive.java
+2 -2
View File
@@ -727,8 +727,8 @@ public class Drive extends SubsystemBase {
//SmartDashboard.putNumber("Left Back Output", m_leftBackMotor.get());
//SmartDashboard.putNumber("Right Back Output", m_rightBackMotor.get());
double leftRPM = m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity();
double rightRPM = m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity();
double leftRPM = ticksToInches(m_leftFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
double rightRPM = ticksToInches(m_rightFrontMotor.getSensorCollection().getIntegratedSensorVelocity());
SmartDashboard.putNumber("Left Motor RPM", leftRPM);
SmartDashboard.putNumber("Right Motor RPM", rightRPM);