From a7ab6c3b55f70cacc6ff1b7e4ac8b765ff89b90d Mon Sep 17 00:00:00 2001 From: rice31415 Date: Thu, 3 Jan 2019 20:49:58 -0800 Subject: [PATCH 1/2] Initial Upload The isFinished() method of the Raise Elevator command now depends on the upper limit switch status. --- .../frc4079/RobotBuilderProject1/commands/RaiseElevator.java | 3 ++- .../frc4079/RobotBuilderProject1/subsystems/Elevator.java | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/RaiseElevator.java b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/RaiseElevator.java index 67da712..8824277 100644 --- a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/RaiseElevator.java +++ b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/RaiseElevator.java @@ -52,7 +52,8 @@ protected void execute() { // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return isTimedOut(); + return Robot.elevator.limitSwitchUp.get(); + } // Called once after isFinished returns true diff --git a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/subsystems/Elevator.java b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/subsystems/Elevator.java index 3d93229..f47c31f 100644 --- a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/subsystems/Elevator.java +++ b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/subsystems/Elevator.java @@ -32,7 +32,7 @@ public class Elevator extends Subsystem { // BEGIN AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS // private final WPI_TalonSRX motor = RobotMap.elevatorMotor; - private final DigitalInput limitSwitchUp = RobotMap.elevatorLimitSwitchUp; + public final DigitalInput limitSwitchUp = RobotMap.elevatorLimitSwitchUp; private final DigitalInput limitSwitchDown = RobotMap.elevatorLimitSwitchDown; // END AUTOGENERATED CODE, SOURCE=ROBOTBUILDER ID=DECLARATIONS From 2128910b920b18c5f78974b32238850324885e89 Mon Sep 17 00:00:00 2001 From: rice31415 Date: Fri, 4 Jan 2019 15:31:00 -0800 Subject: [PATCH 2/2] Added modified DriveDistance commands Added two DriveDistance commands to check for average of ten previous error checks and check for if the given amount of sample errors fall under a given threshold. --- .../commands/DriveDistance1.java | 71 +++++++++++++++++++ .../commands/DriveDistance2.java | 71 +++++++++++++++++++ 2 files changed, 142 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance1.java create mode 100644 src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance2.java diff --git a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance1.java b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance1.java new file mode 100644 index 0000000..2dc61df --- /dev/null +++ b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance1.java @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4079.RobotBuilderProject1.commands; + +import edu.wpi.first.wpilibj.command.Command; +import org.usfirst.frc4079.RobotBuilderProject1.Robot; + +public class DriveDistance1 extends Command { + private double distance, threshold; + private double leftPos, rightPos; + private double error; + private double[] errors; + private int counter; + private double avg; + public DriveDistance1(double dist, double duration, double thresh) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + distance = dist; + threshold = thresh; + error = 0; + errors = new double[10]; + counter = 0; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + leftPos = Robot.driveTrain.getLeftDistance(); + rightPos = Robot.driveTrain.getRightDistance(); + Robot.driveTrain.configureForDrive(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.driveTrain.driveToTarget(leftPos+distance, rightPos-distance); + error = (leftPos + distance) - Robot.driveTrain.getLeftDistance(); + error = errors[counter]; + if (counter == errors.length - 1) { + counter = 0; + } + else counter++; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + for (int i = 0; i < errors.length; i++) { + avg += errors[i]; + } + avg /= errors.length; + return (avg < threshold); + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} diff --git a/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance2.java b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance2.java new file mode 100644 index 0000000..d7f4f35 --- /dev/null +++ b/src/main/java/org/usfirst/frc4079/RobotBuilderProject1/commands/DriveDistance2.java @@ -0,0 +1,71 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 2018 FIRST. All Rights Reserved. */ +/* Open Source Software - may be modified and shared by FRC teams. The code */ +/* must be accompanied by the FIRST BSD license file in the root directory of */ +/* the project. */ +/*----------------------------------------------------------------------------*/ + +package org.usfirst.frc4079.RobotBuilderProject1.commands; + +import edu.wpi.first.wpilibj.command.Command; +import org.usfirst.frc4079.RobotBuilderProject1.Robot; + +public class DriveDistance2 extends Command { + private double distance, threshold; + private double leftPos, rightPos; + private double error; + private double[] errors; + private int counter; + public DriveDistance2(double dist, double duration, double thresh, int sampleNumber) { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.driveTrain); + distance = dist; + threshold = thresh; + error = 0; + errors = new double[sampleNumber]; + counter = 0; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + leftPos = Robot.driveTrain.getLeftDistance(); + rightPos = Robot.driveTrain.getRightDistance(); + Robot.driveTrain.configureForDrive(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.driveTrain.driveToTarget(leftPos+distance, rightPos-distance); + error = (leftPos + distance) - Robot.driveTrain.getLeftDistance(); + error = errors[counter]; + if (counter == errors.length - 1) { + counter = 0; + } + else counter++; + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + for (int i = 0; i < errors.length; i++) { + if (errors[i] > threshold) { + return false; + } + } + return true; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +}