diff --git a/2019 Draft III/src/main/java/frc/robot/OI.java b/2019 Draft III/src/main/java/frc/robot/OI.java new file mode 100644 index 0000000..6e75a64 --- /dev/null +++ b/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)); + } +} diff --git a/2019 Draft III/src/main/java/frc/robot/RobotMap.java b/2019 Draft III/src/main/java/frc/robot/RobotMap.java new file mode 100644 index 0000000..1afae4e --- /dev/null +++ b/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; +} diff --git a/2019 Draft III/src/main/java/frc/robot/commands/ArmLiftTo.java b/2019 Draft III/src/main/java/frc/robot/commands/ArmLiftTo.java new file mode 100644 index 0000000..78b28d8 --- /dev/null +++ b/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(); + } +} diff --git a/2019 Draft III/src/main/java/frc/robot/subsystems/Arm.java b/2019 Draft III/src/main/java/frc/robot/subsystems/Arm.java new file mode 100644 index 0000000..8a332b5 --- /dev/null +++ b/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()); + } +} diff --git a/2019 Draft III/src/main/java/frc/robot/subsystems/Chassis.java b/2019 Draft III/src/main/java/frc/robot/subsystems/Chassis.java new file mode 100644 index 0000000..6c9abb6 --- /dev/null +++ b/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()); + } +} diff --git a/2019 Draft III/src/main/java/frc/robot/subsystems/Claw.java b/2019 Draft III/src/main/java/frc/robot/subsystems/Claw.java new file mode 100644 index 0000000..088ecb7 --- /dev/null +++ b/2019 Draft III/src/main/java/frc/robot/subsystems/Claw.java @@ -0,0 +1,74 @@ +/** + * Claw.java + * contains motors with encoders for claw + * + */ + +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.can.TalonSRX; +import edu.wpi.first.wpilibj.SpeedController; +import edu.wpi.first.wpilibj.VictorSP; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotMap; +import frc.robot.Const; +import frc.robot.commands.ClawDefault; + +public class Claw extends Subsystem { + private TalonSRX lift_motor = new TalonSRX(RobotMap.p_CAN_claw); + private SpeedController shooter_motor = new VictorSP(RobotMap.p_PWM_claw_shooter); + + + private final double kP = 0.005; + private double setpoint = 800; + public boolean enable_pid = true; + + public Claw() { + super(); + lift_motor.setInverted(true); + shooter_motor.setInverted(true); + } + + public void resetSensor() { + lift_motor.setSelectedSensorPosition(-1000); + } + + public double getPos() { + return -lift_motor.getSelectedSensorPosition(); + } + + public double getPID() { + double error = target - getPos(); + double vel = kP * error; + vel = Math.max(-1.5, Math.min(1.5, vel)); + return vel; + } + + public void adjustSetpoint(double val) { + setpoint += Const.adjust_intensity * val; + } + + public void setSetpoint(double val) { + target = val; + } + + public void setVel(double vel) { + lift_motor.set(ControlMode.PercentOutput, Const.global_claw_speed * vel); + } + + public void setShooterVel(double vel) { + shooter_motor.set(vel); + } + + public void log() { + SmartDashboard.putNumber("Claw Position", getPos()); + SmartDashboard.putNumber("Claw Setpoint", target); + } + + @Override + public void initDefaultCommand() { + setDefaultCommand(new ClawDefault()); + } +} diff --git a/gradlew b/gradlew old mode 100644 new mode 100755 diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5aa8709..b4527cb 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -1,9 +1,3 @@ -/** - * Robot.java - * the entry class for the robot code. - * 机器人程序从这里开始。 - */ - package frc.robot; import edu.wpi.cscore.UsbCamera; @@ -20,8 +14,7 @@ import frc.robot.subsystems.Intake; public class Robot extends TimedRobot { - /* create subsystem objects */ - /* 创建子系统的实例 */ + // create subsystem objects public static Chassis chassis = new Chassis(); public static Arm arm = new Arm(); public static Intake intake = new Intake(); @@ -32,45 +25,27 @@ public class Robot extends TimedRobot { Command m_autonomousCommand; SendableChooser m_chooser = new SendableChooser<>(); - /** - * This function is called when the robot is first powered up or the code - * is restarted. - * 这段代码会在机器人上电或程序重启的时候执行一遍。 - */ + @Override public void robotInit() { /* initialize two cameras, one for vision processing and one for the driver. */ UsbCamera aliment_cam = CameraServer.getInstance().startAutomaticCapture(0); - // TODO: codes for CV. + // codes for CV. UsbCamera driver_cam = CameraServer.getInstance().startAutomaticCapture(1); driver_cam.setResolution(640, 480); - // m_chooser.setDefaultOption("Default Auto", new ExampleCommand()); // chooser.addOption("My Auto", new MyAutoCommand()); SmartDashboard.putData("Auto mode", m_chooser); } - /** - * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test. It runs after the mode specific - * periodic functions, but before LiveWindow and SmartDashboard integrated - * updating. - * 这段代码将会在机器人通电的时候反复运行。运行顺序是在模式程序之后， LiveWindow - * 和 SD 更新数据之前。 - */ + @Override public void robotPeriodic() { arm.log(); } - /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. - */ @Override public void disabledInit() { } @@ -80,17 +55,6 @@ public void disabledPeriodic() { Scheduler.getInstance().run(); } - /** - * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable - * chooser code works with the Java SmartDashboard. If you prefer the - * LabVIEW Dashboard, remove all of the chooser code and uncomment the - * getString code to get the auto name from the text box below the Gyro - * - * You can add additional auto modes by adding additional commands to the - * chooser code above (like the commented example) or additional comparisons - * to the switch structure below with additional strings & commands. - */ @Override public void autonomousInit() { m_autonomousCommand = m_chooser.getSelected(); @@ -108,9 +72,6 @@ public void autonomousInit() { } } - /** - * This function is called periodically during autonomous. - */ @Override public void autonomousPeriodic() { Scheduler.getInstance().run(); @@ -127,17 +88,11 @@ public void teleopInit() { } } - /** - * This function is called periodically during operator control. - */ @Override public void teleopPeriodic() { Scheduler.getInstance().run(); } - /** - * This function is called periodically during test mode. - */ @Override public void testPeriodic() { }