Skip to content

Commit

Permalink
Overall tuning of Autonomous
Browse files Browse the repository at this point in the history 
 - Fixed the TurnPID command to work better in CommandGroups
 - Updated SimplePIController to handle continuous PIDSources better
 - Overhauled format of AutonomousWrapper (Note: 1/4 complete. Will WIP)
 - Other minor tweaks and fixes
  • Loading branch information
@matthew-reynolds
matthew-reynolds committed Jan 29, 2017
1 parent b22bcec commit 57ba007
Show file tree
Hide file tree
Showing 12 changed files with 457 additions and 245 deletions.
3 changes: 3 additions & 0 deletions .gitignore
View file
@@ -1 +1,4 @@
/bin/
/build/
/dist/
/sysProps.xml
10 changes: 4 additions & 6 deletions src/org/usfirst/frc/team4946/robot/OI.java
View file
Expand Up @@ -26,9 +26,7 @@ public class OI {

Button turn0Button = new JoystickButton(driveStick, 6);
Button turn90Button = new JoystickButton(driveStick, 7);
Button spinButton = new JoystickButton(operatorStick, 5);
Button getEncAndGyroValues = new JoystickButton(operatorStick, 3);
Button resetEncAndGyroValues = new JoystickButton(operatorStick, 4);
// Button spinButton = new JoystickButton(operatorStick, 5);
Button drive48in = new JoystickButton(driveStick, 4);
Button maintain0deg = new JoystickButton(driveStick, 1);

Expand All @@ -54,10 +52,10 @@ public class OI {
// button.whenReleased(new ExampleCommand());

public OI() {
spinButton.whileHeld(new IntakeForward());
// spinButton.whileHeld(new IntakeForward());
drive48in.whenPressed(new AutoDriveDistancePID(48, 0.6));
turn0Button.whileHeld(new TurnPID(0.0));
turn90Button.whileHeld(new TurnPID(90.0));
turn0Button.whenPressed(new TurnPID(0.0));
turn90Button.whenPressed(new TurnPID(90.0));
}

public Joystick getOperatorJoystick() {
Expand Down
53 changes: 31 additions & 22 deletions src/org/usfirst/frc/team4946/robot/Robot.java
View file
@@ -1,7 +1,6 @@

package org.usfirst.frc.team4946.robot;

import org.usfirst.frc.team4946.robot.commands.autonomous.AutonomousWrapper;
import org.usfirst.frc.team4946.robot.commands.autonomous.AutonomousWrapperTurningFromBack;
import org.usfirst.frc.team4946.robot.subsystems.BallIntake;
import org.usfirst.frc.team4946.robot.subsystems.DriveTrain;

Expand Down Expand Up @@ -30,29 +29,35 @@ public class Robot extends IterativeRobot {
Command auto;
SendableChooser<Integer> m_autoMode;


/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {

ballSubsystem = new BallIntake();
driveSubsystem = new DriveTrain();

oi = new OI();

m_autoMode = new SendableChooser<Integer>();
m_autoMode.addObject("Left Position - With Shoot", RobotConstants.Auto.LEFT_POSITION_SHOOT);
m_autoMode.addObject("Left Position - No Shoot", RobotConstants.Auto.LEFT_POSITION_NO_SHOOT);
m_autoMode.addObject("Middle Position Breach Left - with Shoot", RobotConstants.Auto.MIDDLE_POSITION_BREACH_LEFT_SHOOT);
m_autoMode.addObject("Middle Position Breach Right - No Shoot", RobotConstants.Auto.MIDDLE_POSITION_BREACH_RIGHT_NO_SHOOT);
m_autoMode.addObject("Middle Position No Breach - With Shoot", RobotConstants.Auto.MIDDLE_POSITION_NO_BREACH_SHOOT);
m_autoMode.addObject("Right Position- No Shoot", RobotConstants.Auto.RIGHT_POSITION_NO_SHOOT);
m_autoMode.addDefault("Left Position",
RobotConstants.Auto.LEFT_POSITION);
m_autoMode.addObject("Right Position",
RobotConstants.Auto.RIGHT_POSITION);
// m_autoMode.addObject("Middle Position Breach Left - with Shoot",
// RobotConstants.Auto.MIDDLE_POSITION_BREACH_LEFT_SHOOT);
// m_autoMode.addObject("Middle Position Breach Right - No Shoot",
// RobotConstants.Auto.MIDDLE_POSITION_BREACH_RIGHT_NO_SHOOT);
// m_autoMode.addObject("Middle Position No Breach - With Shoot",
// RobotConstants.Auto.MIDDLE_POSITION_NO_BREACH_SHOOT);
// m_autoMode.addObject("Right Position- No Shoot",
// RobotConstants.Auto.RIGHT_POSITION_NO_SHOOT);
SmartDashboard.putData("Autonomous - Script", m_autoMode);



driveSubsystem.calibrateGyroscope();
}

/**
Expand Down Expand Up @@ -83,11 +88,11 @@ public void disabledPeriodic() {
*/
@Override
public void autonomousInit() {
boolean isRed = DriverStation.getInstance().getAlliance()==Alliance.Red;

boolean isRed = DriverStation.getInstance().getAlliance() == Alliance.Red;
int autoMode = m_autoMode.getSelected();

auto = new AutonomousWrapper(autoMode, isRed);
auto = new AutonomousWrapperTurningFromBack(autoMode, isRed);
auto.start();

}
Expand All @@ -98,6 +103,10 @@ public void autonomousInit() {
@Override
public void autonomousPeriodic() {
Scheduler.getInstance().run();

SmartDashboard.putNumber("Encoder Distance: ",
driveSubsystem.getEncoderDistance());
SmartDashboard.putNumber("Gyro: ", driveSubsystem.getGyroValue());
}

@Override
Expand All @@ -106,12 +115,12 @@ public void teleopInit() {
// teleop starts running. If you want the autonomous to
// continue until interrupted by another command, remove
// this line or comment it out.

driveSubsystem.calibrateGyroscope();

driveSubsystem.resetEncoders();
driveSubsystem.resetGyro();

if (auto != null)
auto.cancel();
auto.cancel();
}

/**
Expand All @@ -120,9 +129,9 @@ public void teleopInit() {
@Override
public void teleopPeriodic() {
Scheduler.getInstance().run();
SmartDashboard.putNumber("Encoder Distance: ", driveSubsystem.getEncoderDistance());
SmartDashboard.putNumber("Encoder Distance: ",
driveSubsystem.getEncoderDistance());
SmartDashboard.putNumber("Gyro: ", driveSubsystem.getGyroValue());
SmartDashboard.putData("Drive: ", driveSubsystem.getCurrentCommand());

}

Expand Down
13 changes: 7 additions & 6 deletions src/org/usfirst/frc/team4946/robot/RobotConstants.java
View file
Expand Up @@ -14,12 +14,13 @@ public static final class Auto {
// Blue
public static final int LEFT_POSITION = 1;
public static final int RIGHT_POSITION = 2;
public static final int BREACH_NO_SHOOT = 3;
public static final int MIDDLE_POSITION_BREACH_SHOOT = 4;
public static final int MIDDLE_POSITION_BREACH_LEFT = 5;
public static final int MIDDLE_POSITION_BREACH_RIGHT = 6;
public static final int MIDDLE_POSITION_JUST_SHOOT = 7;
public static final int MIDDLE_POSITION_DO_NOTHING = 8;
public static final int SIDE_GEAR_AND_SHOOT = 3;
public static final int BREACH_NO_SHOOT = 4;
public static final int MIDDLE_POSITION_BREACH_SHOOT = 5;
public static final int MIDDLE_POSITION_BREACH_LEFT = 6;
public static final int MIDDLE_POSITION_BREACH_RIGHT = 7;
public static final int MIDDLE_POSITION_JUST_SHOOT = 8;
public static final int MIDDLE_POSITION_DO_NOTHING = 9;

}
}
39 changes: 25 additions & 14 deletions src/org/usfirst/frc/team4946/robot/SimplePIController.java
View file
Expand Up @@ -127,12 +127,19 @@ public double getOutput() {
}

public void setSetpoint(double newSetpoint) {

// Make sure that the setPoint is within the input limits
if (m_maximumInput > m_minimumInput) {
if (m_maximumInput > m_minimumInput && m_isContinuous) {
double range = (m_maximumInput - m_minimumInput);

if (newSetpoint > m_maximumInput) {
m_setpoint = m_maximumInput;
m_setpoint = newSetpoint%range;
// m_setpoint = m_maximumInput;
} else if (newSetpoint < m_minimumInput) {
m_setpoint = m_minimumInput;
while (newSetpoint < m_minimumInput)
newSetpoint += range;
m_setpoint = newSetpoint;
// m_setpoint = m_minimumInput;
} else {
m_setpoint = newSetpoint;
}
Expand All @@ -144,21 +151,21 @@ public void setSetpoint(double newSetpoint) {
public void updateInputVal() {
m_inputVal = m_inputSource.pidGet();

double range = m_maximumInput - m_minimumInput;

// If need be, adjust the input to fit in the input range.
// Rather than saying "If it's too big, set it to the biggest allowed,"
// We want the controller to be able to utilize its continuous
// functionality.
// Therefore, we just re-map the input to fit into the input range using
// the modulus operator.
if (m_isContinuous) {
if (m_inputVal > m_maximumInput) {
m_inputVal = m_inputVal % m_maximumInput;
} else if (m_inputVal < m_minimumInput) {
while (m_inputVal < m_minimumInput) {
m_inputVal += m_maximumInput;
}
m_inputVal = m_inputVal % m_maximumInput;
}

while (m_inputVal < m_minimumInput)
m_inputVal += range;

m_inputVal %= m_maximumInput;

}
}

Expand All @@ -168,9 +175,13 @@ public boolean onTarget() {
return (Math.abs(m_setpoint - m_inputVal) < m_tolerance / 100
* (m_maximumInput - m_minimumInput));


return (Math.abs(m_setpoint - m_inputVal) < m_tolerance); // If the setpoint is in the right spot
//&& integralTerm < ((m_maximumInput - m_minimumInput) / 2.0) / 100.0; // And the I term is less than 1/100 of the output
return (Math.abs(m_setpoint - m_inputVal) < m_tolerance); // If the
// setpoint
// is in the
// right
// spot
// && integralTerm < ((m_maximumInput - m_minimumInput) / 2.0) / 100.0;
// // And the I term is less than 1/100 of the output
}

public double getError() {
Expand Down

0 comments on commit 57ba007

Please sign in to comment.