Permalink
Find file
209a607 Oct 10, 2016
@chrisr2 @jgermita
252 lines (193 sloc) 6.15 KB
package com.gryffingear.y2016.systems;
import com.gryffingear.y2016.config.Constants;
import com.gryffingear.y2016.config.Ports;
import com.gryffingear.y2016.utilities.LedOutput;
import com.gryffingear.y2016.utilities.Looper;
import com.gryffingear.y2016.utilities.NegativeInertiaAccumulator;
import edu.wpi.first.wpilibj.AnalogInput;
import edu.wpi.first.wpilibj.Compressor;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
public class SuperSystem {
private static SuperSystem instance = null;
public Drivetrain drive = null;
public Intake intake = null;
public Shooter shoot = null;
public Compressor compressor = null;
public Stager stage = null;
public AnalogInput pixycam = null;
private Arm arm = null;
private Winch winch = null;
LedOutput hoodLed;
LedOutput stagerLed;
LedOutput flashlight;
private long matchTime = 0;
private SuperSystem() {
drive = new Drivetrain( Ports.Drivetrain.DRIVE_LEFT_A_PORT,
Ports.Drivetrain.DRIVE_LEFT_B_PORT,
Ports.Drivetrain.DRIVE_RIGHT_A_PORT,
Ports.Drivetrain.DRIVE_RIGHT_B_PORT, 0);
intake = new Intake(Ports.Intake.INTAKE_MOTOR,
Ports.Intake.INTAKE_SOLENOID,
Ports.Intake.STAGE_SENSOR);
// Shoot? Yes, shoot.
shoot = new Shooter(Ports.Shooter.SHOOTER_MOTOR_A,
Ports.Shooter.SHOOTER_MOTOR_B,
Ports.Shooter.SHOOTER_ENCODER_PORT);
stage = new Stager (Ports.Stager.STAGER_MOTOR);
pixycam = new AnalogInput(Ports.Pixycam.PIXYCAM_PORT);
arm = new Arm (Ports.Arm.ARM_SOLENOID);
winch = new Winch (Ports.Winch.WINCH_MOTOR_A,
Ports.Winch.WINCH_MOTOR_B,
Ports.Winch.WINCH_SOLENOID,
Ports.Winch.CLIMBER_MOTOR,
Ports.Winch.CLIMBER_SENSOR);
hoodLed = new LedOutput(Ports.Shooter.SHOOTER_INDICATOR_LIGHT, Ports.Pneumatics.PCM_CAN_ID);
stagerLed = new LedOutput(Ports.Stager.STAGER_INDICATOR_LIGHT, Ports.Pneumatics.PCM_CAN_ID);
flashlight = new LedOutput(Ports.Shooter.FLASHLIGHT_PORT, Ports.Pneumatics.PCM_CAN_ID);
compressor = new Compressor(Ports.Pneumatics.PCM_CAN_ID);
compressor.setClosedLoopControl(true);
compressor.start();
}
public static SuperSystem getInstance() {
if (instance == null) {
instance = new SuperSystem();
}
return instance;
}
private NegativeInertiaAccumulator turnNia = new NegativeInertiaAccumulator(2.5);
public void drive(double leftIn,
double rightIn,
boolean autoAim,
boolean armPos,
boolean winchClimberPositiveInput,
boolean winchClimberNegativeInput,
boolean winchInput ) {
double throttle = (leftIn + rightIn) / 2.0;
double turning = (leftIn - rightIn) / 2.0;
boolean winchBrakeState = matchTime > 134750; // Auto Brake at 250ms from end of match.
double wOut = 0.0; //winch Out
double cOut = 0.0; //climber Out
if (winchClimberPositiveInput) {
cOut = 1.0;
}else if (winchClimberNegativeInput) {
cOut = -1.0;
} else {
cOut = 0.0;
}
if (winchInput) {
wOut = 1.0;
} else {
wOut = 0.0;
}
if((winch.getScaled())){
wOut = 0.0;
winchBrakeState = true;
}
if(!autoAim) {
turning += turnNia.update(turning);
} else {
throttle = 0.0;
double kP = Constants.SuperSystem.AUTO_AIM_KP;
turning = kP * ((Constants.SuperSystem.AUTO_AIM_TARGET) - pixycam.getVoltage());
}
drive.tankDrive(throttle + turning, throttle - turning);
arm.set(armPos);
winch.runClimber(cOut);
winch.runWinch(-wOut);
winch.setBrake(winchBrakeState);
}
public void operate(double intakeInput,
boolean intakePos,
double stagerInput,
double shooterInput) {
double iOut = 0.0; // intake motor out
boolean ipOut = false; // intake solenoid out
double stOut = 0.0; // stager motor out
double sOut = 0.0; // shooter motor out
boolean wpOut = false;
if(intakeInput > 0.20) {
iOut = -1.0;
} else if(intakeInput < -0.20) {
iOut = 1.0;
} else {
iOut = 0.0;
}
ipOut = intakePos || intakeInput > 0.9;
if(stagerInput > 0.20) {
stOut = -1.0;
if((intake.getBallStaged())) {
if(Math.abs(shooterInput) <= 0.0) {
stOut = 0.0;
}
}
} else if(stagerInput < -0.20) {
stOut = 1.0;
} else {
stOut = 0.0;
}
double shooterCurrent = shoot.getCurrent();
flashlight.set(shooterCurrent > 3);
if(shooterCurrent > 20) { // Just started spinning up...
hoodLed.blink(500);
} else if(shooterCurrent > 15) { // Approaching target speed...
hoodLed.blink(250);
} else if(shooterCurrent > 3) { // On target, ready to shoot.
hoodLed.set(true);
} else { // Not trying to shoot, turn off.
if(matchTime > 115000) { // Blink LEDs based on match time
hoodLed.blink(250);
} else if(matchTime > 105000) {
hoodLed.blink(500);
} else {
hoodLed.set(false);
}
}
if(intake.getBallStaged()) {
stagerLed.set(true);
} else {
if(Math.abs(stOut) > 0.01 || Math.abs(iOut) > 0.01) {
stagerLed.blink(500);
} else {
stagerLed.set(false);
}
}
sOut = shooterInput;
// Do output stuff.
intake.runIntake(iOut);
intake.setIntake(ipOut);
stage.runStager(stOut);
shoot.setPercentVBus(sOut);
// shoot.setVoltage(sOut * 12.0);
}
/**
* Refreshes networktables/smartdashboard with new telemetry data.
*/
public void updateSmartDashboard() {
SmartDashboard.putNumber("ShooterCurrent", shoot.getCurrent());
SmartDashboard.putNumber("ShooterVel", shoot.getSpeed());
SmartDashboard.putNumber("ShooterVoltage", shoot.get());
SmartDashboard.putNumber("ShooterError", shoot.getError());
SmartDashboard.putNumber("Gyro", drive.getYaw());
SmartDashboard.putNumber("Pixycam", pixycam.getVoltage());
SmartDashboard.putBoolean("atSpeed", shoot.atSpeed());
SmartDashboard.putNumber("ShooterOut", shoot.get());
SmartDashboard.putNumber("DriveTotalCurrent", drive.getTotalCurrent());
}
public void ledOn() {
hoodLed.set(true);
stagerLed.set(true);
}
/**
* Tells the supersystem how long we've been in teleop
* @param millis
*/
public void setTeleopTime(long millis) {
this.matchTime = millis;
}
/**
* "pokes" subsystems that need to be updated periodically.
*/
public void poke() {
intake.update();
}
}