Permalink
Browse files

WIP - iterative auto. Removal of command based references

  • Loading branch information...
ccruise committed Oct 13, 2018
1 parent d6c5927 commit 75c568389966a479bdae6618f9146c2747dbf66e
@@ -10,5 +10,7 @@
".classpath": true,
".project": true
},
"wpilib.online": true
"wpilib.online": true,
"wpilib.autoSaveOnDeploy": true,
"wpilib.autoStartRioLog": true
}
@@ -1,19 +1,14 @@

package org.usfirst.frc.team1700.robot;

import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.livewindow.LiveWindow;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

import org.usfirst.frc.team1700.robot.autonmodes.AutonomousBase;

import org.usfirst.frc.team1700.robot.subsystems.DriveSubsystem;
import org.usfirst.frc.team1700.robot.subsystems.ElevatorSubsystem;
import org.usfirst.frc.team1700.robot.subsystems.IntakeSubsystem;


import com.kauailabs.navx.frc.AHRS;

/**
* The VM is configured to automatically run this class, and to call the
* functions corresponding to each mode, as described in the IterativeRobot
@@ -23,11 +18,15 @@
*/
public class Robot extends IterativeRobot {

public static final DriveSubsystem driveSubsystem = new DriveSubsystem();
public static OI oi;

public static final DriveSubsystem driveSubsystem = new DriveSubsystem();
public static final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
public static final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();

public static AutonomousBase auto;

// Control variables
double leftIntakeSpeed = 0;
double rightIntakeSpeed = 0;
Boolean desiredGrabIntakeState = false;
@@ -36,6 +35,9 @@
double rightSpeed = 0;
double elevatorSpeed = 0;

// State variables
Boolean hasGameData = false;

/**
* This function is run when the robot is first started up and should be
* used for any initialization code.
@@ -59,26 +61,19 @@ public void disabledInit() {
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
*
* 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.
*/
@Override
public void autonomousInit() {
// Choose auton mode based on starting location
Boolean preferSwitch = true;
auto = new AutonomousBase(AutonomousBase.StartLocation.LEFT, preferSwitch);
}

/**
* This function is called periodically during autonomous
*/
@Override
public void autonomousPeriodic() {
auto.periodic();
}

@Override
@@ -148,6 +143,5 @@ else if (OI.foldDown.get()){
*/
@Override
public void testPeriodic() {
LiveWindow.run();
}
}
@@ -3,10 +3,8 @@

import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalOutput;
@@ -0,0 +1,148 @@
/* This class implements the camera to be used in autonomous via the IP Camera.
*/
package org.usfirst.frc.team1700.robot.autonmodes;

import java.time.Duration;
import java.time.Instant;
import edu.wpi.first.wpilibj.DriverStation;

public class AutonomousBase{

// Auto game data / decision making variables

public enum StartLocation {
LEFT,
CENTER,
RIGHT;
}

protected enum GameDataState {
WAITING,
RECEIVED,
TIMEOUT;
}

protected enum AutoDecisionState {
SWITCH,
SCALE,
CROSS_SCALE,
CROSS_LINE;
}

Instant start;
GameDataState gameDataState = GameDataState.WAITING;
AutoDecisionState autoDecisionState;
AutoDecisionState crossSideDefault = AutoDecisionState.CROSS_SCALE;
StartLocation startLocation;
String gameData;
Boolean preferSwitch;

public AutonomousBase(StartLocation inputLocation, Boolean inputPreferSwitch) {
startLocation = inputLocation;
preferSwitch = inputPreferSwitch;
}

public void periodic(){

if(gameDataState == GameDataState.WAITING){
updateGameData();
if(gameDataState == GameDataState.TIMEOUT){
autoDecisionState = AutoDecisionState.CROSS_LINE;
}else if(gameDataState == GameDataState.RECEIVED){
// make decision here
makeDecision();
}
}
// gameDataState is update so this can check again (similar to else if)
if(gameDataState != GameDataState.WAITING){

}
}

protected void updateGameData(){
gameData = DriverStation.getInstance().getGameSpecificMessage();
if (gameData.length() < 3) {
// game data not received if length < 3
gameData = DriverStation.getInstance().getGameSpecificMessage();
if (Duration.between(start, Instant.now()).toMillis() > 100) {
DriverStation.reportWarning("timed out :(", false);
gameDataState = GameDataState.TIMEOUT;
}
else{
// gameDataState still WAITING
}
}
else{
// Game data received
gameDataState = GameDataState.RECEIVED;
}
}

protected void makeDecision(){
if(startLocation == StartLocation.LEFT){
if(preferSwitch){
if(gameData.charAt(0) == 'L'){
// Left side, left switch auto
autoDecisionState = AutoDecisionState.SWITCH;
}
else if(gameData.charAt(1) == 'L'){
// Left side, left scale auto
autoDecisionState = AutoDecisionState.SCALE;
}
else{
autoDecisionState = crossSideDefault;
}
}
else{
if(gameData.charAt(1) == 'L'){
// Left side, left scale auto
autoDecisionState = AutoDecisionState.SCALE;
}
else if(gameData.charAt(0) == 'L'){
// Left side, left switch auto
autoDecisionState = AutoDecisionState.SWITCH;
}
else{
autoDecisionState = crossSideDefault;
}
}
}
else if(startLocation == StartLocation.RIGHT){
if(preferSwitch){
if(gameData.charAt(0) == 'R'){
// Right side, right switch auto
autoDecisionState = AutoDecisionState.SWITCH;
}
else if(gameData.charAt(1) == 'R'){
// Right side, right scale auto
autoDecisionState = AutoDecisionState.SCALE;
}
else{
autoDecisionState = crossSideDefault;
}
}
else{
if(gameData.charAt(1) == 'R'){
// Right side, right scale auto
autoDecisionState = AutoDecisionState.SCALE;
}
else if(gameData.charAt(0) == 'R'){
// Right side, right switch auto
autoDecisionState = AutoDecisionState.SWITCH;
}
else{
autoDecisionState = crossSideDefault;
}
}
}
else{
if(preferSwitch){
autoDecisionState = AutoDecisionState.SWITCH;
}
else{
// You probably don't want to do this (scale from center)
autoDecisionState = AutoDecisionState.SCALE;
}
}
}
}
@@ -5,13 +5,9 @@
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.ctre.phoenix.motorcontrol.can.VictorSPX;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;
import com.kauailabs.navx.frc.AHRS;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
*
@@ -20,38 +16,40 @@
public class DriveSubsystem{

// AUTO CONSTANTS
public static double inchesToTicks = 11.94; // 40 ticks/in. * 54/20*50/12 gearbox reduction / (12pi in/shaft rotation)
public double distToSwitch = 140*inchesToTicks;
public double distToAutoLine = 110*inchesToTicks;
public double finalDistToSwitch = 30*inchesToTicks;
public double finalDistToScale = 23*inchesToTicks; // with bad encoder: 55
public static final double inchesToTicks = 11.94; // 40 ticks/in. * 54/20*50/12 gearbox reduction / (12pi in/shaft rotation)
public static final double distToSwitch = 140*inchesToTicks;
public static final double distToAutoLine = 110*inchesToTicks;
public static double finalDistToSwitch = 30*inchesToTicks;
public static double finalDistToScale = 23*inchesToTicks; // with bad encoder: 55

public double crossScaleDist1 = 210*DriveSubsystem.inchesToTicks;
public double crossScaleDist2 = 190*DriveSubsystem.inchesToTicks;
public double crossScaleDist3 = 57*DriveSubsystem.inchesToTicks;
public static final double crossScaleDist1 = 210*DriveSubsystem.inchesToTicks;
public static final double crossScaleDist2 = 190*DriveSubsystem.inchesToTicks;
public static final double crossScaleDist3 = 57*DriveSubsystem.inchesToTicks;

public double sameScaleDist = 226*DriveSubsystem.inchesToTicks; // with bad encoder: 280
public static final double sameScaleDist = 226*DriveSubsystem.inchesToTicks; // with bad encoder: 280

public long waitTime = 250;
public static final long waitTime = 250;

public double crossSwitchDist1 = 18*DriveSubsystem.inchesToTicks;
public double crossSwitchDist2 = 127*DriveSubsystem.inchesToTicks;
public double crossSwitchDist3 = 18*DriveSubsystem.inchesToTicks;
public static final double crossSwitchDist1 = 18*DriveSubsystem.inchesToTicks;
public static final double crossSwitchDist2 = 127*DriveSubsystem.inchesToTicks;
public static final double crossSwitchDist3 = 18*DriveSubsystem.inchesToTicks;

public double centerLeftDist1 = 50*DriveSubsystem.inchesToTicks;
public double centerLeftDist2 = 80*DriveSubsystem.inchesToTicks;
public double centerLeftDist3 = 20*DriveSubsystem.inchesToTicks;
public static final double centerLeftDist1 = 50*DriveSubsystem.inchesToTicks;
public static final double centerLeftDist2 = 80*DriveSubsystem.inchesToTicks;
public static final double centerLeftDist3 = 20*DriveSubsystem.inchesToTicks;

public double centerRightDist1 = 50*DriveSubsystem.inchesToTicks;
public double centerRightDist2 = 70*DriveSubsystem.inchesToTicks;
public double centerRightDist3 = 25*DriveSubsystem.inchesToTicks;
public static final double centerRightDist1 = 50*DriveSubsystem.inchesToTicks;
public static final double centerRightDist2 = 70*DriveSubsystem.inchesToTicks;
public static final double centerRightDist3 = 25*DriveSubsystem.inchesToTicks;

public double centerDistBack = -18*DriveSubsystem.inchesToTicks;
public static final double centerDistBack = -18*DriveSubsystem.inchesToTicks;

public double sameSwitchDist = 140*DriveSubsystem.inchesToTicks;
public static final double sameSwitchDist = 140*DriveSubsystem.inchesToTicks;

public double left = -90;
public double right = 90;
public static final double left = -90;
public static final double right = 90;

public int test = 1;

// MOTORS AND SENSORS
VictorSPX LF = RobotMap.leftFrontDrive;
@@ -71,7 +69,6 @@ public DriveSubsystem() {
RE.setReverseDirection(true);
}


public void driveTank(double leftSpeed, double rightSpeed) {
LF.set(ControlMode.PercentOutput, -leftSpeed);
LB.set(ControlMode.PercentOutput, -leftSpeed);
@@ -3,13 +3,10 @@
import org.usfirst.frc.team1700.robot.RobotMap;

import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.FeedbackDevice;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
*
@@ -23,7 +20,6 @@
DigitalInput BL = RobotMap.elevatorBottomLimitSwitch;
Encoder enc = RobotMap.elevatorEncoder;

//TODO: test and set these
public double scaleHeight = 80; // # ticks to reach scale level
public double scaleAfterSwitchHeight = 40;
public double switchHeight = 26;
@@ -5,13 +5,10 @@
import com.ctre.phoenix.motorcontrol.ControlMode;
import com.ctre.phoenix.motorcontrol.can.TalonSRX;

import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.DoubleSolenoid;
import edu.wpi.first.wpilibj.DriverStation;
import edu.wpi.first.wpilibj.command.Subsystem;

/**
*

0 comments on commit 75c5683

Please sign in to comment.