Skip to content
This repository was archived by the owner on Sep 14, 2019. It is now read-only.

Finished initialization of RobotMap, finished getting Drivetrain to i… #18

Merged
kevinzwang merged 1 commit into from
Jan 15, 2018
Merged

Finished initialization of RobotMap, finished getting Drivetrain to i… #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
22 changes: 11 additions & 11 deletions Robot2018/src/org/usfirst/frc/team199/Robot2018/OI.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,8 @@

package org.usfirst.frc.team199.Robot2018;

import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;
import org.usfirst.frc.team199.Robot2018.commands.ShiftDriveType;
import org.usfirst.frc.team199.Robot2018.commands.ShiftLowGear;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
Expand Down Expand Up @@ -46,26 +46,26 @@ public class OI {
// Start the command when the button is released and let it run the command
// until it is finished as determined by it's isFinished method.
// button.whenReleased(new ExampleCommand());

public Joystick leftJoy;
public JoystickButton shiftDrive;
public JoystickButton shiftDriveType;
public JoystickButton shiftDrive;
public JoystickButton shiftDriveType;
public Joystick rightJoy;
public Joystick manipulator;



public int getButton(String key, int def) {
if(!SmartDashboard.containsKey(key)) {
if (!SmartDashboard.containsKey(key)) {
SmartDashboard.putNumber(key, def);
}
return (int) SmartDashboard.getNumber(key, def);
}

public OI() {
leftJoy = new Joystick(0);
shiftDrive = new JoystickButton(leftJoy, getButton("Button Shift Drive", 1));
shiftDrive.whenPressed(new ShiftLowGear());
shiftDriveType = new JoystickButton(leftJoy, getButton("Button Shift Drive Type", 2));
shiftDriveType.whenPressed(new ShiftDriveType());
shiftDrive = new JoystickButton(leftJoy, getButton("Button Shift Drive", 1));
shiftDrive.whenPressed(new ShiftLowGear());
shiftDriveType = new JoystickButton(leftJoy, getButton("Button Shift Drive Type", 2));
shiftDriveType.whenPressed(new ShiftDriveType());
rightJoy = new Joystick(1);
manipulator = new Joystick(2);
}
Expand Down
61 changes: 31 additions & 30 deletions Robot2018/src/org/usfirst/frc/team199/Robot2018/Robot.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,6 @@
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.command.Scheduler;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

Expand All @@ -24,50 +23,52 @@
* directory.
*/
public class Robot extends IterativeRobot {

public static final Climber climber = new Climber();
public static final ClimberAssist climberAssist = new ClimberAssist();
public static final IntakeEject intakeEject = new IntakeEject();
public static final Lift lift = new Lift();
public static RobotMap rmap;
public static Drivetrain dt;

public static OI oi;

Command autonomousCommand;
SendableChooser<Command> chooser = new SendableChooser<>();


public static double getConst(String key, double def) {
if(!SmartDashboard.containsKey(key)) {
if (!SmartDashboard.containsKey(key)) {
SmartDashboard.putNumber(key, def);
}
return SmartDashboard.getNumber(key, def);
}

public void sendValuesToDashboard() {
ArrayList<String> boolKeys = new ArrayList<String>();
ArrayList<Boolean> boolDef = new ArrayList<Boolean>();
boolKeys.add("Arcade Drive");
boolKeys.add("Arcade Drive Default Setup");
boolKeys.add("Square Drive Values");
boolKeys.add("High Gear");
boolDef.add(true);
boolDef.add(true);
boolDef.add(false);
boolDef.add(false);
for(int i = 0; i < boolKeys.size(); i++) {
if(!SmartDashboard.containsKey(boolKeys.get(i))) {
boolKeys.add("Arcade Drive");
boolKeys.add("Arcade Drive Default Setup");
boolKeys.add("Square Drive Values");
boolKeys.add("High Gear");

boolDef.add(true);
boolDef.add(true);
boolDef.add(false);
boolDef.add(false);
for (int i = 0; i < boolKeys.size(); i++) {
if (!SmartDashboard.containsKey(boolKeys.get(i))) {
SmartDashboard.putBoolean(boolKeys.get(i), boolDef.get(i));
}
}
}

/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
* This function is run when the robot is first started up and should be used
* for any initialization code.
*/
@Override
public void robotInit() {
rmap = new RobotMap();
dt = new Drivetrain();
oi = new OI();
sendValuesToDashboard();
Expand All @@ -76,9 +77,9 @@ public void robotInit() {
}

/**
* 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.
* 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() {
Expand All @@ -92,24 +93,24 @@ public void disabledPeriodic() {

/**
* 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
* 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.
* chooser code above (like the commented example) or additional comparisons to
* the switch structure below with additional strings & commands.
*/
@Override
public void autonomousInit() {
autonomousCommand = chooser.getSelected();

/*
* String autoSelected = SmartDashboard.getString("Auto Selector",
* "Default"); switch(autoSelected) { case "My Auto": autonomousCommand
* = new MyAutoCommand(); break; case "Default Auto": default:
* autonomousCommand = new ExampleCommand(); break; }
* String autoSelected = SmartDashboard.getString("Auto Selector", "Default");
* switch(autoSelected) { case "My Auto": autonomousCommand = new
* MyAutoCommand(); break; case "Default Auto": default: autonomousCommand = new
* ExampleCommand(); break; }
*/

// schedule the autonomous command (example)
Expand Down
113 changes: 63 additions & 50 deletions Robot2018/src/org/usfirst/frc/team199/Robot2018/RobotMap.java
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,91 +35,104 @@ public class RobotMap {
// number and the module. For example you with a rangefinder:
// public static int rangefinderPort = 1;
// public static int rangefinderModule = 1;

public static Encoder leftEnc;
public static WPI_TalonSRX dtLeftDrive;
public static WPI_TalonSRX dtLeftSlave;
public static WPI_TalonSRX dtLeftSlave;
public static SpeedControllerGroup dtLeft;

public static Encoder rightEnc;
public static WPI_TalonSRX dtRightDrive;
public static WPI_TalonSRX dtRightSlave;
public static WPI_TalonSRX dtRightSlave;
public static SpeedControllerGroup dtRight;
public static DifferentialDrive robotDrive;
public static PIDController turnController;
public static PIDController moveController;
// public static PIDController moveLeftController;
// public static PIDController moveRightController;
// public static PIDController moveLeftController;
// public static PIDController moveRightController;

public static AHRS ahrs;
public static AnalogGyro dtGyro;
public static DoubleSolenoid dtGear;

private void configSRX(WPI_TalonSRX mc) {
int kTimeout = (int) Robot.getConst("ConstkTimeoutMs", 10);
mc.configNominalOutputForward(0, kTimeout);
mc.configNominalOutputReverse(0, kTimeout);
mc.configPeakOutputForward(1, kTimeout);
mc.configPeakOutputReverse(-1, kTimeout);
mc.configNominalOutputReverse(0, kTimeout);
mc.configPeakOutputForward(1, kTimeout);
mc.configPeakOutputReverse(-1, kTimeout);
}

public RobotMap() {

leftEnc = new Encoder(getPort("Port1LeftEnc", 0), getPort("Port2LeftEnc", 1));
dtLeftDrive = new WPI_TalonSRX(getPort("PortLeftTalonSRXDrive", 1));
configSRX(dtLeftDrive);
dtLeftSlave = new WPI_TalonSRX(getPort("PortLeftTalonSRXSlave", 1));
configSRX(dtLeftSlave);
dtLeftSlave = new WPI_TalonSRX(getPort("PortLeftTalonSRXSlave", 1));
configSRX(dtLeftSlave);
dtLeft = new SpeedControllerGroup(dtLeftDrive, dtLeftSlave);

rightEnc = new Encoder(getPort("Port1RightEnc", 2), getPort("Port2RightEnc", 3));
dtRightDrive = new WPI_TalonSRX(getPort("PortRightTalonSRXDrive", 2));
configSRX(dtRightDrive);
dtRightSlave = new WPI_TalonSRX(getPort("PortRightTalonSRXSlave", 1));
configSRX(dtRightSlave);
dtRightSlave = new WPI_TalonSRX(getPort("PortRightTalonSRXSlave", 1));
configSRX(dtRightSlave);
dtRight = new SpeedControllerGroup(dtRightDrive, dtRightSlave);

robotDrive = new DifferentialDrive(dtLeft, dtRight);
turnController = new PIDController(Robot.getConst("ConstTurnkP", 1), Robot.getConst("ConstTurnkI", 0), Robot.getConst("ConstTurnkD", 0), ahrs, Robot.dt);
turnController.disable();
turnController.setInputRange(-180, 180);
turnController.setOutputRange(-1.0, 1.0);
turnController.setContinuous();
turnController.setAbsoluteTolerance(Robot.getConst("ConstTurnTolerance", 1));
moveController = new PIDController(Robot.getConst("ConstMovekP", 1), Robot.getConst("ConstMovekI", 0), Robot.getConst("ConstMovekD", 0), Robot.dt, Robot.dt);
turnController.disable();
turnController.setInputRange(-180, 180);
turnController.setOutputRange(-1.0, 1.0);
turnController.setContinuous();
turnController.setAbsoluteTolerance(Robot.getConst("ConstMoveTolerance", 2));
// moveLeftController = new PIDController(Robot.getConst("ConstMoveLeftkP", 1), Robot.getConst("ConstMoveLeftkI", 0), Robot.getConst("ConstMoveLeftkD", 0), Robot.dt.getLeftDrive(), (PIDOutput) Robot.dt.getLeftDrive());
// moveLeftController.disable();
// moveLeftController.setInputRange(0, Double.MAX_VALUE);
// moveLeftController.setOutputRange(-1.0, 1.0);
// moveLeftController.setContinuous(false);
// moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft", 2));
// moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP", 1), Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD", 0), Robot.dt.getRightDrive(), (PIDOutput) Robot.dt.getRightDrive());
// moveRightController.disable();
// moveRightController.setInputRange(0, Double.MAX_VALUE);
// moveRightController.setOutputRange(-1.0, 1.0);
// moveRightController.setContinuous(false);
// moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight", 2));

turnController = new PIDController(Robot.getConst("ConstTurnkP", 1), Robot.getConst("ConstTurnkI", 0),
Robot.getConst("ConstTurnkD", 0), ahrs, Robot.dt);
turnController.disable();
turnController.setInputRange(-180, 180);
turnController.setOutputRange(-1.0, 1.0);
turnController.setContinuous();
turnController.setAbsoluteTolerance(Robot.getConst("ConstTurnTolerance", 1));
moveController = new PIDController(Robot.getConst("ConstMovekP", 1), Robot.getConst("ConstMovekI", 0),
Robot.getConst("ConstMovekD", 0), Robot.dt, Robot.dt);
turnController.disable();
turnController.setInputRange(-180, 180);
turnController.setOutputRange(-1.0, 1.0);
turnController.setContinuous();
turnController.setAbsoluteTolerance(Robot.getConst("ConstMoveTolerance", 2));
// moveLeftController = new PIDController(Robot.getConst("ConstMoveLeftkP", 1),
// Robot.getConst("ConstMoveLeftkI", 0), Robot.getConst("ConstMoveLeftkD", 0),
// Robot.dt.getLeftDrive(), (PIDOutput) Robot.dt.getLeftDrive());
// moveLeftController.disable();
// moveLeftController.setInputRange(0, Double.MAX_VALUE);
// moveLeftController.setOutputRange(-1.0, 1.0);
// moveLeftController.setContinuous(false);
// moveLeftController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceLeft",
// 2));
// moveRightController = new PIDController(Robot.getConst("ConstMoveRightkP",
// 1), Robot.getConst("ConstMoveRightkI", 0), Robot.getConst("ConstMoveRightkD",
// 0), Robot.dt.getRightDrive(), (PIDOutput) Robot.dt.getRightDrive());
// moveRightController.disable();
// moveRightController.setInputRange(0, Double.MAX_VALUE);
// moveRightController.setOutputRange(-1.0, 1.0);
// moveRightController.setContinuous(false);
// moveRightController.setAbsoluteTolerance(Robot.getConst("ConstMoveToleranceRight",
// 2));

ahrs = new AHRS(SerialPort.Port.kMXP);
dtGyro = new AnalogGyro(getPort("PortGyro", 0));
dtGear = new DoubleSolenoid(getPort("Port1dtGearSolenoid", 0), getPort("Port2dtGearSolenoid", 1));

}

/**
* Used in RobotMap to find ports for robot components, getPort also puts numbers if they don't exist yet.
* @param key The port key
* @param def The default value
* Used in RobotMap to find ports for robot components, getPort also puts
* numbers if they don't exist yet.
*
* @param key
* The port key
* @param def
* The default value
* @return returns the Port
*/
public int getPort(String key, int def) {
if(!SmartDashboard.containsKey(key)) {
if (!SmartDashboard.containsKey(key)) {
SmartDashboard.putNumber(key, def);
}
return (int)SmartDashboard.getNumber(key, def);
return (int) SmartDashboard.getNumber(key, def);
}
}
Loading