Permalink
Browse files

Iterative teleop complete

  • Loading branch information...
Linnea
Linnea committed Oct 13, 2018
1 parent 595c4f1 commit bf0f7285c876e4178ec00d9b8b5fbf16f34e544b
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<projectDescription>
<name>Robot_2018</name>
<name>robot_2018</name>
<comment>Project Robot_2018 created by Buildship.</comment>
<projects>
</projects>
0 gradlew 100644 → 100755
No changes.
@@ -3,37 +3,15 @@

import edu.wpi.first.wpilibj.DriverStation;
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;

import org.usfirst.frc.team1700.robot.commands.Intake.IntakeInCommand;
import org.usfirst.frc.team1700.robot.commands.AutoCGs.AutoForward;
import org.usfirst.frc.team1700.robot.commands.AutoCGs.LeftScaleAuto;
import org.usfirst.frc.team1700.robot.commands.AutoCGs.LeftSwitchAuto;
import org.usfirst.frc.team1700.robot.commands.AutoCGs.RightScaleAuto;
import org.usfirst.frc.team1700.robot.commands.AutoCGs.RightSwitchAuto;
import org.usfirst.frc.team1700.robot.commands.AutoCGs.CenterScaleAuto;
import org.usfirst.frc.team1700.robot.commands.AutoCGs.CenterSwitchAuto;
import org.usfirst.frc.team1700.robot.commands.AutoCGs.testAutoCG;
import org.usfirst.frc.team1700.robot.commands.Drivetrain.DriveForwardTimeOutCommand;
import org.usfirst.frc.team1700.robot.commands.Elevator.ElevatorForTimeCommand;
import org.usfirst.frc.team1700.robot.commands.Elevator.ElevatorMoveCommand;
import org.usfirst.frc.team1700.robot.commands.Elevator.ElevatorResetCommand;
import org.usfirst.frc.team1700.robot.commands.Elevator.ElevatorStopCommand;
import org.usfirst.frc.team1700.robot.commands.Elevator.ElevatorToInchesCommand;
import org.usfirst.frc.team1700.robot.commands.Intake.FoldIntakeCommand;
import org.usfirst.frc.team1700.robot.commands.Intake.ReleaseIntakeCommand;
import org.usfirst.frc.team1700.robot.commands.Intake.RunIntakeCommand;
import org.usfirst.frc.team1700.robot.commands.Intake.StopIntakeCommand;
import org.usfirst.frc.team1700.robot.commands.Intake.GrabIntakeCommand;
import org.usfirst.frc.team1700.robot.commands.Intake.GrabIntakeCommand;
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;

/**
@@ -50,8 +28,13 @@
public static final ElevatorSubsystem elevatorSubsystem = new ElevatorSubsystem();
public static final IntakeSubsystem intakeSubsystem = new IntakeSubsystem();

Command autonomousCommand;
SendableChooser<Command> chooser = new SendableChooser<>();
double leftIntakeSpeed = 0;
double rightIntakeSpeed = 0;
Boolean desiredGrabIntakeState = false;
Boolean desiredFoldIntakeState = true;
double leftSpeed = 0;
double rightSpeed = 0;
double elevatorSpeed = 0;

/**
* This function is run when the robot is first started up and should be
@@ -60,15 +43,6 @@
@Override
public void robotInit() {
oi = new OI();
// chooser.addDefault("Default Auto (Drive Forward)", new AutoForward());
// chooser.addObject("Left Switch Auto", new LeftSwitchAuto());
// chooser.addObject("Center Switch Auto", new CenterSwitchAuto());
// chooser.addObject("Right Switch Auto", new RightSwitchAuto());
// chooser.addObject("Left Scale Auto", new LeftScaleAuto());
// chooser.addObject("Center Scale Auto", new CenterScaleAuto());
// chooser.addObject("Right Scale Auto", new RightScaleAuto());
// chooser.addObject("Test (DON'T USE THIS IN COMPETITION!)", new testAutoCG());
// SmartDashboard.putData("Auto Mode", chooser);
System.out.println("ROBOT INITIATED!! :)");
}

@@ -83,7 +57,6 @@ public void disabledInit() {

@Override
public void disabledPeriodic() {
Scheduler.getInstance().run();
}

/**
@@ -99,24 +72,13 @@ public void disabledPeriodic() {
*/
@Override
public void autonomousInit() {
autonomousCommand = new LeftSwitchAuto();
// chooser.getSelected();
if (autonomousCommand.getName() == "testAutoCG" && DriverStation.getInstance().isFMSAttached()) {
autonomousCommand = new LeftSwitchAuto();
}
DriverStation.getInstance().reportWarning(autonomousCommand.getName(), false);
if (autonomousCommand != null) {
autonomousCommand.start();
}
DriverStation.getInstance().reportWarning("AUTO INIT!!", false);
}

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

@Override
@@ -129,28 +91,56 @@ public void teleopInit() {
@Override
public void teleopPeriodic() {

// INTAKE
OI.foldUp.whenPressed(new FoldIntakeCommand(true));
OI.foldDown.whenPressed(new FoldIntakeCommand(false));
OI.stopIntake.whileHeld(new StopIntakeCommand());
OI.stopIntake.whenReleased(new IntakeInCommand());
OI.letGo.whileHeld(new GrabIntakeCommand(true));
OI.letGo.whenReleased(new RunIntakeCommand());
OI.squeeze.whenPressed(new GrabIntakeCommand(false));
OI.squeeze.whenPressed(new IntakeInCommand());
OI.releaseIntakeFast.whileHeld(new ReleaseIntakeCommand(-1));
OI.releaseIntakeFast.whenReleased(new RunIntakeCommand());
OI.releaseIntakeFast.whenReleased(new GrabIntakeCommand(true));
OI.releaseIntakeSlow.whileHeld(new ReleaseIntakeCommand(-0.4));
OI.releaseIntakeSlow.whenReleased(new RunIntakeCommand());
OI.releaseIntakeSlow.whenReleased(new GrabIntakeCommand(true));

// UPDATE ROBOT STATE
if (OI.releaseIntakeFast.)
leftSpeed = OI.leftJoy.getRawAxis(1);
rightSpeed = OI.rightJoy.getRawAxis(1);
elevatorSpeed = -OI.coJoy.getRawAxis(1);

if (OI.releaseIntakeFast.get()){
leftIntakeSpeed = -1;
rightIntakeSpeed = -1;
desiredGrabIntakeState = RobotMap.GRAB_INTAKE_OPEN;
}
else if (OI.releaseIntakeSlow.get()){
leftIntakeSpeed = -0.4;
rightIntakeSpeed = -0.4;
desiredGrabIntakeState = RobotMap.GRAB_INTAKE_OPEN;
}
else if (OI.stopIntake.get()){
leftIntakeSpeed = 0;
rightIntakeSpeed = 0;
//no update to desiredGrabIntakeState
}
else {
leftIntakeSpeed = 0.5;
rightIntakeSpeed = 0.3;
if (OI.squeeze.get()){
desiredGrabIntakeState = RobotMap.GRAB_INTAKE_CLOSE;
}
else if (OI.letGo.get()){
desiredGrabIntakeState = RobotMap.GRAB_INTAKE_OPEN;
}
else{
//no update to desiredGrabIntakeState
}
}

if (OI.foldUp.get()){
desiredFoldIntakeState = true;
}
else if (OI.foldDown.get()){
desiredFoldIntakeState = false;
}
else {
// no update to desiredFoldIntakeState
}

// EXECUTE
DriveSubsystem.driverControl();
ElevatorSubsystem.driverControl();
driveSubsystem.driveTank(leftSpeed, rightSpeed);
elevatorSubsystem.elevatorMove(elevatorSpeed);
intakeSubsystem.fold(desiredFoldIntakeState);
intakeSubsystem.grab(desiredGrabIntakeState);
intakeSubsystem.runIntake(leftIntakeSpeed, rightIntakeSpeed);
}

/**
@@ -55,6 +55,10 @@

// DIGITAL OUTPUT
public static DigitalOutput arduinoTrigger = new DigitalOutput(10);

// STATE ENUMS
public static Boolean GRAB_INTAKE_OPEN = true;
public static Boolean GRAB_INTAKE_CLOSE = false;


}

This file was deleted.

Oops, something went wrong.

This file was deleted.

Oops, something went wrong.

This file was deleted.

Oops, something went wrong.
Oops, something went wrong.

0 comments on commit bf0f728

Please sign in to comment.