summaryrefslogtreecommitdiff
path: root/src/org/usfirst/frc/team4272/robot2015/Robot.java
diff options
context:
space:
mode:
Diffstat (limited to 'src/org/usfirst/frc/team4272/robot2015/Robot.java')
-rw-r--r--src/org/usfirst/frc/team4272/robot2015/Robot.java263
1 files changed, 83 insertions, 180 deletions
diff --git a/src/org/usfirst/frc/team4272/robot2015/Robot.java b/src/org/usfirst/frc/team4272/robot2015/Robot.java
index f9569a6..22abd30 100644
--- a/src/org/usfirst/frc/team4272/robot2015/Robot.java
+++ b/src/org/usfirst/frc/team4272/robot2015/Robot.java
@@ -1,19 +1,11 @@
package org.usfirst.frc.team4272.robot2015;
-import org.usfirst.frc.team4272.robotlib.Toggler;
-import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Axis;
-import org.usfirst.frc.team4272.robotlib.Xbox360Controller.Button;
import edu.wpi.first.wpilibj.IterativeRobot;
-import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Relay;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj.Timer;
-import java.io.File;
import java.io.FileWriter;
-import java.io.BufferedWriter;
import java.io.IOException;
import java.io.PrintWriter;
/**
@@ -24,182 +16,93 @@ import java.io.PrintWriter;
* directory.
*/
public class Robot extends IterativeRobot {
- private HwRobot robot = new HwRobot();
- private HwOI oi;
-
- private PrintWriter log;
-
- private class Control {
- double lDrive, rDrive, winch;
- Relay.Value lIntake, rIntake;
- boolean grab, push;
- public Control() {
- lDrive = rDrive = winch = 0;
- grab = push = false;
- lIntake = rIntake = Relay.Value.kOff;
+ private final HwRobot robot = new HwRobot();
+ private final HwOI oi = new HwOI();
+ private final Control control = new Control();
+ private PrintWriter log;
+
+ private Autonomous auto;
+ private Teleop teleop;
+ /**
+ * This function is run when the robot is first started up and should be
+ * used for any initialization code.
+ */
+ public void robotInit() {
+ try {
+ inLog();
+ } catch (Exception e) {}
+ }
+
+ private void inLog() throws IOException {
+ log = new PrintWriter(new FileWriter("/usr/local/frc/share/lukeshu.log", true));
+ log.println("\ntime"
+ +",c:lDrive,c:rDrive,c:winch,c:lIntake,c:rIntake,c:grab,c:push"
+ +",i:lDrive.dist,i:lDrive.rate"
+ +",i:rDrive.dist,i:rDrive.rate"
+ +",i:winch.dist");
+ }
+
+ private void doLog() throws Exception {
+ log.println(0
+ +","+control.lDrive
+ +","+control.rDrive
+ +","+control.winch
+ +","+fmtRelay(control.lIntake)
+ +","+fmtRelay(control.rIntake)
+ +","+control.grab
+ +","+control.push
+ +","+robot.lDriveE.getDistance()
+ +","+robot.lDriveE.getRate()
+ +","+robot.rDriveE.getDistance()
+ +","+robot.rDriveE.getRate()
+ +","+robot.winchE.getRate()
+ );
+ }
+
+ private String fmtRelay(Relay.Value v) {
+ if (v == Relay.Value.kForward) {
+ return "kForward";
+ } else if (v == Relay.Value.kReverse) {
+ return "kReverse";
+ } else if (v == Relay.Value.kOn) {
+ return "kOn";
+ } else if (v == Relay.Value.kOff) {
+ return "kOff";
+ } else {
+ return "unknown";
}
}
- Control control = new Control();
-
- /**
- * This function is run when the robot is first started up and should be
- * used for any initialization code.
- */
- public void robotInit() {
- try {
- inLog();
- } catch (Exception e) {}
- }
-
- private void inLog() throws IOException {
- log = new PrintWriter(new FileWriter("/usr/local/frc/share/lukeshu.log", true));
- log.println("\ntime"
- +",c:lDrive,c:rDrive,c:winch,c:lIntake,c:rIntake,c:grab,c:push"
- +",i:lDrive.dist,i:lDrive.rate"
- +",i:rDrive.dist,i:rDrive.rate"
- +",i:winch.dist");
- }
-
- private void doLog() throws Exception {
- log.println(0
- +","+control.lDrive
- +","+control.rDrive
- +","+control.winch
- +","+fmtRelay(control.lIntake)
- +","+fmtRelay(control.rIntake)
- +","+control.grab
- +","+control.push
- +","+robot.lDriveE.getDistance()
- +","+robot.lDriveE.getRate()
- +","+robot.rDriveE.getDistance()
- +","+robot.rDriveE.getRate()
- +","+robot.winchE.getRate()
- );
- }
-
- private String fmtRelay(Relay.Value v) {
- if (v == Relay.Value.kForward) {
- return "kForward";
- } else if (v == Relay.Value.kReverse) {
- return "kReverse";
- } else if (v == Relay.Value.kOn) {
- return "kOn";
- } else if (v == Relay.Value.kOff) {
- return "kOff";
- } else {
- return "unknown";
- }
- }
-
- private void doRobot() {
- try {
- robot.lDriveM.pidWrite( control.lDrive);
- robot.rDriveM.pidWrite(-control.rDrive);
- robot.winchM.pidWrite(-control.winch);
- robot.lIntake.set(control.lIntake);
- robot.rIntake.set(control.rIntake);
- robot.grab.setForward(control.grab);
- robot.push.setForward(!control.push);
- doLog();
- } catch (Exception e) {}
- }
- private Timer autotime = new Timer();
- public void autonomousInit() {
- autotime.reset();
- autotime.start();
- robot.rDriveE.reset();
- robot.lDriveE.reset();
- }
- public void autonomousPeriodic() {
- if (/*autotime.get() < 4*/robot.rDriveE.getDistance() < (15.5*140*1.1)) {
- control.rDrive = control.lDrive = -0.7;
- } else {
- control.rDrive = control.lDrive = 0;
- }
- doRobot();
- }
+ public void autonomousInit() {
+ auto = new Autonomous(robot);
+ }
+ public void autonomousPeriodic() {
+ try {
+ robot.run(auto.run(control));
+ doLog();
+ } catch (Exception e) {}
+ }
+
+ public void teleopInit() {
+ teleop = new Teleop();
+ }
+
+ public void teleopPeriodic() {
+ try {
+ robot.run(teleop.run(control, oi));
+ doLog();
+ } catch (Exception e) {}
+ }
- private Toggler grabButton = new Toggler();
- private Toggler pushButton = new Toggler();
- private Toggler ledsButton = new Toggler();
- public void teleopInit() {
- //robot.lDrive.reset();
- //robot.rDrive.reset();
- //robot.winch.reset();
- //robot.lDrive.enable();
- //robot.rDrive.enable();
- //robot.winch.enable();
- robot.grab.setEnabled(true);
- robot.push.setEnabled(true);
- oi = new HwOI();
- grabButton.update(false);
- pushButton.update(false);
- ledsButton.update(false);
- }
-
- private static double jsScale(Joystick j) {
- double y = -j.getY();/* +:forward; -:backward */
- double z = -j.getZ();/* +:more-sensitive; -:less-sensitive */
- return Math.copySign(Math.pow(Math.abs(y), 2.0-z), y);
- }
-
- public void teleopPeriodic() {
- /* This runs at 50Hz */
+ public void disabledInit() {
+ }
- /* Drive */
- control.lDrive = jsScale(oi.lStick);
- control.rDrive = jsScale(oi.rStick);
- /* Winch */
- control.winch = oi.xbox.getAxis(Axis.LY);/* up is neg, down is pos */
- if (Math.abs(control.winch) < 0.1) { control.winch = 0; }
-
- SmartDashboard.putNumber("winch M", -control.winch);
- SmartDashboard.putNumber("winch E", robot.winchE.get());
- SmartDashboard.putBoolean("winch T", robot.winchT.get());
- SmartDashboard.putBoolean("winch B", robot.winchB.get());
-
- /* left intake */
- if (oi.xbox.getButton(Button.LB)) {
- control.lIntake = Relay.Value.kReverse;
- } else if (oi.xbox.getAxis(Axis.LT) > 0.75) {
- control.lIntake = Relay.Value.kForward;
- } else {
- control.lIntake = Relay.Value.kOff;
- }
-
- /* right intake */
- if (oi.xbox.getButton(Button.RB)) {
- control.rIntake=(Relay.Value.kReverse);
- } else if (oi.xbox.getAxis(Axis.RT) > 0.75) {
- control.rIntake=(Relay.Value.kForward);
- } else {
- control.rIntake=(Relay.Value.kOff);
- }
-
- control.grab = grabButton.update(oi.xbox.getButton(Button.A));
- control.push = pushButton.update(oi.xbox.getButton(Button.B));
+ public void disabledPeriodic() {
+ }
- doRobot();
- }
-
- public void disabledInit() {
- //robot.lDrive.reset();
- //robot.rDrive.reset();
- //robot.winch.reset();
- robot.grab.setEnabled(false);
- robot.push.setEnabled(false);
- }
-
- public void disabledPeriodic() {
-
- }
-
- /**
- * This function is called periodically during test mode
- */
- public void testPeriodic() {
-
- }
-
+ /**
+ * This function is called periodically during test mode
+ */
+ public void testPeriodic() {
+ }
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-19 06:16:18 +0000