-
Notifications
You must be signed in to change notification settings - Fork 0
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Arm auton edits #23
base: master
Are you sure you want to change the base?
Arm auton edits #23
Changes from all commits
b754c2d
9e87a08
ac6125d
31f4f2c
9c958fd
8b16298
5fddbe8
a3983d3
ddc716b
3740195
9d37333
d12bfd1
a9b52bf
cc6547b
b17a3f0
77a9f2b
882a760
360851b
16d45f7
dfa4882
932675b
894f00b
ab03886
9864c01
ca6a985
4f91233
8574b49
52db5a2
f0b6204
67f807d
88319b9
4bbbe6f
a8050f2
cb00a95
af142a7
04cf4f7
bda4fff
9622e63
0a8193f
1f7c8de
1cced3c
2d60239
826f472
3513f42
188857c
a1b8457
740fadc
4680d66
4db86a5
616a113
8772639
11d3094
bf523ca
687297b
24998bb
6c64bfa
2ddb37a
0ee8f2b
493197a
a4ca06d
c6a66e9
fe75062
2045b86
5dc9fa8
8b49d80
c53b464
8b3aa1d
7c9cbc3
c0d18f4
f739993
c5de4c4
2daba2e
5d35340
0d1beba
fca7b45
26eeb8e
ca29073
453ff58
9080d54
62d6f65
e697322
91aec1b
8c9928d
93fad97
254c04f
71f6766
aefc32b
1328385
84c0cfa
9bf5fd1
dfdee30
3fb0a60
7204acb
caee701
5338840
684637a
12bf890
c3caf69
7c607c3
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -2,13 +2,16 @@ | |
|
||
|
||
import org.usfirst.frc4904.robot.humaninterface.HumanInterfaceConfig; | ||
import org.usfirst.frc4904.robot.humaninterface.operators.LeakyRelu; | ||
import org.usfirst.frc4904.robot.subsystems.Arm; | ||
import org.usfirst.frc4904.robot.subsystems.CrateIO; | ||
import org.usfirst.frc4904.robot.subsystems.RollyBOI; | ||
import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick; | ||
import org.usfirst.frc4904.standard.custom.controllers.CustomXbox; | ||
import org.usfirst.frc4904.standard.custom.motioncontrollers.CANTalonSRX; | ||
import org.usfirst.frc4904.standard.custom.motioncontrollers.CustomPIDController; | ||
import org.usfirst.frc4904.standard.custom.sensors.CANEncoder; | ||
import org.usfirst.frc4904.standard.custom.sensors.CANSensor; | ||
import org.usfirst.frc4904.standard.custom.sensors.EncoderPair; | ||
import org.usfirst.frc4904.standard.custom.sensors.NavX; | ||
import org.usfirst.frc4904.standard.custom.sensors.PDP; | ||
|
@@ -18,16 +21,20 @@ | |
import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.AccelerationCap; | ||
import org.usfirst.frc4904.standard.subsystems.motor.speedmodifiers.EnableableModifier; | ||
import edu.wpi.first.wpilibj.DoubleSolenoid; | ||
import edu.wpi.first.wpilibj.SerialPort; | ||
import edu.wpi.first.wpilibj.VictorSP; | ||
|
||
public class RobotMap { | ||
public static class Port { | ||
|
||
public static class Port { // TODO: Correct Ports | ||
public static class HumanInput { | ||
public static final int joystick = 0; | ||
public static final int xboxController = 1; | ||
} | ||
|
||
public static class CANMotor { | ||
public static final int armMotorA = 9; | ||
public static final int armMotorB = 14; | ||
public static final int crateIORollerMotorLeft = 1; | ||
public static final int crateIORollerMotorRight = 7; | ||
public static final int rollyBOIRollerMotorLeft = 11; | ||
|
@@ -42,8 +49,9 @@ public static class PWM { | |
} | ||
|
||
public static class CAN { | ||
public static final int leftEncoder = -1; | ||
public static final int rightEncoder = -1; | ||
public static final int leftEncoder = 0x610; | ||
public static final int rightEncoder = 0x611; | ||
public static final int armEncoderPort = 0x612; | ||
} | ||
|
||
public static class Pneumatics { | ||
|
@@ -54,7 +62,7 @@ public static class Pneumatics { | |
|
||
public static class Metrics { // TODO: Check in later with design to confirm these metrics. | ||
public static class Wheel { | ||
public static final double TICKS_PER_REVOLUTION = 256; | ||
public static final double TICKS_PER_REVOLUTION = 1024; | ||
public static final double DIAMETER_INCHES = 4; | ||
public static final double CIRCUMFERENCE_INCHES = Metrics.Wheel.DIAMETER_INCHES * Math.PI; | ||
public static final double TICKS_PER_INCH = Metrics.Wheel.TICKS_PER_REVOLUTION | ||
|
@@ -64,11 +72,12 @@ public static class Wheel { | |
public static final double INCHES_PER_TICK = Metrics.Wheel.CIRCUMFERENCE_INCHES | ||
/ Metrics.Wheel.TICKS_PER_REVOLUTION; | ||
} | ||
public static final double LENGTH = 32.75; | ||
public static final double WIDTH = 27.75; | ||
public static final double LENGTH = 49.04;// 32.75; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. what are the comments for? |
||
public static final double WIDTH = 34.25;// 27.75; | ||
} | ||
|
||
public static class Component { | ||
public static Arm arm; | ||
public static PDP pdp; | ||
public static Motor crateIORollerLeft; | ||
public static Motor crateIORollerRight; | ||
|
@@ -84,12 +93,15 @@ public static class Component { | |
public static SolenoidShifters shifter; | ||
public static EnableableModifier rightWheelAccelerationCap; | ||
public static EnableableModifier leftWheelAccelerationCap; | ||
public static CustomJoystick operatorStick; | ||
public static CustomXbox driverXbox; | ||
public static CANEncoder leftWheelEncoder; | ||
public static CANEncoder rightWheelEncoder; | ||
public static EncoderPair chassisEncoders; | ||
public static CustomPIDController chassisTurnMC; | ||
public static CustomPIDController drivePID; | ||
public static NavX navx; | ||
public static CANSensor intakeSwitch; | ||
} | ||
|
||
public static class HumanInput { | ||
|
@@ -103,43 +115,58 @@ public static class Operator { | |
} | ||
|
||
public RobotMap() { | ||
Component.joystick = new CustomJoystick(Port.HumanInput.joystick); | ||
/* General */ | ||
Component.pdp = new PDP(); | ||
Component.crateIORollerLeft = new Motor("CrateIORollerLeft", new CANTalonSRX(Port.CANMotor.crateIORollerMotorLeft)); | ||
Component.crateIORollerRight = new Motor("CrateIORollerRight", new CANTalonSRX(Port.CANMotor.crateIORollerMotorRight)); | ||
Component.crateIO = new CrateIO(Component.crateIORollerLeft, Component.crateIORollerRight); | ||
Component.rollyBOIRollerLeft = new Motor("RollyBOIRollerLeft", new CANTalonSRX(Port.CANMotor.rollyBOIRollerMotorLeft)); | ||
Component.rollyBOIRollerLeft.setInverted(true); | ||
Component.rollyBOIRollerRight = new Motor("RollyBOIRollerRight", | ||
new CANTalonSRX(Port.CANMotor.rollyBOIRollerMotorRight)); | ||
Component.rollyBOIGrabber = new RollyBOI.Grabber(Port.Pneumatics.rollyBOIGrabber.buildDoubleSolenoid()); | ||
Component.rollyBOI = new RollyBOI(Component.rollyBOIRollerLeft, Component.rollyBOIRollerRight, | ||
Component.rollyBOIGrabber); | ||
// Wheels | ||
Component.navx = new NavX(SerialPort.Port.kMXP); | ||
/* Chassis */ | ||
// Wheel Encoders | ||
Component.leftWheelEncoder = new CANEncoder("LeftEncoder", Port.CAN.leftEncoder); | ||
Component.rightWheelEncoder = new CANEncoder("RightEncoder", Port.CAN.rightEncoder); | ||
Component.leftWheelEncoder.setDistancePerPulse(Metrics.Wheel.INCHES_PER_TICK); | ||
Component.rightWheelEncoder.setDistancePerPulse(Metrics.Wheel.INCHES_PER_TICK); | ||
Component.chassisEncoders = new EncoderPair(Component.leftWheelEncoder, Component.rightWheelEncoder); | ||
// Acceleration Caps | ||
Component.leftWheelAccelerationCap = new EnableableModifier(new AccelerationCap(Component.pdp)); | ||
Component.leftWheelAccelerationCap.enable(); | ||
Component.rightWheelAccelerationCap = new EnableableModifier(new AccelerationCap(Component.pdp)); | ||
Component.rightWheelAccelerationCap.enable(); | ||
// Wheels | ||
Component.leftWheel = new Motor("LeftWheel", Component.leftWheelAccelerationCap, | ||
new VictorSP(Port.PWM.leftDriveA), new VictorSP(Port.PWM.leftDriveB)); | ||
Component.rightWheel = new Motor("RightWheel", Component.rightWheelAccelerationCap, | ||
new VictorSP(Port.PWM.rightDriveA), new VictorSP(Port.PWM.rightDriveB)); | ||
// Chassis | ||
Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.pcmID, Port.Pneumatics.shifter.forward, | ||
Port.Pneumatics.shifter.reverse); | ||
Component.chassisEncoders = new EncoderPair(Component.leftWheelEncoder, Component.rightWheelEncoder); | ||
Component.shifter = new SolenoidShifters(Port.Pneumatics.shifter.buildDoubleSolenoid()); | ||
Component.chassis = new TankDriveShifting("2018-Chassis", Component.leftWheel, Component.rightWheel, Component.shifter); | ||
/* CrateIO */ | ||
Component.crateIORollerLeft = new Motor("CrateIORollerLeft", new CANTalonSRX(Port.CANMotor.crateIORollerMotorLeft)); | ||
Component.crateIORollerRight = new Motor("CrateIORollerRight", new CANTalonSRX(Port.CANMotor.crateIORollerMotorRight)); | ||
Component.crateIO = new CrateIO(Component.crateIORollerLeft, Component.crateIORollerRight); | ||
/* RollyBoi */ | ||
Component.rollyBOIRollerLeft = new Motor("RollyBOIRollerLeft", new CANTalonSRX(Port.CANMotor.rollyBOIRollerMotorLeft)); | ||
Component.rollyBOIRollerLeft.setInverted(true); | ||
Component.rollyBOIRollerRight = new Motor("RollyBOIRollerRight", | ||
new CANTalonSRX(Port.CANMotor.rollyBOIRollerMotorRight)); | ||
Component.rollyBOIGrabber = new RollyBOI.Grabber(Port.Pneumatics.rollyBOIGrabber.buildDoubleSolenoid()); | ||
Component.rollyBOI = new RollyBOI(Component.rollyBOIRollerLeft, Component.rollyBOIRollerRight, | ||
Component.rollyBOIGrabber); | ||
/* Arm */ | ||
// Encoders | ||
CANEncoder armEncoder = new CANEncoder(Port.CAN.armEncoderPort); | ||
CustomPIDController armController = new CustomPIDController(0.01, 0.00001, -0.001, 0, armEncoder); | ||
armController.setIThreshold(25); | ||
armController.setAbsoluteTolerance(20); | ||
// Motors | ||
CANTalonSRX armA = new CANTalonSRX(Port.CANMotor.armMotorA); | ||
CANTalonSRX armB = new CANTalonSRX(Port.CANMotor.armMotorB); | ||
armB.setInverted(true); | ||
// General | ||
Component.arm = new Arm(armController, new LeakyRelu(Arm.ARM_SPEED_LOWER, Arm.ARM_SPEED_RAISE), armEncoder, | ||
armA, armB); | ||
/* Controllers */ | ||
HumanInput.Driver.xbox = new CustomXbox(Port.HumanInput.xboxController); | ||
HumanInput.Driver.xbox.setDeadZone(HumanInterfaceConfig.XBOX_DEADZONE); | ||
HumanInput.Operator.joystick = new CustomJoystick(Port.HumanInput.joystick); | ||
HumanInput.Operator.joystick.setDeadzone(HumanInterfaceConfig.STICK_LEFT_DEADZONE); | ||
// Controllers | ||
Component.driverXbox = new CustomXbox(Port.HumanInput.xboxController); | ||
Component.driverXbox.setDeadZone(0.1); | ||
} | ||
|
||
public static class PCMPort { | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,32 @@ | ||
package org.usfirst.frc4904.robot.commands; | ||
|
||
|
||
/* | ||
* ArmSet is similar to MotorPositionConstant | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This isn't ArmSet, this is ArmMove. |
||
* but takes an ArmState instead of an absolute position. | ||
*/ | ||
import org.usfirst.frc4904.robot.RobotMap; | ||
import org.usfirst.frc4904.robot.subsystems.Arm; | ||
import org.usfirst.frc4904.standard.commands.motor.MotorPositionConstant; | ||
|
||
public class ArmMove extends MotorPositionConstant { | ||
protected final Arm.ArmState state; | ||
|
||
public ArmMove(Arm.ArmState state, boolean endOnArrival) { | ||
super(RobotMap.Component.arm, state.position, endOnArrival); | ||
this.state = state; | ||
} | ||
|
||
/** | ||
* @see #ArmSet | ||
* endOnArrival defaults to true | ||
*/ | ||
public ArmMove(Arm.ArmState state) { | ||
super(RobotMap.Component.arm, state.position); | ||
this.state = state; | ||
} | ||
|
||
public Arm.ArmState getState() { | ||
return state; | ||
} | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,21 @@ | ||
package org.usfirst.frc4904.robot.commands; | ||
|
||
|
||
import org.usfirst.frc4904.robot.subsystems.Arm; | ||
import edu.wpi.first.wpilibj.command.CommandGroup; | ||
|
||
/** | ||
* | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. This should be a proper javadocs. |
||
* Once ArmMove puts the arm into | ||
* the desired position, the boolean | ||
* endOnArrival is true, causing | ||
* the 'on' value in ArmBrakeSet to | ||
* change to true, extending the | ||
* piston and stopping the arm. | ||
* | ||
*/ | ||
public class ArmSet extends CommandGroup { | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Isn't this command the exact same thing as ArmMove, but with a different name? Do we need this? |
||
public ArmSet(Arm.ArmState state) { | ||
addSequential(new ArmMove(state, true)); | ||
} | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,15 @@ | ||
package org.usfirst.frc4904.robot.commands; | ||
|
||
|
||
import org.usfirst.frc4904.robot.RobotMap; | ||
import org.usfirst.frc4904.robot.subsystems.RollyBOI; | ||
import org.usfirst.frc4904.standard.commands.motor.MotorConstant; | ||
import edu.wpi.first.wpilibj.command.CommandGroup; | ||
|
||
public class IndexerRollersIntake extends CommandGroup { | ||
public IndexerRollersIntake() { | ||
super("IndexerIntake"); | ||
addParallel(new MotorConstant(RobotMap.Component.rollyBOIRollerLeft, RollyBOI.INTAKE_SPEED)); | ||
addParallel(new MotorConstant(RobotMap.Component.rollyBOIRollerRight, RollyBOI.INTAKE_SPEED)); | ||
} | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,15 @@ | ||
package org.usfirst.frc4904.robot.commands; | ||
|
||
|
||
import org.usfirst.frc4904.robot.RobotMap; | ||
import org.usfirst.frc4904.robot.subsystems.RollyBOI; | ||
import org.usfirst.frc4904.standard.commands.motor.MotorConstant; | ||
import edu.wpi.first.wpilibj.command.CommandGroup; | ||
|
||
public class IndexerRollersOuttake extends CommandGroup { | ||
public IndexerRollersOuttake() { | ||
super("IndexerIntake"); | ||
addParallel(new MotorConstant(RobotMap.Component.rollyBOIRollerLeft, RollyBOI.OUTTAKE_SPEED)); | ||
addParallel(new MotorConstant(RobotMap.Component.rollyBOIRollerRight, RollyBOI.OUTTAKE_SPEED)); | ||
} | ||
} |
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,12 +1,13 @@ | ||
package org.usfirst.frc4904.robot.commands; | ||
|
||
|
||
import org.usfirst.frc4904.robot.subsystems.RollyBOI; | ||
import edu.wpi.first.wpilibj.command.CommandGroup; | ||
|
||
public class OuttakeSquared extends CommandGroup { | ||
public OuttakeSquared() { | ||
super("OuttakeSquared"); | ||
addParallel(new IndexerOuttake()); | ||
addParallel(new IndexerOuttake(RollyBOI.OUTTAKE_SPEED)); | ||
addParallel(new IntakeRollersOuttake()); | ||
} | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This should have the correct ports. Either that or this comment shouldn't be here.