challenge-bot.com Git - 3501/2015-FRC-Spark/blob - src/org/usfirst/frc3501/RiceCatRobot/commands/SmoothAccelerateForDistance.java

   1 package org.usfirst.frc3501.RiceCatRobot.commands;
   2
   3 import org.usfirst.frc3501.RiceCatRobot.Robot;
   4 import org.usfirst.frc3501.RiceCatRobot.RobotMap.Direction;
   5 import org.usfirst.frc3501.RiceCatRobot.subsystems.DriveTrain;
   6
   7 import edu.wpi.first.wpilibj.Timer;
   8 import edu.wpi.first.wpilibj.command.Command;
   9
  10 /**
  11  * This command will drive the specified distance at specified speed.
  12  *
  13  */
  14 public class SmoothAccelerateForDistance extends Command {
  15   private double distance; // in units of cm
  16   private double speed;
  17
  18   public SmoothAccelerateForDistance(double distance, double speed) {
  19     // Since negative values in setMotorSpeeds(...) are forwards, we reverse
  20     // speed here so positive input values will move the robot forwards.
  21     this.speed = -speed;
  22
  23     // ensure distance is positive
  24     this.distance = Math.max(distance, -distance);
  25   }
  26
  27   @Override
  28   protected void initialize() {
  29     Robot.driveTrain.resetEncoders();
  30     Robot.driveTrain.setMotorSpeeds(speed, speed);
  31   }
  32
  33   @Override
  34   protected void execute() {
  35     // nothing to do since motor speeds already set
  36   }
  37
  38   @Override
  39   protected boolean isFinished() {
  40     return Math.abs(Robot.driveTrain.getLeftDistance()) >= distance
  41         && Math.abs(Robot.driveTrain.getRightDistance()) >= distance;
  42   }
  43
  44   @Override
  45   protected void end() {
  46     Robot.driveTrain.stop();
  47   }
  48
  49   @Override
  50   protected void interrupted() {
  51     end();
  52   }
  53 }