Skip to content
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

Open
wants to merge 99 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
99 commits
Select commit Hold shift + click to select a range
b754c2d
Field of daisies
oldgalileo Jan 7, 2018
9e87a08
Updated fmsData and field reference in field
billwpierce Jan 7, 2018
ac6125d
Added Field stuff
oldgalileo Jan 7, 2018
31f4f2c
Errors begone
oldgalileo Jan 7, 2018
9c958fd
Added autonconfig file
billwpierce Jan 7, 2018
8b16298
Initialize nearby switch time auton routine
billwpierce Jan 7, 2018
5fddbe8
Mixed merge conflict
billwpierce Jan 7, 2018
a3983d3
Added auton shell. Stale branch until prototype code finished
billwpierce Jan 7, 2018
ddc716b
Added basic driving code without xbox
billwpierce Jan 9, 2018
3740195
Added driver and chassis to robot and robotmap
billwpierce Jan 9, 2018
9d37333
Removed logger
billwpierce Jan 9, 2018
d12bfd1
renamed driver class, removed useless command
billwpierce Jan 9, 2018
a9b52bf
renamed to skrt skrt
billwpierce Jan 9, 2018
cc6547b
rerenamed to nathangain
billwpierce Jan 9, 2018
b17a3f0
Merge branch 'master' into skrtskrt
billwpierce Jan 9, 2018
77a9f2b
Merge pull request #4 from RoboticsTeam4904/skrtskrt
oldgalileo Jan 9, 2018
882a760
Merge branch 'master' into dead-reckoning
billwpierce Jan 9, 2018
360851b
updated time to unknown
billwpierce Jan 9, 2018
16d45f7
added todo
billwpierce Jan 9, 2018
dfa4882
changed standard version
billwpierce Jan 9, 2018
932675b
Refactored
billwpierce Jan 11, 2018
894f00b
Added our alliance color to the field class
billwpierce Jan 11, 2018
ab03886
Added runif command to left switch auton
billwpierce Jan 11, 2018
9864c01
Added left and right subclasses of switch time.
Jan 11, 2018
ca6a985
Fixed reversal of sides.
Jan 11, 2018
4f91233
Renamed auton versions
billwpierce Jan 11, 2018
8574b49
Added tick and inch conversion to robotmap
billwpierce Jan 12, 2018
52db5a2
move wheel metrics to wheel subsubclass
billwpierce Jan 12, 2018
f0b6204
Added encoders to robotmap
billwpierce Jan 12, 2018
67f807d
Added encoder pair for wheels
billwpierce Jan 12, 2018
88319b9
Added Pid controller for distance auton
billwpierce Jan 12, 2018
4bbbe6f
Added side specific distance
billwpierce Jan 12, 2018
a8050f2
changed added libraries
billwpierce Jan 13, 2018
cb00a95
Changes to auton structure including the abstract strategy class and …
billwpierce Jan 15, 2018
af142a7
reference made static
billwpierce Jan 15, 2018
04cf4f7
fixed if/else error from commandgroup
billwpierce Jan 15, 2018
bda4fff
reordered sideswitchdistance
billwpierce Jan 15, 2018
9622e63
Added strategy class
billwpierce Jan 15, 2018
0a8193f
fixed compiler errors from logkitten and missing strategy import
billwpierce Jan 15, 2018
1f7c8de
removed unused imports
billwpierce Jan 15, 2018
1cced3c
removed variables from 2017
billwpierce Jan 15, 2018
2d60239
redid rightswitch code, and replaced LogKitten.wtf's with noop's
billwpierce Jan 15, 2018
826f472
removed unnecessary classpath line
billwpierce Jan 15, 2018
3513f42
Added MidSwitchDistance class for auton.
Jan 16, 2018
188857c
Replaced two RunIfs with a RunIfElse.
Jan 16, 2018
a1b8457
Added left/right side testing to the piece class
billwpierce Jan 18, 2018
740fadc
Implemented left/right side testing function
billwpierce Jan 18, 2018
4680d66
Added documentation to the field class
billwpierce Jan 18, 2018
4db86a5
removed empty docs
billwpierce Jan 18, 2018
616a113
Merge branch 'dead-reckoning' into pinky-testing
billwpierce Jan 18, 2018
8772639
Merge branch 'dead-reckoning' into pinky-testing
billwpierce Jan 18, 2018
11d3094
Updated CenterSwitchDistance() for new strategy implementation
billwpierce Jan 18, 2018
bf523ca
Removed unused imports for no other reason than my sanity
billwpierce Jan 18, 2018
687297b
formatting
billwpierce Jan 18, 2018
24998bb
Merge branch 'master' into pinky-testing
billwpierce Jan 19, 2018
6c64bfa
updated metrics
billwpierce Jan 19, 2018
2ddb37a
reformatted javadocs
billwpierce Jan 19, 2018
0ee8f2b
updated isSideOurs() functions for the offchance that the alliance is…
billwpierce Jan 19, 2018
493197a
Added explicit super constructors
billwpierce Jan 19, 2018
a4ca06d
Merge pull request #13 from RoboticsTeam4904/pinky-testing
oldgalileo Jan 19, 2018
c6a66e9
Added command to make robot perpendicular with switch upon arriving i…
Jan 21, 2018
fe75062
removed unnecesary variable
billwpierce Jan 23, 2018
2045b86
Added general intake with adjustments and limit switch
billwpierce Feb 18, 2018
5dc9fa8
auton tuned
billwpierce Feb 18, 2018
8b49d80
testing
billwpierce Feb 19, 2018
c53b464
from auton testing; BROKEN
billwpierce Feb 19, 2018
8b3aa1d
removed repetitive joystick
billwpierce Feb 19, 2018
7c9cbc3
debugging time auton and adding distance auton with chassis encoders
oshlern Feb 19, 2018
c0d18f4
Merge branch 'master' into auton-testing
billwpierce Feb 20, 2018
f739993
testing
billwpierce Feb 20, 2018
c5de4c4
New go to cube pile
Feb 21, 2018
2daba2e
testing
billwpierce Feb 21, 2018
5d35340
Merge branch 'auton-testing' of https://github.com/RoboticsTeam4904/2…
billwpierce Feb 21, 2018
0d1beba
testing from prog meeting
billwpierce Feb 26, 2018
fca7b45
can reader fix
billwpierce Mar 3, 2018
26eeb8e
Cleaned up Robot.java
billwpierce Mar 3, 2018
ca29073
Cleaned up RobotMap.java
billwpierce Mar 3, 2018
453ff58
arm setpts
ajnadel Mar 4, 2018
9080d54
Osher's code
billwpierce Mar 4, 2018
62d6f65
testing variable arm speed
billwpierce Mar 5, 2018
e697322
testing
billwpierce Mar 5, 2018
91aec1b
Put far side switch strategies in folder, added far side scale strate…
lauwong Mar 6, 2018
8c9928d
Commented switch strategies and added far left scale strategies + far…
lauwong Mar 6, 2018
93fad97
Fixed FarLeftScaleLeftDistance turning and added FarRightScale strate…
lauwong Mar 6, 2018
254c04f
added strategies for outtake to declutter auton
oshlern Mar 6, 2018
71f6766
Merge branch 'auton-testing' of https://github.com/RoboticsTeam4904/2…
oshlern Mar 6, 2018
aefc32b
Added new variables and values for the dimensions of the field.
Krakavius Mar 6, 2018
1328385
Merge branch 'auton-testing' of https://github.com/RoboticsTeam4904/2…
Krakavius Mar 6, 2018
84c0cfa
improved scale strategies and implemented and documented use of outta…
oshlern Mar 6, 2018
9bf5fd1
Merge branch 'auton-testing' of https://github.com/RoboticsTeam4904/2…
oshlern Mar 6, 2018
dfdee30
Edited operator controls
billwpierce Mar 7, 2018
3fb0a60
Merge branch 'auton-testing' of https://github.com/RoboticsTeam4904/2…
billwpierce Mar 7, 2018
7204acb
Removed things that don't need to be merged.
Mar 9, 2018
caee701
Added speedmodifier to arm.
Mar 9, 2018
5338840
Removed general stuff.
Mar 9, 2018
684637a
Removed disk brake
Mar 9, 2018
12bf890
Merge branch 'master' into arm-auton-edits
Codadillo Mar 10, 2018
c3caf69
Responded to some review comments.
Mar 10, 2018
7c607c3
Merge branch 'arm-auton-edits' of https://github.com/RoboticsTeam4904…
Mar 10, 2018
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 17 additions & 0 deletions .classpath
Original file line number Diff line number Diff line change
Expand Up @@ -11,5 +11,22 @@
<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix.jar" sourcepath="USERLIBS_DIR/CTRE_Phoenix-sources.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/CTRE_Phoenix-sources.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/navx_frc-sources.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/commons-lang3-3.0.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/json-20171018.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/jsr80-1.0.1.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/libusb4java-1.2.0-linux-arm.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/libusb4java-1.2.0-linux-x86.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/libusb4java-1.2.0-linux-x86_64.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/libusb4java-1.2.0-osx-x86.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/libusb4java-1.2.0-osx-x86_64.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/libusb4java-1.2.0-windows-x86.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/libusb4java-1.2.0-windows-x86_64.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/libusb4java-1.2.0.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/usb4java-1.2.0-javadoc.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/usb4java-1.2.0-sources.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/usb4java-1.2.0.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/usb4java-javax-1.2.0-javadoc.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/usb4java-javax-1.2.0-sources.jar"/>
<classpathentry kind="var" path="USERLIBS_DIR/usb4java-javax-1.2.0.jar"/>
<classpathentry kind="output" path="bin"/>
</classpath>
77 changes: 52 additions & 25 deletions src/org/usfirst/frc4904/robot/RobotMap.java
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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
Copy link
Author

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.

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;
Expand All @@ -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 {
Expand All @@ -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
Expand All @@ -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;
Copy link
Author

Choose a reason for hiding this comment

The 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;
Expand All @@ -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 {
Expand All @@ -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 {
Expand Down
32 changes: 32 additions & 0 deletions src/org/usfirst/frc4904/robot/commands/ArmMove.java
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
Copy link
Author

Choose a reason for hiding this comment

The 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;
}
}
21 changes: 21 additions & 0 deletions src/org/usfirst/frc4904/robot/commands/ArmSet.java
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;

/**
*
Copy link
Author

Choose a reason for hiding this comment

The 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 {
Copy link
Author

Choose a reason for hiding this comment

The 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));
}
}
7 changes: 3 additions & 4 deletions src/org/usfirst/frc4904/robot/commands/IndexerOuttake.java
Original file line number Diff line number Diff line change
Expand Up @@ -2,15 +2,14 @@


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 IndexerOuttake extends CommandGroup {
public IndexerOuttake() {
public IndexerOuttake(double speed) {
super("IndexerOuttake");
addParallel(new IndexerGrabberRelease());
addParallel(new MotorConstant(RobotMap.Component.rollyBOI.rollerLeft, RollyBOI.OUTTAKE_SPEED));
addParallel(new MotorConstant(RobotMap.Component.rollyBOI.rollerRight, RollyBOI.OUTTAKE_SPEED));
addParallel(new MotorConstant(RobotMap.Component.rollyBOI.rollerLeft, speed));
addParallel(new MotorConstant(RobotMap.Component.rollyBOI.rollerRight, speed));
}
}
15 changes: 15 additions & 0 deletions src/org/usfirst/frc4904/robot/commands/IndexerRollersIntake.java
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));
}
}
15 changes: 15 additions & 0 deletions src/org/usfirst/frc4904/robot/commands/IndexerRollersOuttake.java
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));
}
}
3 changes: 2 additions & 1 deletion src/org/usfirst/frc4904/robot/commands/OuttakeSquared.java
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());
}
}
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,9 @@ protected double scaleGain(double input, double gain, double exp) {

@Override
public void bindCommands() {
RobotMap.Component.driverXbox.lb
RobotMap.HumanInput.Driver.xbox.lb
.whenPressed(new ChassisShift(RobotMap.Component.chassis.getShifter(), SolenoidShifters.ShiftState.DOWN));
RobotMap.Component.driverXbox.rb
RobotMap.HumanInput.Driver.xbox.rb
.whenPressed(new ChassisShift(RobotMap.Component.chassis.getShifter(), SolenoidShifters.ShiftState.UP));
}

Expand All @@ -37,15 +37,15 @@ public double getX() {

@Override
public double getY() {
double rawSpeed = RobotMap.Component.driverXbox.rt.getX() - RobotMap.Component.driverXbox.lt.getX();
double rawSpeed = RobotMap.HumanInput.Driver.xbox.rt.getX() - RobotMap.HumanInput.Driver.xbox.lt.getX();
double speed = scaleGain(rawSpeed, NathanGain.SPEED_GAIN, NathanGain.SPEED_EXP)
* NathanGain.Y_SPEED_SCALE;
return speed;
}

@Override
public double getTurnSpeed() {
double rawTurnSpeed = RobotMap.Component.driverXbox.leftStick.getX();
double rawTurnSpeed = RobotMap.HumanInput.Driver.xbox.leftStick.getX();
double turnSpeed = scaleGain(rawTurnSpeed, NathanGain.TURN_GAIN, NathanGain.TURN_EXP)
* NathanGain.TURN_SPEED_SCALE;
return turnSpeed;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,15 @@


import org.usfirst.frc4904.robot.RobotMap;
import org.usfirst.frc4904.robot.commands.ArmMove;
import org.usfirst.frc4904.robot.commands.IndexerRollersIntake;
import org.usfirst.frc4904.robot.commands.IndexerRollersOuttake;
import org.usfirst.frc4904.robot.commands.IntakeSquared;
import org.usfirst.frc4904.robot.commands.OuttakeSquared;
import org.usfirst.frc4904.robot.subsystems.Arm;
import org.usfirst.frc4904.standard.commands.SingleOp;
import org.usfirst.frc4904.standard.commands.motor.MotorControl;
import org.usfirst.frc4904.standard.custom.controllers.CustomJoystick;
import org.usfirst.frc4904.standard.humaninput.Operator;

public class DefaultOperator extends Operator {
Expand All @@ -17,7 +24,18 @@ public DefaultOperator(String name) {

@Override
public void bindCommands() {
RobotMap.Component.joystick.button3.onlyWhileHeld(new IntakeSquared());
RobotMap.Component.joystick.button4.onlyWhileHeld(new OuttakeSquared());
// RobotMap.HumanInput.Operator.joystick.button1.onlyWhileReleased(new ArmBrakeSet(true));
RobotMap.HumanInput.Operator.joystick.button1.onlyWhileHeld(
new MotorControl(RobotMap.Component.arm, RobotMap.HumanInput.Operator.joystick, CustomJoystick.Y_AXIS));
RobotMap.HumanInput.Operator.joystick.button3.onlyWhileHeld(new IntakeSquared());
RobotMap.HumanInput.Operator.joystick.button4.onlyWhileHeld(new OuttakeSquared());
RobotMap.HumanInput.Operator.joystick.button5.onlyWhileHeld(new IndexerRollersIntake());
RobotMap.HumanInput.Operator.joystick.button6.onlyWhileHeld(new IndexerRollersOuttake());
RobotMap.HumanInput.Operator.joystick.button9.whenPressed(new SingleOp(() -> {
RobotMap.Component.arm.encoder.reset();
}));
RobotMap.HumanInput.Operator.joystick.button8.onlyWhileHeld(new ArmMove(Arm.ArmState.ARM_POSITION_SCALE, true));
RobotMap.HumanInput.Operator.joystick.button10.onlyWhileHeld(new ArmMove(Arm.ArmState.ARM_POSITION_SWITCH, true));
RobotMap.HumanInput.Operator.joystick.button12.onlyWhileHeld(new ArmMove(Arm.ArmState.ARM_POSITION_INTAKE, true));
}
}
Loading