From 694ae046c2c60f6205092c4e78e6985d5c017437 Mon Sep 17 00:00:00 2001 From: Taiga Nishida <97322839+tainish@users.noreply.github.com> Date: Fri, 4 Mar 2022 17:26:55 -0800 Subject: [PATCH 1/3] Update RobotMap.java --- src/main/java/org/usfirst/frc4904/robot/RobotMap.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index 75360ae..e278a87 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -30,6 +30,7 @@ public static class CANMotor { public static int RIGHT_DRIVE_B = 3; public static int LEFT_DRIVE_A = 4; public static int LEFT_DRIVE_B = 5; + public static int CLIMBER = 6; } public static class PWM { @@ -93,6 +94,7 @@ public static class Component { public static Motor rightWheelB; public static Motor leftWheelA; public static Motor leftWheelB; + public static Motor climber; public static TankDrive chassis; public static CustomPIDController drivePID; } @@ -120,12 +122,14 @@ public RobotMap() { CANTalonFX leftWheelBTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_B); CANTalonFX rightWheelATalon = new CANTalonFX(Port.CANMotor.RIGHT_DRIVE_A); CANTalonFX rightWheelBTalon = new CANTalonFX(Port.CANMotor.RIGHT_DRIVE_B); + CANTalonFX climberTalon = new CANTalonFX(Port.CANMotor.CLIMBER); // Wheels Component.rightWheelA = new Motor("rightWheelA", false, rightWheelATalon); Component.rightWheelB = new Motor("rightWheelB", false, rightWheelBTalon); Component.leftWheelA = new Motor("leftWheelA", true, leftWheelATalon); Component.leftWheelB = new Motor("leftWheelB", true, leftWheelBTalon); + Component.climber = new Motor("climber", false, climberTalon); // Wheel Encoders Component.leftWheelCANCoder = new CustomCANCoder(Port.CAN.LEFT_WHEEL_ENCODER, From bfc29ee5ff709f377f77f4e7cf7e392e2b057e49 Mon Sep 17 00:00:00 2001 From: Taiga Nishida <97322839+tainish@users.noreply.github.com> Date: Fri, 4 Mar 2022 17:36:39 -0800 Subject: [PATCH 2/3] Add climber --- .../frc4904/robot/commands/ClimberOff.java | 15 +++++++++++++++ .../usfirst/frc4904/robot/commands/ClimberOn.java | 11 +++++++++++ 2 files changed, 26 insertions(+) create mode 100644 src/main/java/org/usfirst/frc4904/robot/commands/ClimberOff.java create mode 100644 src/main/java/org/usfirst/frc4904/robot/commands/ClimberOn.java diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOff.java b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOff.java new file mode 100644 index 0000000..af92746 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOff.java @@ -0,0 +1,15 @@ +package org.usfirst.frc4904.robot.commands; + +import org.usfirst.frc4904.robot.RobotMap; +import org.usfirst.frc4904.standard.commands.motor.MotorIdle; + + +public class ClimberOff extends MotorIdle { + + /** + * Set indexer motor speed to zero + */ + public ClimberOff() { + super(RobotMap.Component.climber); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOn.java b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOn.java new file mode 100644 index 0000000..451f70f --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOn.java @@ -0,0 +1,11 @@ +package org.usfirst.frc4904.robot.commands; + +import org.usfirst.frc4904.robot.RobotMap; +import org.usfirst.frc4904.standard.commands.motor.MotorConstant; + +public class ClimberOn extends MotorConstant { + public final static double DEFAULT_INTAKE_MOTOR_SPEED = 0.5; //TODO: needs value + public ClimberOn() { + super("ClimberOn", RobotMap.Component.climber, DEFAULT_INTAKE_MOTOR_SPEED); + } +} \ No newline at end of file From ded41572dd47a36b1366413c3500ccc6173cf421 Mon Sep 17 00:00:00 2001 From: Taiga Nishida <97322839+tainish@users.noreply.github.com> Date: Fri, 11 Mar 2022 18:01:26 -0800 Subject: [PATCH 3/3] Changed from one motor to two motors --- .../org/usfirst/frc4904/robot/RobotMap.java | 17 +++++++++++++---- .../{ClimberOff.java => ClimberAOff.java} | 6 +++--- .../frc4904/robot/commands/ClimberAOn.java | 11 +++++++++++ .../frc4904/robot/commands/ClimberBOff.java | 15 +++++++++++++++ .../frc4904/robot/commands/ClimberBOn.java | 11 +++++++++++ .../frc4904/robot/commands/ClimberOn.java | 11 ----------- 6 files changed, 53 insertions(+), 18 deletions(-) rename src/main/java/org/usfirst/frc4904/robot/commands/{ClimberOff.java => ClimberAOff.java} (65%) create mode 100644 src/main/java/org/usfirst/frc4904/robot/commands/ClimberAOn.java create mode 100644 src/main/java/org/usfirst/frc4904/robot/commands/ClimberBOff.java create mode 100644 src/main/java/org/usfirst/frc4904/robot/commands/ClimberBOn.java delete mode 100644 src/main/java/org/usfirst/frc4904/robot/commands/ClimberOn.java diff --git a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java index e278a87..cf7775a 100644 --- a/src/main/java/org/usfirst/frc4904/robot/RobotMap.java +++ b/src/main/java/org/usfirst/frc4904/robot/RobotMap.java @@ -30,7 +30,8 @@ public static class CANMotor { public static int RIGHT_DRIVE_B = 3; public static int LEFT_DRIVE_A = 4; public static int LEFT_DRIVE_B = 5; - public static int CLIMBER = 6; + public static int CLIMBER_A = 6; + public static int CLIMBER_B = 7; } public static class PWM { @@ -94,7 +95,8 @@ public static class Component { public static Motor rightWheelB; public static Motor leftWheelA; public static Motor leftWheelB; - public static Motor climber; + public static Motor climberA; + public static Motor climberB; public static TankDrive chassis; public static CustomPIDController drivePID; } @@ -122,14 +124,12 @@ public RobotMap() { CANTalonFX leftWheelBTalon = new CANTalonFX(Port.CANMotor.LEFT_DRIVE_B); CANTalonFX rightWheelATalon = new CANTalonFX(Port.CANMotor.RIGHT_DRIVE_A); CANTalonFX rightWheelBTalon = new CANTalonFX(Port.CANMotor.RIGHT_DRIVE_B); - CANTalonFX climberTalon = new CANTalonFX(Port.CANMotor.CLIMBER); // Wheels Component.rightWheelA = new Motor("rightWheelA", false, rightWheelATalon); Component.rightWheelB = new Motor("rightWheelB", false, rightWheelBTalon); Component.leftWheelA = new Motor("leftWheelA", true, leftWheelATalon); Component.leftWheelB = new Motor("leftWheelB", true, leftWheelBTalon); - Component.climber = new Motor("climber", false, climberTalon); // Wheel Encoders Component.leftWheelCANCoder = new CustomCANCoder(Port.CAN.LEFT_WHEEL_ENCODER, @@ -147,6 +147,15 @@ public RobotMap() { Component.chassisTalonEncoders = new EncoderPair(Component.leftWheelTalonEncoder, Component.rightWheelTalonEncoder); Component.chassisCANCoders = new EncoderPair(Component.leftWheelCANCoder, Component.rightWheelCANCoder); + /* Climber */ + // TalonFX + CANTalonFX climberATalon = new CANTalonFX(Port.CANMotor.CLIMBER_A); + CANTalonFX climberBTalon = new CANTalonFX(Port.CANMotor.CLIMBER_B); + + // Climber components + Component.climberA = new Motor("climberA", false, climberATalon); + Component.climberB = new Motor("climberB", false, climberBTalon); + // General Chassis Component.chassis = new TankDrive("2022-Chassis", Component.leftWheelA, Component.leftWheelB, Component.rightWheelA, Component.rightWheelB); diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOff.java b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberAOff.java similarity index 65% rename from src/main/java/org/usfirst/frc4904/robot/commands/ClimberOff.java rename to src/main/java/org/usfirst/frc4904/robot/commands/ClimberAOff.java index af92746..613c782 100644 --- a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOff.java +++ b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberAOff.java @@ -4,12 +4,12 @@ import org.usfirst.frc4904.standard.commands.motor.MotorIdle; -public class ClimberOff extends MotorIdle { +public class ClimberAOff extends MotorIdle { /** * Set indexer motor speed to zero */ - public ClimberOff() { - super(RobotMap.Component.climber); + public ClimberAOff() { + super(RobotMap.Component.climberA); } } \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberAOn.java b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberAOn.java new file mode 100644 index 0000000..5a1777c --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberAOn.java @@ -0,0 +1,11 @@ +package org.usfirst.frc4904.robot.commands; + +import org.usfirst.frc4904.robot.RobotMap; +import org.usfirst.frc4904.standard.commands.motor.MotorConstant; + +public class ClimberAOn extends MotorConstant { + public final static double DEFAULT_CLIMBER_MOTOR_A_SPEED = 0.5; //TODO: needs value + public ClimberAOn() { + super("ClimberAOn", RobotMap.Component.climberA, DEFAULT_CLIMBER_MOTOR_A_SPEED); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberBOff.java b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberBOff.java new file mode 100644 index 0000000..4e21db8 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberBOff.java @@ -0,0 +1,15 @@ +package org.usfirst.frc4904.robot.commands; + +import org.usfirst.frc4904.robot.RobotMap; +import org.usfirst.frc4904.standard.commands.motor.MotorIdle; + + +public class ClimberBOff extends MotorIdle { + + /** + * Set indexer motor speed to zero + */ + public ClimberBOff() { + super(RobotMap.Component.climberB); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberBOn.java b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberBOn.java new file mode 100644 index 0000000..a52ea29 --- /dev/null +++ b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberBOn.java @@ -0,0 +1,11 @@ +package org.usfirst.frc4904.robot.commands; + +import org.usfirst.frc4904.robot.RobotMap; +import org.usfirst.frc4904.standard.commands.motor.MotorConstant; + +public class ClimberBOn extends MotorConstant { + public final static double DEFAULT_CLIMBERA_MOTOR_B_SPEED = 0.5; //TODO: needs value + public ClimberBOn() { + super("ClimberBOn", RobotMap.Component.climberB, DEFAULT_CLIMBERA_MOTOR_B_SPEED); + } +} \ No newline at end of file diff --git a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOn.java b/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOn.java deleted file mode 100644 index 451f70f..0000000 --- a/src/main/java/org/usfirst/frc4904/robot/commands/ClimberOn.java +++ /dev/null @@ -1,11 +0,0 @@ -package org.usfirst.frc4904.robot.commands; - -import org.usfirst.frc4904.robot.RobotMap; -import org.usfirst.frc4904.standard.commands.motor.MotorConstant; - -public class ClimberOn extends MotorConstant { - public final static double DEFAULT_INTAKE_MOTOR_SPEED = 0.5; //TODO: needs value - public ClimberOn() { - super("ClimberOn", RobotMap.Component.climber, DEFAULT_INTAKE_MOTOR_SPEED); - } -} \ No newline at end of file