--- /dev/null
+* todo
+ * figure out where the frame perimeter starts, and where the arm is mounted
+ relative to that
+ * find out actual arm and hand length, as well as the height at which the arm
+ is mounted
+ * for getArmHorizontalDisplacement figure out if measurements are all in
+ inches
+* doing
+
+* done
+ * code review
+ * change arm and hand to "armHorizontal/armVertical" and "handHorizontal/
+ handVertical"
+ * change method "getArmHorizontalDist" to "getArmHorizontalDisplacement"
+ * change method "getArmHeight" to "getArmVerticalDisplacement"
+
\ No newline at end of file
*/
public class Constants {
- public static class OI {
- // Computer Ports
- public final static int LEFT_STICK_PORT = 0;
- public final static int RIGHT_STICK_PORT = 1;
- // Ports on the Joystick
- public final static int TRIGGER_PORT = 1;
- public final static int DECREMENT_SHOOTER_SPEED_PORT = 2;
- public final static int INCREMENT_SHOOTER_SPEED_PORT = 3;
- public final static int SHOOT_PORT = 4;
- public final static int LOG_PORT = 5;
- }
+ public static class OI {
+ // Computer Ports
+ public final static int LEFT_STICK_PORT = 0;
+ public final static int RIGHT_STICK_PORT = 1;
+ // Ports on the Joystick
+ public final static int TRIGGER_PORT = 1;
+ public final static int DECREMENT_SHOOTER_SPEED_PORT = 2;
+ public final static int INCREMENT_SHOOTER_SPEED_PORT = 3;
+ public final static int SHOOT_PORT = 4;
+ public final static int LOG_PORT = 5;
+ }
- public static class DriveTrain {
- // Drivetrain Motor Related Ports
- public static final int FRONT_LEFT = 0;
- public static final int FRONT_RIGHT = 0;
- public static final int REAR_LEFT = 0;
- public static final int REAR_RIGHT = 0;
+ public static class DriveTrain {
+ // Drivetrain Motor Related Ports
+ public static final int FRONT_LEFT = 0;
+ public static final int FRONT_RIGHT = 0;
+ public static final int REAR_LEFT = 0;
+ public static final int REAR_RIGHT = 0;
- // Encoder related ports
- public final static int ENCODER_LEFT_A = 3;
- public final static int ENCODER_LEFT_B = 4;
- public final static int ENCODER_RIGHT_A = 2;
- public final static int ENCODER_RIGHT_B = 1;
- }
+ // Encoder related ports
+ public final static int ENCODER_LEFT_A = 3;
+ public final static int ENCODER_LEFT_B = 4;
+ public final static int ENCODER_RIGHT_A = 2;
+ public final static int ENCODER_RIGHT_B = 1;
- public static class Scaler {
- // Piston channels
- public final static int FORWARD_CHANNEL = 0;
- public final static int REVERSE_CHANNEL = 0;
+ private final static double WHEEL_DIAMETER = 6.0; // in inches
+ private final static double PULSES_PER_ROTATION = 256; // in pulses
+ private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
+ private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
+ public final static double INCHES_PER_PULSE = (((Math.PI) * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION)
+ / WHEEL_SPROCKET_DIAMETER) * WHEEL_DIAMETER;
+ }
- // Winch port
- public final static int WINCH_MOTOR = 0;
- }
+ public static class Scaler {
+ // Piston channels
+ public final static int FORWARD_CHANNEL = 0;
+ public final static int REVERSE_CHANNEL = 0;
- public static class Shooter {
- public static final int PORT = 0;
- public static final int PUNCH_FORWARD_PORT = 0;
- public static final int PUNCH_REVERSE_PORT = 1;
- public static final int ANGLE_ADJUSTER_PORT = 0;
+ // Winch port
+ public final static int WINCH_MOTOR = 0;
+ }
- public static final DoubleSolenoid.Value punch = DoubleSolenoid.Value.kForward;
- public static final DoubleSolenoid.Value retract = DoubleSolenoid.Value.kReverse;
+ public static class Shooter {
+ public static final int PORT = 0;
+ public static final int PUNCH_FORWARD_PORT = 0;
+ public static final int PUNCH_REVERSE_PORT = 1;
+ public static final int ANGLE_ADJUSTER_PORT = 0;
- // Encoder port
- public static final int ENCODER_PORT_A = 0;
- public static final int ENCODER_PORT_B = 0;
+ public static final DoubleSolenoid.Value punch = DoubleSolenoid.Value.kForward;
+ public static final DoubleSolenoid.Value retract = DoubleSolenoid.Value.kReverse;
- public static enum State {
- RUNNING, STOPPED;
- }
- }
+ // Encoder port
+ public static final int ENCODER_PORT_A = 0;
+ public static final int ENCODER_PORT_B = 0;
- public static class IntakeArm {
- public static final int ROLLER_PORT = 0;
- public static final int INTAKE_PORT = 1;
- public static final int INTAKE_CHANNEL = 0;
- public static final double INTAKE_SPEED = 0.5;
- public static final double OUTPUT_SPEED = -0.5;
- public final static double FULL_RANGE = 270.0; // in degrees
- public final static double OFFSET = -135.0; // in degrees
- }
+ public static enum State {
+ RUNNING, STOPPED;
+ }
+ }
- public static class DefenseArm {
- // Potentiometer related ports
- public static final int ARM_CHANNEL = 0;
- public static final int ARM_PORT = 0;
- public static final int HAND_PORT = 1;
- public static final int HAND_CHANNEL = 1;
- public final static double FULL_RANGE = 270.0; // in degrees
- public final static double OFFSET = -135.0; // in degrees
- public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3 level
- }
+ public static class IntakeArm {
+ public static final int ROLLER_PORT = 0;
+ public static final int ARM_PORT = 1;
+ public static final int POT_CHANNEL = 0;
+ public static final double INTAKE_SPEED = 0.5;
+ public static final double OUTPUT_SPEED = -0.5;
+ public final static double FULL_RANGE = 270.0; // in degrees
+ public final static double OFFSET = -135.0; // in degrees
+ }
- public enum Defense {
- PORTCULLIS, SALLY_PORT, ROUGH_TERRAIN, LOW_BAR, CHEVAL_DE_FRISE, DRAWBRIDGE, MOAT, ROCK_WALL, RAMPART;
- }
+ public static class DefenseArm {
+ // Potentiometer related ports
+ public static final int ARM_CHANNEL = 0;
+ public static final int ARM_PORT = 0;
+ public static final int HAND_PORT = 1;
+ public static final int HAND_CHANNEL = 1;
+ public final static double FULL_RANGE = 270.0; // in degrees
+ public final static double OFFSET = -135.0; // in degrees
+ public final static double[] armPotValue = { 0.0, 45.0, 90.0 }; // 3
+ // level
+ public final static double ARM_LENGTH = 0; // TODO: find actual length
+ public final static double HAND_LENGTH = 0; // TODO: find actual length
+ public final static double ARM_MOUNTED_HEIGHT = 0; // TODO: find actual
+ // height
+ }
+
+ public enum Defense {
+ PORTCULLIS, SALLY_PORT, ROUGH_TERRAIN, LOW_BAR, CHEVAL_DE_FRISE, DRAWBRIDGE, MOAT, ROCK_WALL, RAMPART;
+ }
}
--- /dev/null
+package org.usfirst.frc.team3501.robot;
+
+import java.util.TimerTask;
+
+import edu.wpi.first.wpilibj.I2C;
+import edu.wpi.first.wpilibj.I2C.Port;
+import edu.wpi.first.wpilibj.PIDSource;
+import edu.wpi.first.wpilibj.PIDSourceType;
+import edu.wpi.first.wpilibj.Timer;
+
+public class Lidar implements PIDSource {
+ private I2C i2c;
+ private byte[] distance;
+ private java.util.Timer updater;
+
+ private final int LIDAR_ADDR = 0x62;
+ private final int LIDAR_CONFIG_REGISTER = 0x00;
+ private final int LIDAR_DISTANCE_REGISTER = 0x8f;
+
+ public Lidar(Port port) {
+ i2c = new I2C(port, LIDAR_ADDR);
+
+ distance = new byte[2];
+
+ updater = new java.util.Timer();
+ }
+
+ // Distance in cm
+ public int getDistance() {
+ return (int) Integer.toUnsignedLong(distance[0] << 8)
+ + Byte.toUnsignedInt(distance[1]);
+ }
+
+ @Override
+ public double pidGet() {
+ return getDistance();
+ }
+
+ // Start 10Hz polling
+ public void start() {
+ updater.scheduleAtFixedRate(new LIDARUpdater(), 0, 100);
+ }
+
+ // Start polling for period in milliseconds
+ public void start(int period) {
+ updater.scheduleAtFixedRate(new LIDARUpdater(), 0, period);
+ }
+
+ public void stop() {
+ updater.cancel();
+ updater = new java.util.Timer();
+ }
+
+ // Update distance variable
+ public void update() {
+ i2c.write(LIDAR_CONFIG_REGISTER, 0x04); // Initiate measurement
+ Timer.delay(0.04); // Delay for measurement to be taken
+ i2c.read(LIDAR_DISTANCE_REGISTER, 2, distance); // Read in measurement
+ Timer.delay(0.005); // Delay to prevent over polling
+ }
+
+ // Timer task to keep distance updated
+ private class LIDARUpdater extends TimerTask {
+ @Override
+ public void run() {
+ while (true) {
+ update();
+ try {
+ Thread.sleep(10);
+ } catch (InterruptedException e) {
+ e.printStackTrace();
+ }
+ }
+ }
+ }
+
+ @Override
+ public void setPIDSourceType(PIDSourceType pidSource) {
+ }
+
+ @Override
+ public PIDSourceType getPIDSourceType() {
+ return null;
+ }
+}
import org.usfirst.frc.team3501.robot.Constants.Defense;
import org.usfirst.frc.team3501.robot.subsystems.DefenseArm;
import org.usfirst.frc.team3501.robot.subsystems.DriveTrain;
+import org.usfirst.frc.team3501.robot.subsystems.IntakeArm;
import org.usfirst.frc.team3501.robot.subsystems.Scaler;
import org.usfirst.frc.team3501.robot.subsystems.Shooter;
public static DriveTrain driveTrain;
public static Shooter shooter;
public static Scaler scaler;
- public static DefenseArm defenseArm;
+ public static IntakeArm intakeArm;
// Sendable Choosers send a drop down menu to the Smart Dashboard.
SendableChooser positionChooser;
SendableChooser positionOneDefense, positionTwoDefense, positionThreeDefense,
- positionFourDefense, positionFiveDefense;
+ positionFourDefense,
+ positionFiveDefense;
@Override
public void robotInit() {
oi = new OI();
shooter = new Shooter();
scaler = new Scaler();
- defenseArm = new DefenseArm();
+ intakeArm = new IntakeArm();
- // Sendable Choosers allows the driver to select the position of the robot
+ // Sendable Choosers allows the driver to select the position of the
+ // robot
// and the positions of the defenses from a drop-down menu on the Smart
// Dashboard
// make the Sendable Choosers
SmartDashboard.putData("Position Two Defense Chooser", positionTwoDefense);
SmartDashboard.putData("Position Three Defense Chooser",
positionThreeDefense);
- SmartDashboard.putData("Position Four Defense Chooser",
- positionFourDefense);
- SmartDashboard.putData("Position Five Defense Chooser",
- positionFiveDefense);
+ SmartDashboard
+ .putData("Position Four Defense Chooser", positionFourDefense);
+ SmartDashboard
+ .putData("Position Five Defense Chooser", positionFiveDefense);
}
@Override
+++ /dev/null
-package org.usfirst.frc.team3501.robot.commands;
-
-import edu.wpi.first.wpilibj.command.Command;
-
-public class MoveIntakeArm extends Command {
-
- public MoveIntakeArm() {
- }
-
- @Override
- protected void initialize() {
- }
-
- @Override
- protected void execute() {
- }
-
- @Override
- protected boolean isFinished() {
- return false;
- }
-
- @Override
- protected void end() {
- }
-
- @Override
- protected void interrupted() {
- }
-}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands;
+
+import org.usfirst.frc.team3501.robot.Robot;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveIntakeArmToAngle extends Command {
+ private double currentAngle;
+ private double targetAngle;
+ private double targetSpeed;
+ private double SENSITIVITY_THRESHOLD = 0.1;
+ private boolean isDecreasing = false;
+
+ public MoveIntakeArmToAngle(double angle, double speed) {
+ requires(Robot.intakeArm);
+ targetAngle = angle;
+ targetSpeed = speed;
+
+ }
+
+ @Override
+ protected void initialize() {
+ currentAngle = Robot.intakeArm.getArmAngle();
+ double difference = targetAngle - currentAngle;
+ if (difference > 0) {
+ Robot.intakeArm.setArmSpeed(targetSpeed);
+ isDecreasing = true;
+ } else {
+ Robot.intakeArm.setArmSpeed(-targetSpeed);
+ isDecreasing = false;
+ }
+ }
+
+ @Override
+ protected void execute() {
+
+ }
+
+ @Override
+ protected boolean isFinished() {
+ currentAngle = Robot.intakeArm.getArmAngle();
+
+ if (isDecreasing == true) {
+ return (currentAngle <= targetAngle + SENSITIVITY_THRESHOLD);
+ } else {
+ return (currentAngle >= targetAngle + SENSITIVITY_THRESHOLD);
+ }
+ }
+
+ @Override
+ protected void end() {
+ Robot.intakeArm.stop();
+ }
+
+ @Override
+ protected void interrupted() {
+ }
+
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands;
+
+public class MoveIntakeArmToBallIntakeHeight {
+
+}
--- /dev/null
+package org.usfirst.frc.team3501.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+
+public class MoveIntakeArmUp extends Command {
+ public MoveIntakeArmUp() {
+ }
+
+ @Override
+ protected void initialize() {
+ }
+
+ @Override
+ protected void execute() {
+ }
+
+ @Override
+ protected boolean isFinished() {
+ return false;
+ }
+
+ @Override
+ protected void end() {
+ }
+
+ @Override
+ protected void interrupted() {
+ }
+
+}
private double footHeight;
private double[] potHandAngles;
private double[] potArmAngles;
+
// angles corresponding to pre-determined heights we will need
public DefenseArm() {
defenseHand.set(speed);
}
+ // TODO: figure out if measurements are all in inches
+ public double getArmHorizontalDisplacement() {
+ double armHorizontalDisplacement = Constants.DefenseArm.ARM_LENGTH
+ * Math.cos(getArmPotAngle());
+ double handHorizontalDisplacement = Constants.DefenseArm.HAND_LENGTH
+ * Math.cos(getHandPotAngle());
+ return (armHorizontalDisplacement + handHorizontalDisplacement);
+ }
+
+ public double getArmVerticalDisplacement() {
+ double armMounted = Constants.DefenseArm.ARM_MOUNTED_HEIGHT;
+ double armVerticalDisplacement = Constants.DefenseArm.ARM_LENGTH
+ * Math.sin(getArmPotAngle());
+ double handVerticalDisplacement = Constants.DefenseArm.HAND_LENGTH
+ * Math.sin(getHandPotAngle());
+ return (armMounted + armVerticalDisplacement + handVerticalDisplacement);
+ }
+
+ public boolean isOutsideRange() {
+ if (getArmHorizontalDisplacement() < 15)
+ return false;
+ return true;
+ }
+
@Override
protected void initDefaultCommand() {
}
package org.usfirst.frc.team3501.robot.subsystems;
import org.usfirst.frc.team3501.robot.Constants;
+import org.usfirst.frc.team3501.robot.Lidar;
import edu.wpi.first.wpilibj.CANTalon;
-import edu.wpi.first.wpilibj.CANTalon.TalonControlMode;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.Encoder;
-import edu.wpi.first.wpilibj.PIDController;
+import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.command.Subsystem;
public class DriveTrain extends Subsystem {
// Drivetrain related objects
private Encoder leftEncoder, rightEncoder;
+ public static Lidar lidar;
private CANTalon frontLeft, frontRight, rearLeft, rearRight;
- private PIDController frontLeftC, frontRightC, rearLeftC, rearRightC;
- // Drivetrain specific constants that relate to the inches per pulse value for
- // the encoders
- private final static double WHEEL_DIAMETER = 6.0; // in inches
- private final static double PULSES_PER_ROTATION = 256; // in pulses
- private final static double OUTPUT_SPROCKET_DIAMETER = 2.0; // in inches
- private final static double WHEEL_SPROCKET_DIAMETER = 3.5; // in inches
- public final static double INCHES_PER_PULSE = (((Math.PI)
- * OUTPUT_SPROCKET_DIAMETER / PULSES_PER_ROTATION) / WHEEL_SPROCKET_DIAMETER)
- * WHEEL_DIAMETER;
- // Drivetrain specific constants that relate to the PID controllers
- private final static double Kp = 1.0, Ki = 0.0, Kd = 0.0;
public DriveTrain() {
frontLeft = new CANTalon(Constants.DriveTrain.FRONT_LEFT);
frontRight = new CANTalon(Constants.DriveTrain.FRONT_RIGHT);
rearLeft = new CANTalon(Constants.DriveTrain.REAR_LEFT);
rearRight = new CANTalon(Constants.DriveTrain.REAR_RIGHT);
- this.configureTalons();
-
- frontLeftC = new PIDController(Kp, Ki, Kd, frontLeft, frontLeft);
- frontRightC = new PIDController(Kp, Ki, Kd, frontRight, frontRight);
- rearLeftC = new PIDController(Kp, Ki, Kd, frontLeft, rearLeft);
- rearRightC = new PIDController(Kp, Ki, Kd, frontRight, rearRight);
- this.enablePIDControllers();
+ lidar = new Lidar(I2C.Port.kOnboard);
leftEncoder = new Encoder(Constants.DriveTrain.ENCODER_LEFT_A,
Constants.DriveTrain.ENCODER_LEFT_B, false, EncodingType.k4X);
rightEncoder = new Encoder(Constants.DriveTrain.ENCODER_RIGHT_A,
Constants.DriveTrain.ENCODER_RIGHT_B, false, EncodingType.k4X);
- leftEncoder.setDistancePerPulse(INCHES_PER_PULSE);
- rightEncoder.setDistancePerPulse(INCHES_PER_PULSE);
+ leftEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
+ rightEncoder.setDistancePerPulse(Constants.DriveTrain.INCHES_PER_PULSE);
}
@Override
protected void initDefaultCommand() {
}
- public void configureTalons() {
- frontLeft.changeControlMode(TalonControlMode.Speed);
- frontRight.changeControlMode(TalonControlMode.Speed);
- rearLeft.changeControlMode(TalonControlMode.Speed);
- rearRight.changeControlMode(TalonControlMode.Speed);
-
- frontLeft.configEncoderCodesPerRev(256);
- frontRight.configEncoderCodesPerRev(256);
- rearLeft.configEncoderCodesPerRev(256);
- rearRight.configEncoderCodesPerRev(256);
-
- frontLeft.enableControl();
- frontRight.enableControl();
- rearLeft.enableControl();
- rearRight.enableControl();
- }
-
- public void enablePIDControllers() {
- frontLeftC.enable();
- frontRightC.enable();
- rearLeftC.enable();
- rearRightC.enable();
- }
-
- public void drive(double left, double right) {
- frontLeftC.setSetpoint(left);
- rearLeftC.setSetpoint(left);
- frontRightC.setSetpoint(right);
- rearRightC.setSetpoint(right);
- }
-
public void resetEncoders() {
leftEncoder.reset();
rightEncoder.reset();
}
+ public double getLidarDistance() {
+ return lidar.pidGet();
+ }
+
public double getRightSpeed() {
return rightEncoder.getRate(); // in inches per second
}
* The IntakeArm consists of two rollers that are controlled by one motor, with
* a potentiometer on it.
*
- * The motor controls the rollers, making them roll forwards and backwards.
- * The Intake rollers are on the back of the robot. As the rollers run, they
- * intake the ball.
+ * The motor controls the rollers, making them roll forwards and backwards. The
+ * Intake rollers are on the back of the robot. As the rollers run, they intake
+ * the ball.
*
* @author superuser
*
*/
public class IntakeArm extends Subsystem {
- private CANTalon intakeRoller;
- private CANTalon intakeArm;
- private AnalogPotentiometer intakePot;
-
- public IntakeArm() {
- intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT);
- intakeArm = new CANTalon(Constants.IntakeArm.INTAKE_PORT);
- intakePot = new AnalogPotentiometer(
- Constants.IntakeArm.INTAKE_CHANNEL,
- Constants.IntakeArm.FULL_RANGE,
- Constants.IntakeArm.OFFSET);
- }
-
- /***
- * This method sets the voltage of the motor to intake the ball.
- * The voltage values are constants in Constants class
- */
- public void intakeBall() {
- intakeRoller.set(Constants.IntakeArm.INTAKE_SPEED);
- }
-
- /***
- * This method sets the voltage of the motor to output the ball.
- * The voltage values are constants in Constants class
- */
- public void outputBall() {
- intakeRoller.set(Constants.IntakeArm.OUTPUT_SPEED);
- }
-
- /***
- * This method gets you the current voltage of the motor that controls the
- * intake arm roller. The range of voltage is from [-1,1].
- * A negative voltage makes the motor run backwards.
- *
- * @return Returns the voltage of the motor that controls the roller. The
- * range of the voltage goes from [-1,1].
- * A negative voltage indicates that the motor is running backwards.
- */
-
- public double getRollerVoltage() {
- return intakeRoller.get();
- }
-
- /***
- * This method sets the voltage of the arm motor. The range is from [-1,1]. A
- * negative voltage makes the direction of the motor go backwards.
- *
- * @param voltage
- * The voltage that you set the motor at. The range of the voltage of
- * the arm motor is from [-1,1]. A
- * negative voltage makes the direction of the motor go backwards.
- */
-
- public void setArmVoltage(double voltage) {
- if (voltage > 1)
- voltage = 1;
- else if (voltage < -1)
- voltage = -1;
-
- intakeArm.set(voltage);
- }
-
- /***
- * This method gets you the current voltage of the motor that controls the
- * intake arm. The range of voltage is from [-1,1].
- * A negative voltage makes the motor run backwards.
- *
- * @return Returns the voltage of the motor that controls the arm. The
- * range of the voltage goes from [-1,1].
- * A negative voltage indicates that the motor is running backwards.
- */
-
- public double getArmVoltage() {
- return intakeArm.get();
- }
-
- /***
- * This method checks to see if the presence of the ball inside is true or
- * false.
- *
- * @return Returns whether the ball is inside as true or false
- */
-
- public boolean isBallInside() {
- return true;
- }
-
- /***
- * This method checks to see if the motors controlling the rollers are
- * currently running.
- *
- * @return Returns whether the motors are currently running, and returns the
- * state of the condition (true or false).
- *
- */
-
- public boolean areRollersRolling() {
- return true;
- }
-
- /***
- * This method gets the angle of the potentiometer on the Intake Arm.
- *
- * @return angle of potentiometer
- */
- public double getArmAngle() {
- return intakePot.get();
- }
-
- @Override
- protected void initDefaultCommand() {
-
- }
+ private CANTalon intakeRoller;
+ private CANTalon intakeArm;
+ private AnalogPotentiometer intakePot;
+ private double[] potAngles = { 0, 45, 90 };
+
+ public IntakeArm() {
+ intakeRoller = new CANTalon(Constants.IntakeArm.ROLLER_PORT);
+ intakeArm = new CANTalon(Constants.IntakeArm.ARM_PORT);
+ intakePot = new AnalogPotentiometer(Constants.IntakeArm.POT_CHANNEL, Constants.IntakeArm.FULL_RANGE,
+ Constants.IntakeArm.OFFSET);
+
+ }
+
+ /***
+ * This method sets the voltage of the motor to intake the ball. The voltage
+ * values are constants in Constants class
+ */
+ public void intakeBall() {
+ intakeRoller.set(Constants.IntakeArm.INTAKE_SPEED);
+ }
+
+ /***
+ * This method sets the voltage of the motor to output the ball. The voltage
+ * values are constants in Constants class
+ */
+ public void outputBall() {
+ intakeRoller.set(Constants.IntakeArm.OUTPUT_SPEED);
+ }
+
+ /***
+ * This method gets you the current voltage of the motor that controls the
+ * intake arm roller. The range of voltage is from [-1,1]. A negative
+ * voltage makes the motor run backwards.
+ *
+ * @return Returns the voltage of the motor that controls the roller. The
+ * range of the voltage goes from [-1,1]. A negative voltage
+ * indicates that the motor is running backwards.
+ */
+
+ public double getRollerVoltage() {
+ return intakeRoller.get();
+ }
+
+ /***
+ * This method sets the voltage of the arm motor. The range is from [-1,1].
+ * A negative voltage makes the direction of the motor go backwards.
+ *
+ * @param voltage
+ * The voltage that you set the motor at. The range of the
+ * voltage of the arm motor is from [-1,1]. A negative voltage
+ * makes the direction of the motor go backwards.
+ */
+
+ public void setArmSpeed(double voltage) {
+ if (voltage > 1)
+ voltage = 1;
+ else if (voltage < -1)
+ voltage = -1;
+
+ intakeArm.set(voltage);
+ }
+
+ /***
+ * This method gets you the current voltage of the motor that controls the
+ * intake arm. The range of voltage is from [-1,1]. A negative voltage makes
+ * the motor run backwards.
+ *
+ * @return Returns the voltage of the motor that controls the arm. The range
+ * of the voltage goes from [-1,1]. A negative voltage indicates
+ * that the motor is running backwards.
+ */
+
+ public double getArmSpeed() {
+ return intakeArm.get();
+ }
+
+ /***
+ * This method checks to see if the presence of the ball inside is true or
+ * false.
+ *
+ * @return Returns whether the ball is inside as true or false
+ */
+
+ public boolean isBallInside() {
+ return true;
+ }
+
+ /***
+ * This method checks to see if the motors controlling the rollers are
+ * currently running.
+ *
+ * @return Returns whether the motors are currently running, and returns the
+ * state of the condition (true or false).
+ *
+ */
+
+ public boolean areRollersRolling() {
+ return true;
+ }
+
+ /***
+ * This method gets the angle of the potentiometer on the Intake Arm.
+ *
+ * @return angle of potentiometer
+ */
+
+ public double getArmAngle() {
+ return intakePot.get() + Constants.IntakeArm.ZERO_ANGLE;
+ }
+
+ public void stop() {
+ setArmSpeed(0);
+ }
+
+ public double getAngleForLevel(double targetLevel) {
+ return potAngles[(int) (targetLevel - 1)];
+ }
+
+ @Override
+ protected void initDefaultCommand() {
+
+ }
}
projects / 3501 / stronghold-2016 / commitdiff