This repository has been archived by the owner on Oct 16, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Autobalance work + making A1C better--> centerline protection and get…
… last cube fully in.
- Loading branch information
MOE 365 Programming Laptop
authored and
MOE 365 Programming Laptop
committed
Mar 5, 2023
1 parent
4d211c4
commit b43c1c0
Showing
4 changed files
with
157 additions
and
6 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,129 @@ | ||
| package frc.robot.autonomous; | ||
|
|
||
| import edu.wpi.first.wpilibj.Timer; | ||
| import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; | ||
| import frc.robot.generic.GenericRobot; | ||
|
|
||
|
|
||
| public class mobilityEngage extends genericAutonomous{ | ||
| double basePower = 35.0; | ||
| double high = 13.5; | ||
| double climbPower = 30.0; | ||
| double correctionPower = 13.0; | ||
| double desiredPitch = 9.0; | ||
| double firstBreak = 5; | ||
| double dropping = 9; | ||
| double xspd; | ||
| double timerDelta; | ||
| Timer m_timer = new Timer(); | ||
| double xPos; | ||
| @Override | ||
| public void autonomousInit(GenericRobot robot) { | ||
| autonomousStep = 0; | ||
| timerDelta = 0; | ||
| climbPower = 30; | ||
| basePower = 35.0; | ||
| correctionPower = 13.0; | ||
| robot.setPigeonYaw(-90); | ||
| robot.resetStartDists(); | ||
| robot.resetAttitude(); | ||
| robot.resetStartPivots(); | ||
| robot.resetStartHeading(); | ||
| robot.setPose(); | ||
| } | ||
|
|
||
| @Override | ||
| public void autonomousPeriodic(GenericRobot robot) { | ||
| SmartDashboard.putNumber("autonomousStep", autonomousStep); | ||
|
|
||
| double currPitch = -robot.getRoll(); | ||
| switch (autonomousStep) { | ||
| case 0: | ||
| xspd = basePower; | ||
| if (Math.abs(currPitch) > firstBreak) { | ||
|
|
||
| //Add length of the robot from front encoder to end of back wheel. | ||
| autonomousStep++; | ||
| } | ||
| break; | ||
| case 1: | ||
| xspd = basePower; | ||
| if (currPitch > high) { | ||
| xPos = robot.getPose().getX(); | ||
| autonomousStep++; | ||
| } | ||
| break; | ||
| case 2: | ||
| xspd = climbPower; | ||
| if (Math.abs(currPitch) < dropping) { | ||
| autonomousStep++; | ||
| } | ||
| break; | ||
| case 3: | ||
| xspd = climbPower; | ||
| if (Math.abs(currPitch) > high){ | ||
| autonomousStep ++; | ||
| } | ||
| break; | ||
| case 4: | ||
| xspd = climbPower; | ||
| if (Math.abs(currPitch) < desiredPitch){ | ||
| xPos = robot.getPose().getX(); | ||
| autonomousStep ++; | ||
| } | ||
| break; | ||
| case 5: | ||
| xspd = climbPower; | ||
| if (Math.abs(robot.getPose().getX() - xPos) > 36){ | ||
| xspd = 0; | ||
| autonomousStep ++; | ||
| } | ||
| break; | ||
| case 6: | ||
| climbPower = -30; | ||
| basePower = -35.0; | ||
| correctionPower = -13.0; | ||
| autonomousStep ++; | ||
| break; | ||
| case 7: | ||
| xspd = basePower; | ||
| if (Math.abs(currPitch) > high) { | ||
| xPos = robot.getPose().getX(); | ||
| autonomousStep++; | ||
| } | ||
| break; | ||
| case 8: | ||
| if(Math.abs(robot.getPose().getX() - xPos) >= 32){ | ||
| climbPower = -13; | ||
| } | ||
| xspd = climbPower; | ||
| if (Math.abs(currPitch) < dropping) { | ||
| autonomousStep++; | ||
| } | ||
| break; | ||
| case 9: | ||
| xspd = -correctionPower; | ||
| m_timer.reset(); | ||
| m_timer.start(); | ||
| //This is a future feature to stop and let others get on before autobalancing. | ||
| autonomousStep++; | ||
| break; | ||
| case 10: | ||
| if ((currPitch < -desiredPitch) && (m_timer.get() <1)) { //correcting begins | ||
| timerDelta = m_timer.get(); | ||
| xspd = -correctionPower; //backward | ||
| } else if ((currPitch > desiredPitch) && (m_timer.get() <1)) { | ||
| timerDelta = m_timer.get(); | ||
| xspd = correctionPower;//forward | ||
| } else { | ||
| xspd = 0; | ||
| if (m_timer.get()-timerDelta > .25) { | ||
| m_timer.reset(); | ||
| m_timer.start(); | ||
| } | ||
| } | ||
| break; | ||
| } | ||
| robot.setDrive(xspd, 0, 0, true); | ||
| } | ||
| } |