From d46cf97a48bff4fdcb100dd3dd32fc254114a42e Mon Sep 17 00:00:00 2001 From: Emma0044 <43865856+Emma0044@users.noreply.github.com> Date: Sat, 25 Jan 2020 14:08:34 -0500 Subject: [PATCH] Drive distance auto (#6) * added drive distance command * added pid control to drive distance auto * finished driveDistance command * created TurnToAngle auto command. * added groups to auto call. * changed angle kP value. Co-authored-by: PJ Reiniger --- 2020InfiniteRecharge/snobotsim/SnobotSim.json | 2 +- .../main/java/frc/robot/RobotContainer.java | 15 ++++- .../robot/commands/autonomous/AutoShoot.java | 3 +- .../commands/autonomous/DriveDistance.java | 59 +++++++++++++++++++ .../commands/autonomous/TurnToAngle.java | 59 +++++++++++++++++++ 5 files changed, 135 insertions(+), 3 deletions(-) create mode 100644 2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/DriveDistance.java create mode 100644 2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/TurnToAngle.java diff --git a/2020InfiniteRecharge/snobotsim/SnobotSim.json b/2020InfiniteRecharge/snobotsim/SnobotSim.json index 8b67f0e6..4c619cf6 100644 --- a/2020InfiniteRecharge/snobotsim/SnobotSim.json +++ b/2020InfiniteRecharge/snobotsim/SnobotSim.json @@ -17,7 +17,7 @@ "com.snobot.simulator:snobot_sim_joysticks:${version_number}", "com.snobot.simulator:snobot_sim_utilities:${version_number}", - "edu.wpi.first.ntcore:ntcore-jni:${wpilibVersion}:${nativeclassifier}" + "edu.wpi.first.ntcore:ntcore-jni:${wpilibVersion}:${nativeclassifier}", "edu.wpi.first.hal:hal-jni:${wpilibVersion}:${nativeclassifier}" ], "jni": [ diff --git a/2020InfiniteRecharge/src/main/java/frc/robot/RobotContainer.java b/2020InfiniteRecharge/src/main/java/frc/robot/RobotContainer.java index f603221e..4861e876 100644 --- a/2020InfiniteRecharge/src/main/java/frc/robot/RobotContainer.java +++ b/2020InfiniteRecharge/src/main/java/frc/robot/RobotContainer.java @@ -8,7 +8,9 @@ package frc.robot; import frc.robot.commands.DriveByJoystick; +import frc.robot.commands.autonomous.DriveDistance; import frc.robot.commands.autonomous.TimedDriveStraight; +import frc.robot.commands.autonomous.TurnToAngle; import frc.robot.subsystems.Chassis; import frc.robot.subsystems.ControlPanel; import frc.robot.subsystems.Limelight; @@ -16,6 +18,7 @@ import frc.robot.subsystems.ShooterConveyor; import frc.robot.subsystems.ShooterIntake; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -60,6 +63,16 @@ public RobotContainer() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new TimedDriveStraight(chassis, 5, 10); + SequentialCommandGroup group = new SequentialCommandGroup(); + group.addCommands(new DriveDistance(chassis, 100, 1)); + group.addCommands(new TurnToAngle(chassis, 90, 3)); + group.addCommands(new DriveDistance(chassis, 50, 1)); + group.addCommands(new TurnToAngle(chassis, 45, 3)); + group.addCommands(new DriveDistance(chassis, 25, 1)); + + return group; + //return new TimedDriveStraight(chassis, 5, 10); + //return new DriveDistance(chassis, 100, 1); + //return new TurnToAngle(chassis, 90, 5); } } diff --git a/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/AutoShoot.java b/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/AutoShoot.java index 94fcfd1a..bec80cbe 100644 --- a/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/AutoShoot.java +++ b/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/AutoShoot.java @@ -4,9 +4,10 @@ public class AutoShoot extends CommandBase { + public AutoShoot() { // Use requires() here to declare subsystem dependencies - //super.addRequirements(Shooter); When a subsystem is written, add the requires line back in. + //super.addRequirements(Shooter); When a subsystem is written, add the requires line back in. } public void initialize(){ diff --git a/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/DriveDistance.java b/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/DriveDistance.java new file mode 100644 index 00000000..05c38a0e --- /dev/null +++ b/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/DriveDistance.java @@ -0,0 +1,59 @@ +package frc.robot.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Chassis; + +public class DriveDistance extends CommandBase { + + Chassis chassis; + + private double m_distance; + private double m_allowableError; + private double m_error; + + private double AUTO_KP = 0.1; + + public DriveDistance(Chassis chassis, double distance, double allowableError) { + // Use requires() here to declare subsystem dependencies + //super.addRequirements(Shooter); When a subsystem is written, add the requires line back in. + this.chassis = chassis; + + m_distance = distance + chassis.getAverageEncoderDistance(); + m_allowableError = allowableError; + } + + public void initialize(){ + } + + // Called repeatedly when this Command is scheduled to run + public void execute() { + double currentDistance; + + currentDistance = chassis.getAverageEncoderDistance(); + + m_error = m_distance - currentDistance; + + double speed = m_error * AUTO_KP; + + chassis.setSpeed(speed); + + //System.out.println("error:" + m_error + "speed:" + speed); + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + if(Math.abs(m_error) < m_allowableError){ + System.out.println("Done!"); + return true; + } + else { + System.out.println("drive to distance" + "error:" + m_error + "allowableError" + m_allowableError); + return false; + } + } + + // Called once after isFinished returns true + public void end(boolean interrupted) { + chassis.setSpeed(0); + } +} \ No newline at end of file diff --git a/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/TurnToAngle.java b/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/TurnToAngle.java new file mode 100644 index 00000000..df485ddd --- /dev/null +++ b/2020InfiniteRecharge/src/main/java/frc/robot/commands/autonomous/TurnToAngle.java @@ -0,0 +1,59 @@ +package frc.robot.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Chassis; + +public class TurnToAngle extends CommandBase { + + Chassis chassis; + + private double m_angle; + private double m_allowableError; + private double m_error; + + private double AUTO_KP = 0.05; + + public TurnToAngle(Chassis chassis, double angle, double allowableError) { + // Use requires() here to declare subsystem dependencies + //super.addRequirements(Shooter); When a subsystem is written, add the requires line back in. + this.chassis = chassis; + + m_angle = angle + chassis.getHeading(); + m_allowableError = allowableError; + } + + public void initialize(){ + } + + // Called repeatedly when this Command is scheduled to run + public void execute() { + double currentAngle; + + currentAngle = chassis.getHeading(); + + m_error = m_angle - currentAngle; + + double turnSpeed = m_error * AUTO_KP; + + chassis.setSpeedAndSteer(0, turnSpeed); + + //System.out.println("error:" + m_error + "speed:" + speed); + } + + // Make this return true when this Command no longer needs to run execute() + public boolean isFinished() { + if(Math.abs(m_error) < m_allowableError){ + System.out.println("Done!"); + return true; + } + else { + System.out.println("Turn to angle" + "error:" + m_error + "allowableError" + m_allowableError); + return false; + } + } + + // Called once after isFinished returns true + public void end(boolean interrupted) { + chassis.setSpeed(0); + } +} \ No newline at end of file