Improved readability and easier customization

2019 Draft III/src/main/java/frc/robot/OI.java

@@ -0,0 +1,104 @@
+package frc.robot;
+
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.GenericHID.Hand;
+import edu.wpi.first.wpilibj.buttons.Button;
+import edu.wpi.first.wpilibj.buttons.JoystickButton;
+import frc.robot.commands.ArmLiftTo;
+import frc.robot.commands.ClawLiftTo;
+import frc.robot.commands.ClimbAdvance;
+import frc.robot.commands.ChassisChangeSpeed;
+import frc.robot.commands.ExtensionToggle;
+import frc.robot.commands.HookToggle;
+import frc.robot.commands.RobotChangeState;
+import frc.robot.commands.SupportIn;
+import frc.robot.commands.SupportOut;
+import frc.robot.commands.IntakeCollect;
+import frc.robot.commands.IntakeShoot;
+import frc.robot.commands.OneButtonClimbUp;
+import frc.robot.commands.Tester;
+import frc.robot.commands.ToCollect;
+import frc.robot.commands.ToMiddle;
+import frc.robot.commands.ToTop;
+
+
+public class OI {
+  /* initialize two Xbox controller with id 0 and 1 on DS */
+  public static final XboxController stick_0 = new XboxController(0);
+  public static final XboxController stick_1 = new XboxController(1);
+
+  /* create button objects, the last number is the id of the button, starting from 1 */
+  public static Button stick_0_A = new JoystickButton(stick_0, 1);
+  public static Button stick_0_B = new JoystickButton(stick_0, 2);
+  public static Button stick_0_X = new JoystickButton(stick_0, 3);
+  public static Button stick_0_Y = new JoystickButton(stick_0, 4);
+  public static Button stick_0_back = new JoystickButton(stick_0, 7);
+  public static Button stick_0_start = new JoystickButton(stick_0, 8);
+  public static Button stick_0_LBumper = new JoystickButton(stick_0, 5);
+  public static Button stick_0_RBumper = new JoystickButton(stick_0, 6);
+  public static XboxPOV stick_0_UP = new XboxPOV(stick_0, 0);
+  public static XboxPOV stick_0_DOWN = new XboxPOV(stick_0, 180);
+
+  public static Button stick_1_A = new JoystickButton(stick_1, 1);
+  public static Button stick_1_B = new JoystickButton(stick_1, 2);
+  public static Button stick_1_X = new JoystickButton(stick_1, 3);
+  public static Button stick_1_Y = new JoystickButton(stick_1, 4);
+  public static XboxPOV stick_1_UP = new XboxPOV(stick_1, 0);
+  public static XboxPOV stick_1_RIGHT = new XboxPOV(stick_1, 90);
+  public static XboxPOV stick_1_DOWN = new XboxPOV(stick_1, 180);
+  public static double joystick_threshold = 0.15;
+
+  public OI() {
+    /* binding buttons to commands */
+    /* 将任务 (command) 绑定到按钮 */
+    // stick_0_A.whenPressed(new SupportOut());
+    // stick_0_B.whenPressed(new SupportIn());
+    // stick_0_X.whenPressed(new IntakeLiftTo(3924));
+    // stick_0_Y.whenPressed(new IntakeLiftTo(3850));
+
+    stick_0_UP.whenPressed(new SupportIn());
+    stick_0_DOWN.whileHeld(new SupportOut());
+    stick_0_back.whenPressed(new OneButtonClimbUp());
+    stick_0_start.whileHeld(new ChassisChangeSpeed());
+
+
+    stick_0_LBumper.whileHeld(new ClimbAdvance());
+    // stick_0_RBumper.whenPressed(new RobotChangeState(false));
+
+    stick_1_UP.whenPressed(new ToTop());
+    stick_1_RIGHT.whenPressed(new ToMiddle());
+    stick_1_DOWN.whenPressed(new ToCollect());
+
+    stick_1_A.whileHeld(new IntakeCollect());
+    stick_1_B.whileHeld(new IntakeShoot());
+    stick_1_X.toggleWhenPressed(new HookToggle());
+    stick_1_Y.toggleWhenPressed(new ExtensionToggle());
+  }
+
+  public static double unify(double val) {
+    if (Math.abs(val) < joystick_threshold) {
+      val = 0;
+    }
+    return val;
+  }
+
+  public static double[] getDriveAxis() {
+    double[] result = {
+      unify(-stick_0.getY(Hand.kLeft)), 
+      unify(stick_0.getTriggerAxis(Hand.kLeft) - stick_0.getTriggerAxis(Hand.kRight))
+    };
+    return result;
+  }
+
+  public static double getIntakeAxis() {
+    return unify(-stick_0.getY(Hand.kRight));
+  }
+
+  public static double getClawAxis() {
+    return unify(-stick_1.getY(Hand.kRight));
+  }
+
+  public static double getArmAxis() {
+    return unify(-stick_1.getY(Hand.kLeft));
+  }
+}

2019 Draft III/src/main/java/frc/robot/RobotMap.java

@@ -0,0 +1,45 @@
+/**
+ * RobotMap.java 
+ * provides mapping for the I/O ports. not used.
+ */
+
+package frc.robot;
+
+/**
+ * The RobotMap is a mapping from the ports sensors and actuators are wired into
+ * to a variable name. This provides flexibility changing wiring, makes checking
+ * the wiring easier and significantly reduces the number of magic numbers
+ * floating around.
+ */
+public class RobotMap {
+
+
+
+  /* Chassis */
+  public static final int[] p_CAN_chassis_left = {10, 11, 12};
+  public static final int[] p_CAN_chassis_right = {13, 14, 15};
+  public static final int p_PWM_chassis_roller = 0;
+  public static final int p_PEN_lift_up = 0;
+  public static final double chassis_speed_z = 0.4;
+  public static final double chassis_speed_y = 0.7;
+
+  /* Arm */
+  public static final int[] p_CAN_arm = {20, 21};
+  public static final double[] arm_speed = {0.6, 0.4};
+  public static final double arm_kP = 0.003, arm_kD = 0.0001;
+
+  /* Claw */
+  public static final int p_CAN_claw = 22;
+  public static final int p_PWM_claw_shooter = 1;
+  public static final int p_DIG_arm_limit = 0;
+
+  /* Hook */
+  public static final int p_PWM_hook_servo = 9;
+  public static final int p_PEN_hook_extender = 1;
+
+  /* Intake */
+  public static final int p_PWM_intake_lift = 2;
+  public static final int p_PWM_intake_collector = 3;
+  public static final int p_ANA_intake_encoder = 0;
+  public static final int p_DIG_intake_limit = 2;
+}

2019 Draft III/src/main/java/frc/robot/commands/ArmLiftTo.java

@@ -0,0 +1,44 @@
+/**
+ * ChassisDefault.java
+ */
+
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj.command.Command;
+import frc.robot.OI;
+import frc.robot.Robot;
+
+public class ArmLiftTo extends Command {
+  private static final double threshold = 20;
+  private double tar_;
+
+  public ArmLiftTo(double tar) {
+    super(4);
+    requires(Robot.arm);
+    tar_ = tar;
+  }
+
+  @Override
+  protected void execute() {
+    /* 0212: PID算法，现在只加了P，先试试效果 */
+    Robot.arm.setTarget(tar_);
+    Robot.arm.setVel(Robot.arm.getPID());
+  }
+
+
+  @Override
+  protected boolean isFinished() {
+    return Math.abs(tar_ - Robot.arm.getPos()) < threshold
+          || OI.getArmAxis() != 0 || isTimedOut();
+  }
+
+  @Override
+  protected void end() {
+    Robot.arm.setVel(0);
+  }
+
+  @Override
+  protected void interrupted() {
+    end();
+  }
+}

2019 Draft III/src/main/java/frc/robot/subsystems/Arm.java

@@ -0,0 +1,88 @@
+/**
+ * Arm.java
+ * contains motors with encoders for both arm and claw
+ * 
+ */
+
+package frc.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.ControlMode;
+import com.ctre.phoenix.motorcontrol.can.TalonSRX;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc.robot.Const;
+import frc.robot.RobotMap;
+import frc.robot.commands.ArmDefault;
+
+public class Arm extends Subsystem {
+  private TalonSRX motor_arm_left = new TalonSRX(RobotMap.p_CAN_arm[0]);
+  private TalonSRX motor_arm_right = new TalonSRX(RobotMap.p_CAN_arm[1]);
+  private DigitalInput arm_limit_sw = new DigitalInput(RobotMap.p_DIG_arm_limit);
+
+
+  private final double kP = 0.004, kD = 0.00015;
+  private double setpoint = 0;
+
+  public Arm() {
+    super();
+    motor_arm_left.setInverted(false);
+    motor_arm_right.setInverted(true);
+  }
+
+  public void resetSensor() {
+    motor_arm_left.setSelectedSensorPosition(0);
+    // motor_arm_right.setSelectedSensorPosition(0);
+  }
+
+  public double getPos() {
+    return -motor_arm_left.getSelectedSensorPosition();
+    // return motor_arm_right.getSelectedSensorPosition();
+  }
+
+  public double getVel() {
+    return -motor_arm_left.getSelectedSensorVelocity();
+    // return motor_arm_right.getSelectedSensorVelocity();
+  }
+
+
+  public double getPID() {
+    /* 0212: PID算法，现在只加了P，先试试效果 */
+    double error = target - getPos();
+    double vel = kP * error - kD * getVel();
+    vel = Math.max(-1.2, Math.min(1.2, vel));
+    return vel;
+  }
+
+  public void setTarget(double val) {
+    target = val;
+  }
+
+  public void setVel(double vel) {
+    if (vel > 0) {
+      /* upward */
+      motor_arm_left.set(ControlMode.PercentOutput, Const.global_arm_speed[0] * vel);
+      motor_arm_right.set(ControlMode.PercentOutput, Const.global_arm_speed[0] * vel);
+    } else {
+      /* downward */
+      if (!end_stop.get()) {
+        motor_arm_left.set(ControlMode.PercentOutput, 0);
+        motor_arm_right.set(ControlMode.PercentOutput, 0);
+      } else {
+        motor_arm_left.set(ControlMode.PercentOutput, Const.global_arm_speed[1] * vel);
+        motor_arm_right.set(ControlMode.PercentOutput, Const.global_arm_speed[1] * vel);
+      }
+    }
+  }
+
+  public void log() {
+    SmartDashboard.putNumber("Arm Position", getPos());
+    SmartDashboard.putNumber("Arm Setpoint", target);
+    SmartDashboard.putBoolean("Arm Limit", end_stop.get());
+  }
+
+  @Override
+  public void initDefaultCommand() {
+    setDefaultCommand(new ArmDefault());
+  }
+}

2019 Draft III/src/main/java/frc/robot/subsystems/Chassis.java

@@ -0,0 +1,81 @@
+/**
+ * Chassis.java
+ * contains motors on the chassis and a gyro for navigation.
+ * 定义了底盘电机和一个陀螺仪。
+ */
+package frc.robot.subsystems;
+
+import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+import edu.wpi.first.wpilibj.ADXRS450_Gyro;
+import edu.wpi.first.wpilibj.Solenoid;
+import edu.wpi.first.wpilibj.Spark;
+import edu.wpi.first.wpilibj.SpeedController;
+import edu.wpi.first.wpilibj.command.Subsystem;
+import frc.robot.Const;
+import frc.robot.RobotMap;
+import frc.robot.commands.ChassisDefault;
+
+public class Chassis extends Subsystem {
+  private SpeedController motor_left_0 = new WPI_VictorSPX(RobotMap.p_CAN_chassis_left[0]);
+  private SpeedController motor_left_1 = new WPI_VictorSPX(RobotMap.p_CAN_chassis_left[1]);
+  private SpeedController motor_left_2 = new WPI_VictorSPX(RobotMap.p_CAN_chassis_left[2]);
+  private SpeedController motor_right_0 = new WPI_VictorSPX(RobotMap.p_CAN_chassis_right[0]);
+  private SpeedController motor_right_1 = new WPI_VictorSPX(RobotMap.p_CAN_chassis_right[1]);
+  private SpeedController motor_right_2 = new WPI_VictorSPX(RobotMap.p_CAN_chassis_right[2]);
+  private SpeedController motor_roller = new Spark(RobotMap.p_PWM_chassis_roller);
+
+  private Solenoid lift_up = new Solenoid(RobotMap.p_PEN_lift_up);
+
+  /* using the 'default FRC gyro' */
+  /* 使用的是默认的插在RIO上的陀螺仪 */
+  // private ADXRS450_Gyro gyro = new ADXRS450_Gyro();
+
+  /* the coefficient for the speed of the chassis motor */
+  /* 电机速度系数，用来控制全局电机最大转速 */
+
+  public int speed_mode = 0;
+
+  public Chassis() {
+    super();
+
+    /* correct the direction of motor: forward is positive */
+    motor_left_0.setInverted(false);
+    motor_left_1.setInverted(false);
+    motor_left_2.setInverted(false);
+    motor_right_0.setInverted(true);
+    motor_right_1.setInverted(true);
+    motor_right_2.setInverted(true);
+  }
+
+  /**
+   * method for driving the chassis.
+   * @param y: forward is positive
+   * @param z: CCW is positive
+   */
+  public void drive(double y, double z) {
+    motor_left_0.set(Const.global_y_speed[speed_mode] * y - Const.global_z_speed[speed_mode] * z);
+    motor_left_1.set(Const.global_y_speed[speed_mode] * y - Const.global_z_speed[speed_mode] * z);
+    motor_left_2.set(Const.global_y_speed[speed_mode] * y - Const.global_z_speed[speed_mode] * z);
+    motor_right_0.set(Const.global_y_speed[speed_mode] * y + Const.global_z_speed[speed_mode] * z);
+    motor_right_1.set(Const.global_y_speed[speed_mode] * y + Const.global_z_speed[speed_mode] * z);
+    motor_right_2.set(Const.global_y_speed[speed_mode] * y + Const.global_z_speed[speed_mode] * z);
+  }
+
+  public void toggleLift(boolean state) {
+    lift_up.set(state);
+  }
+
+  public void setRollerVel(double vel) {
+    motor_roller.set(vel);
+  }
+
+  public void log() {
+  }
+
+  @Override
+  public void initDefaultCommand() {
+    /* the default command of this subsystem is to drive according
+    to joystick input. */
+    setDefaultCommand(new ChassisDefault());
+  }
+}