Skip to content
This repository has been archived by the owner on Aug 22, 2022. It is now read-only.

Commit

Permalink
Merge pull request #4 from Team100/manipulator
Browse files Browse the repository at this point in the history 
Bag Day Code Freeze
  • Loading branch information
@alexanderbeaver799710
alexanderbeaver799710 authored Feb 20, 2019
2 parents 77c0357 + 0783638 commit 5fe5e5f
Show file tree
Hide file tree
Showing 45 changed files with 2,121 additions and 85 deletions.
10 changes: 10 additions & 0 deletions RobotCode/Team100Robot/Team100_2019Workspace.code-workspace
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,10 @@
{
"folders": [
{
"path": "."
}
],
"settings": {
"java.configuration.updateBuildConfiguration": "automatic"
}
}
2 changes: 1 addition & 1 deletion RobotCode/Team100Robot/build.gradle
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
plugins {
id "java"
id "edu.wpi.first.GradleRIO" version "2019.2.1"
id "edu.wpi.first.GradleRIO" version "2019.3.2"
}

def ROBOT_MAIN_CLASS = "org.usfirst.frc100.Team100Robot.Main"
Expand Down
141 changes: 98 additions & 43 deletions RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Constants.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,74 +19,121 @@ public class Constants {
/*
* CAN/PWM IDs
*/
public static SerialPort.Port NAVX_COMM_PORT = SerialPort.Port.kUSB;
public static final int PCM_CANID = 0;
public static final int DRIVE_TRAIN_LEFT_MASTER_CANID = 0;
public static final int DRIVE_TRAIN_LEFT_FOLLOWER_CANID = 1;
public static final int DRIVE_TRAIN_RIGHT_MASTER_CANID = 15;
public static final int DRIVE_TRAIN_RIGHT_FOLLOWER_CANID = 14;
public static final int ELEVATOR_MASTER_CANID = 10;
public static final int ELEVATOR_FOLLOWER_CANID = 11;
public static final int ELEVATOR_CARRIAGE_SHOULDER_CANID = 2;
public static final int CARGO_PICKUP_TALON_CANID = 3;
public static final int CARGO_PICKUP_TILT_PWM = 6;
public static final int CARGO_PICKUP_ROLLER_PWM = 7;
public static final int HATCH_PICKUP_ROLLER_PWM = 8;
public static final int HATCH_PICKUP_TILT_CANID = 9;
public static final int CARGO_HATCH_SCORER_TOP_CANID = 4;
public static final int CARGO_HATCH_SCORER_BOTTOM_CANID = 5;
public static final int CLIMBER_MASTER_CANID = 12;
public static final int CLIMBER_FOLLOWER_CANID = 13;
public static SerialPort.Port NAVX_COMM_PORT = SerialPort.Port.kUSB; //Type: ENUM
public static final int PCM_CANID = 0; //Type: CAN ID
public static final int DRIVE_TRAIN_LEFT_MASTER_CANID = 0; //Type: CAN ID
public static final int DRIVE_TRAIN_LEFT_FOLLOWER_CANID = 3; //Type: CAN ID
public static final int DRIVE_TRAIN_RIGHT_MASTER_CANID = 15; //Type: CAN ID
public static final int DRIVE_TRAIN_RIGHT_FOLLOWER_CANID = 13; //Type: CAN ID
public static final int ELEVATOR_MASTER_CANID = 11; //Type: CAN ID
public static final int ELEVATOR_FOLLOWER_CANID = 9; //Type: CAN ID
public static final int ELEVATOR_CARRIAGE_SHOULDER_CANID = 1; //Type: CAN ID
public static final int CARGO_PICKUP_TILT_CANID = 2; //Type: CAN ID
public static final int CARGO_PICKUP_ROLLER1_PWM = 6; //Type: PWM ID
public static final int CARGO_PICKUP_ROLLER2_PWM = 7; //Type: PWM ID
public static final int HATCH_PICKUP_ROLLER_PWM = 8; //Type: PWM ID
public static final int HATCH_PICKUP_TILT_CANID = 10; //Type: CAN ID
public static final int CARGO_HATCH_SCORER_TOP_CANID = 5; //Type: CAN ID
public static final int CARGO_HATCH_SCORER_BOTTOM_CANID = 4; //Type: CAN ID
public static final int CLIMBER_MASTER_CANID = 14; //Type: CAN ID
public static final int CLIMBER_FOLLOWER_CANID = 12; //Type: CAN ID

/*
* PNEUMATICS SOLENOID PCM IDs
*/
public static final int DRIVETRAIN_SHIFTER_PCMID = 0;
public static final int LOADING_STATION_INTAKE_PCMID = 1;
public static final int CARGO_GROUND_PICKUP_PCMID = 5;
public static final int HATCH_PICKUP_PCMID = 4;
public static final int CARGO_SCORER_PCMID = 3;
public static final int HATCH_SCORER_PCMID = 2;
public static final int CLIMBER_DEPLOY_PCMID = 6;
public static final int DRIVETRAIN_SHIFTER_PCMID = 5; //Type: PCM ID
public static final int LOADING_STATION_INTAKE_PCMID = 1; //Type: PCM ID
public static final int CARGO_GROUND_PICKUP_PCMID = 6; //Type: PCM ID
public static final int CARGO_GROUND_PICKUP2_PCMID = 7; //Type: PCM ID
public static final int HATCH_PICKUP_PCMID = 2; //Type: PCM ID
public static final int CARGO_SCORER_PCMID = 4; //Type: PCM ID DOESN'T EXIST
public static final int HATCH_SCORER_PCMID = 3; //Type: PCM ID
public static final int CLIMBER_DEPLOY_PCMID = 0; //Type: PCM ID

/*
* Drivetrain
*/
public static final double DRIVE_TRAIN_MAX_MOTOR_OUTPUT = 0.8;
public static final boolean DRIVE_TRAIN_LEFT_MASTER_INVERT = false;
public static final InvertType DRIVE_TRAIN_LEFT_FOLLOWER_INVERT = InvertType.FollowMaster;
public static final boolean DRIVE_TRAIN_RIGHT_MASTER_INVERT = false;
public static final InvertType DRIVE_TRAIN_RIGHT_FOLLOWER_INVERT = InvertType.FollowMaster;
public static final boolean ARCADE_DRIVE_MODE = true;
public static final double DRIVE_TRAIN_MAX_MOTOR_OUTPUT = 0.8; //Type: Percent Output (-1-1)
public static final boolean DRIVE_TRAIN_LEFT_MASTER_INVERT = false; //Type: Boolean
public static final InvertType DRIVE_TRAIN_LEFT_FOLLOWER_INVERT = InvertType.FollowMaster; //Type: ENUM
public static final boolean DRIVE_TRAIN_RIGHT_MASTER_INVERT = false; //Type: Boolean
public static final InvertType DRIVE_TRAIN_RIGHT_FOLLOWER_INVERT = InvertType.FollowMaster; //Type: ENUM
public static final boolean ARCADE_DRIVE_MODE = true; //Type: Boolean
//PID Rotation with NavX
public static final double DT_TURN_P = 0.007;
public static final double DT_TURN_I = 0.00004;
public static final double DT_TURN_D = 0.002;
public static final double DT_TURN_F = 0;
public static final double DT_TURN_MIN_ROTATION_ANGLE = -180;
public static final double DT_TURN_MAX_ROTATION_ANGLE = 180;
public static final double DT_TURN_MIN_OUTPUT = -1;
public static final double DT_TURN_MAX_OUTPUT = 1;
public static final double DT_TURN_ABSOLUTE_TOLERANCE = 1;
public static final double DT_TURN_MOTOR_OUTPUT = 0.5;
public static final double DT_TURN_P = 0.007; //Type: PIDF Double
public static final double DT_TURN_I = 0.00004; //Type: PIDF Double
public static final double DT_TURN_D = 0.002; //Type: PIDF Double
public static final double DT_TURN_F = 0; //Type: PIDF Double
public static final double DT_TURN_MIN_ROTATION_ANGLE = -180; //Type: Degrees
public static final double DT_TURN_MAX_ROTATION_ANGLE = 180; //Type: Degrees
public static final double DT_TURN_MIN_OUTPUT = -1; //Type: Percent Output (-1-1)
public static final double DT_TURN_MAX_OUTPUT = 1; //Type: Percent Output (-1-1)
public static final double DT_TURN_ABSOLUTE_TOLERANCE = 1; //Type: Degrees
public static final double DT_TURN_MOTOR_OUTPUT = 0.5; //Type: Percent Output (-1-1)

/*
* Elevator
*/

/**Power to apply when going down during homing sequence... <em>keep very small</em>*/
public static final double HOMING_GOING_DOWN_POWER = -.3; //Type: Percent Output (-1-1)
/**Power to apply when going up during homing sequence... <em>keep very small</em> */
public static final double HOMING_GOING_UP_POWER = .5; //Type: Percent Output (-1-1)
/**Buffer in elevator target for acceptable destination */
public static final int ELEVATOR_POSITION_BUFFER = 85; //Type: Encoder Ticks

/**
* Elevator Timeout in Milliseconds
*/
public static final int ELEVATOR_MASTER_TIMEOUT = 10; //Type: Milliseconds


////////////// PID

/**Elevator PID P */
public static final double ELEVATOR_KP = 2; //Type: PIDF Double

/**Elevator PID I */
public static final double ELEVATOR_KI = 0; //Type: PIDF Double

/**Elevator PID D */
public static final double ELEVATOR_KD = 0; //Type: PIDF Double

/**Elevator PID F */
public static final double ELEVATOR_KF = 1; //Type: PIDF Double


/**Conversion factor from Inches to Encoder ticks
* For elevator control
*/
public static final int ELEVATOR_INCH_TO_ENCODER_CONVERSION_FACTION = 86; //Type: Encoder Ticks

public static final double ELEVATOR_START_HEIGHT_IN_INCHES = 16.5; //Type: Inches
public static final double ELEVATOR_MAX_HEIGHT_IN_INCHES = 78.5; //Type: Inches
/*
* Elevator Carriage
*/

/*
* Shoulder
*/
public static final int SHOULDER_MASTER_TIMEOUT = 10; //Type: Milliseconds

public static final double SHOULDER_KP = 0.001; //Type: PIDF Double
public static final double SHOULDER_KI = 0; //Type: PIDF Double
public static final double SHOULDER_KD = 0; //Type: PIDF Double
public static final double SHOULDER_KF = 0; //Type: PIDF Double
public static final int SHOULDER_TIMEOUT = 10; //Type: Milliseconds
public static final int SHOULDER_BUFFER = 10; //Type: Encoder Ticks
/*
* Cargo Floor Pickup
*/

/*
* Hatch Floor Pickup
*/
public static final double HATCH_PICKUP_INTAKE_SPEED = 0.6;
public static final double HATCH_PICKUP_TILT_SPEED = 0.2;
public static final double HATCH_PICKUP_INTAKE_SPEED = 0.6; //Type: Percent Output (-1-1)
public static final double HATCH_PICKUP_TILT_SPEED = 0.2; //Type: Percent Output (-1-1)
public static final int HATCH_PICKUP_UPPER_LIMIT_SWITCH_SLOT = 0;
public static final int HATCH_PICKUP_LOWER_LIMIT_SWITCH_SLOT = 1;

Expand All @@ -98,4 +145,12 @@ public class Constants {
* Climber
*/

/*
* Cargo Manipulator
*/
public static final int CARGO_MANIPULATOR_TOP_TALONSRX_ID = 5;
public static final int CARGO_MANIPULATOR_BOTTOM_TALONSRX_ID = 4;
public static final double CARGO_MANIPULATOR_INTAKE_SPEED = 1;
public static final double CARGO_MANIPULATOR_OUTTAKE_SPEED = -1;

}
49 changes: 44 additions & 5 deletions RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/OI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,19 @@
package org.usfirst.frc100.Team100Robot;

import org.usfirst.frc100.Team100Robot.commands.*;
import org.usfirst.frc100.Team100Robot.commands.CargoManipulator.CargoManipulatorOuttake;
import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Drive;
import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Shift.ShiftToHigh;
import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Shift.ShiftToLow;
import org.usfirst.frc100.Team100Robot.commands.Elevator.ElevatorPageDown;
import org.usfirst.frc100.Team100Robot.commands.Elevator.ElevatorPageUp;
import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Bill.BillLower;
import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Bill.BillRaise;
import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Pusher.*;
import org.usfirst.frc100.Team100Robot.commands.IntakeArm.IntakeArmIntakeElement;
import org.usfirst.frc100.Team100Robot.commands.Shoulder.ShoulderDown;
import org.usfirst.frc100.Team100Robot.commands.Shoulder.ShoulderUp;

import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
Expand Down Expand Up @@ -52,6 +65,7 @@ public class OI {
private Joystick leftStick;
private Joystick rightStick;
private Joystick manipulatorControl;
private Joystick testingJS;

private JoystickButton shiftLow;
private JoystickButton shiftHigh;
Expand All @@ -66,6 +80,7 @@ public class OI {
private JoystickButton elevatorStageUp;
private JoystickButton hatchRelease;
private JoystickButton elevatorStageDown;


public OI() {

Expand All @@ -75,16 +90,17 @@ public OI() {
leftStick = new Joystick(0);
rightStick = new Joystick(1);
manipulatorControl = new Joystick(2);
testingJS = new Joystick(4);


/*
* Joystick 0 (Left Stick)
*/
spitCargo = new JoystickButton(leftStick, 1);
spitCargo.whenPressed(new SpitCargo());
spitCargo.whileHeld(new CargoManipulatorOuttake());

shiftLow = new JoystickButton(leftStick, 3);
shiftLow.whenPressed(new LowGear());
shiftLow.whenPressed(new ShiftToLow());

hatchUp = new JoystickButton(leftStick, 4);

Expand All @@ -94,13 +110,13 @@ public OI() {
intakeCargo = new JoystickButton(rightStick, 1);

shiftHigh = new JoystickButton(rightStick, 3);
shiftHigh.whenPressed(new HighGear());
shiftHigh.whenPressed(new ShiftToHigh());

cargoUp = new JoystickButton(rightStick, 4);
cargoUp.whenPressed(new CargoUp());
cargoUp.whenPressed(new ShoulderUp());

cargoDown = new JoystickButton(rightStick, 5);
cargoDown.whenPressed(new CargoDown());
cargoDown.whenPressed(new ShoulderDown());

//Joystick 2 (Manipulator Control)
climberToggle = new JoystickButton(manipulatorControl, 4);
Expand All @@ -114,6 +130,26 @@ public OI() {
elevatorStageDown = new JoystickButton(manipulatorControl, 8);




elevatorStageUp.whenPressed(new ElevatorPageUp());
elevatorStageDown.whenPressed(new ElevatorPageDown());
intakeCargo.whileActive(new IntakeArmIntakeElement());


/*
* Testing Joystick __(will not be used for comp)___
*/
JoystickButton duckBillExtend = new JoystickButton(testingJS, 1);
duckBillExtend.whileActive(new BillRaise());
JoystickButton duckBillRetract = new JoystickButton(testingJS,2);
duckBillRetract.whileActive(new BillLower());
JoystickButton pusherExtend = new JoystickButton(testingJS, 3);
pusherExtend.whileActive(new ExtendPusher());
JoystickButton pusherRetract = new JoystickButton(testingJS, 4);
pusherRetract.whileActive(new RetractPusher());



// SmartDashboard Buttons
SmartDashboard.putData("Autonomous Command", new AutonomousCommand());
Expand All @@ -133,6 +169,9 @@ public Joystick getRightStick() {
public Joystick getManipulatorControl() {
return manipulatorControl;
}
public Joystick getTestingJS(){
return testingJS;
}

}

30 changes: 28 additions & 2 deletions RobotCode/Team100Robot/src/main/java/org/usfirst/frc100/Team100Robot/Robot.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,15 @@
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import org.usfirst.frc100.Team100Robot.commands.*;
import org.usfirst.frc100.Team100Robot.commands.CargoManipulator.CargoManipulatorIntake;
import org.usfirst.frc100.Team100Robot.commands.CargoManipulator.CargoManipulatorOuttake;
import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Shift.ShiftToHigh;
import org.usfirst.frc100.Team100Robot.commands.Drivetrain.Shift.ShiftToLow;
import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Bill.BillLower;
import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Bill.BillRaise;
import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Pusher.ExtendPusher;
import org.usfirst.frc100.Team100Robot.commands.HatchManipulator.Pusher.RetractPusher;
import org.usfirst.frc100.Team100Robot.commands.IntakeArm.IntakeArmIntakeElement;
import org.usfirst.frc100.Team100Robot.subsystems.*;
import com.kauailabs.navx.frc.*;

Expand All @@ -44,14 +53,15 @@ public class Robot extends TimedRobot {
public static Elevator elevator;
public static double currentHeading;
public static AHRS ahrs;
public static Manipulator manipulator;

/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
*/
@Override
public void robotInit() {
ahrs = new AHRS(Constants.NAVX_COMM_PORT);
// ahrs = new AHRS(Constants.NAVX_COMM_PORT);

drivetrain = new Drivetrain();
shifter = new Shifter();
Expand All @@ -61,6 +71,7 @@ public void robotInit() {
cargoPickup = new CargoPickup();
hatchPickup = new HatchPickup();
elevator = new Elevator();
manipulator = new Manipulator();

// OI must be constructed after subsystems. If the OI creates Commands
//(which it very likely will), subsystems are not guaranteed to be
Expand All @@ -72,6 +83,21 @@ public void robotInit() {
chooser.setDefaultOption("Autonomous Command", new AutonomousCommand());

SmartDashboard.putData("Auto mode", chooser);





// Put Command Triggers
SmartDashboard.putData("CargoManipIntake",new CargoManipulatorIntake());
SmartDashboard.putData("CargoManipOuttake",new CargoManipulatorOuttake());
SmartDashboard.putData("DTShiftToHigh",new ShiftToHigh());
SmartDashboard.putData("DTShiftToLow",new ShiftToLow());
SmartDashboard.putData("BillRaise",new BillRaise());
SmartDashboard.putData("BillLower",new BillLower());
SmartDashboard.putData("PusherExtend",new ExtendPusher());
SmartDashboard.putData("PusherRetract",new RetractPusher());
SmartDashboard.putData("IntakeElement", new IntakeArmIntakeElement());
}

/**
Expand Down Expand Up @@ -119,7 +145,7 @@ public void teleopInit() {
@Override
public void teleopPeriodic() {
Scheduler.getInstance().run();
currentHeading = ahrs.getFusedHeading();
//currentHeading = ahrs.getFusedHeading();
}

public static double getCurrentHeading(){
Expand Down
Loading

0 comments on commit 5fe5e5f

Please sign in to comment.