From 2ecf6669a0886b8cac0193d1081db7c44064d6cc Mon Sep 17 00:00:00 2001 From: Ryan Cahoon Date: Mon, 28 Sep 2020 16:52:58 +0000 Subject: [PATCH] Demo routine --- .../com/team766/robot/AutonomousModes.java | 2 + src/main/java/com/team766/robot/OI.java | 16 +++ src/main/java/com/team766/robot/Robot.java | 7 +- .../com/team766/robot/mechanisms/Drive.java | 98 +++++++++++++++++++ .../com/team766/robot/mechanisms/Intake.java | 26 +++++ .../team766/robot/mechanisms/Launcher.java | 18 ++++ .../procedures/ExampleDriveSequence.java | 36 +++++++ .../com/team766/robot/procedures/Launch.java | 16 +++ 8 files changed, 218 insertions(+), 1 deletion(-) create mode 100644 src/main/java/com/team766/robot/mechanisms/Drive.java create mode 100644 src/main/java/com/team766/robot/mechanisms/Intake.java create mode 100644 src/main/java/com/team766/robot/mechanisms/Launcher.java create mode 100644 src/main/java/com/team766/robot/procedures/ExampleDriveSequence.java create mode 100644 src/main/java/com/team766/robot/procedures/Launch.java diff --git a/src/main/java/com/team766/robot/AutonomousModes.java b/src/main/java/com/team766/robot/AutonomousModes.java index 0528d3ac..00f97114 100644 --- a/src/main/java/com/team766/robot/AutonomousModes.java +++ b/src/main/java/com/team766/robot/AutonomousModes.java @@ -13,6 +13,8 @@ public class AutonomousModes { // new AutonomousMode("DriveFast", () -> new DriveStraight(1.0)), // new AutonomousMode("DriveSlow", () -> new DriveStraight(0.4)), + new AutonomousMode("ExampleDriveSequence", () -> new ExampleDriveSequence()), + new AutonomousMode("DoNothing", () -> new DoNothing()), }; } \ No newline at end of file diff --git a/src/main/java/com/team766/robot/OI.java b/src/main/java/com/team766/robot/OI.java index d7f65d05..d68f7f09 100644 --- a/src/main/java/com/team766/robot/OI.java +++ b/src/main/java/com/team766/robot/OI.java @@ -26,6 +26,8 @@ public OI() { } public void run(final Context context) { + context.takeOwnership(Robot.drive); + context.takeOwnership(Robot.intake); while (true) { // wait for driver station data (and refresh it using the WPILib APIs) context.waitFor(() -> RobotProvider.instance.hasNewDriverStationData()); @@ -33,6 +35,20 @@ public void run(final Context context) { // Add driver controls here - make sure to take/release ownership // of mechanisms when appropriate. + Robot.drive.setArcadeDrive(joystick0.getAxis(1), joystick1.getAxis(0)); + + if (joystick0.getButtonPressed(1)) { + context.startAsync(new Launch()); + } + + if (joystick1.getButtonPressed(2)) { + Robot.intake.setIntakeArm(true); + Robot.intake.setIntakePower(1.0); + } + if (joystick1.getButtonPressed(3)) { + Robot.intake.setIntakeArm(false); + Robot.intake.setIntakePower(0.0); + } } } } diff --git a/src/main/java/com/team766/robot/Robot.java b/src/main/java/com/team766/robot/Robot.java index 89ef434d..db7f6c83 100644 --- a/src/main/java/com/team766/robot/Robot.java +++ b/src/main/java/com/team766/robot/Robot.java @@ -4,9 +4,14 @@ public class Robot { // Declare mechanisms here - + public static Drive drive; + public static Intake intake; + public static Launcher launcher; public static void robotInit() { // Initialize mechanisms here + drive = new Drive(); + intake = new Intake(); + launcher = new Launcher(); } } diff --git a/src/main/java/com/team766/robot/mechanisms/Drive.java b/src/main/java/com/team766/robot/mechanisms/Drive.java new file mode 100644 index 00000000..7dc78d8e --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Drive.java @@ -0,0 +1,98 @@ +package com.team766.robot.mechanisms; + +import com.team766.framework.Mechanism; +import com.team766.hal.EncoderReader; +import com.team766.hal.GyroReader; +import com.team766.hal.RobotProvider; +import com.team766.hal.MotorController; + +public class Drive extends Mechanism { + + private MotorController m_leftMotor; + private MotorController m_rightMotor; + private EncoderReader m_leftEncoder; + private EncoderReader m_rightEncoder; + private GyroReader m_gyro; + + // variables for calculating position using odometry + private double m_x; + private double m_y; + private double m_previousLeft; + private double m_previousRight; + private double m_previousAngle; + private double m_velocity; + private double m_previousTime = RobotProvider.getTimeProvider().get(); + + public Drive() { + m_leftMotor = RobotProvider.instance.getMotor("drive.leftMotor"); + m_rightMotor = RobotProvider.instance.getMotor("drive.rightMotor"); + m_leftEncoder = RobotProvider.instance.getEncoder("drive.leftEncoder"); + m_rightEncoder = RobotProvider.instance.getEncoder("drive.rightEncoder"); + m_gyro = RobotProvider.instance.getGyro("drive.gyro"); + } + + @Override + public void run() { + double leftDistance = m_leftEncoder.getDistance(); + double rightDistance = m_rightEncoder.getDistance(); + double distance = ((leftDistance - m_previousLeft) + (rightDistance - m_previousRight)) / 2.0; + + // Use trapezoidal integration to get better position accuracy + m_x += Math.cos(m_previousAngle) * distance / 2; + m_y += Math.sin(m_previousAngle) * distance / 2; + double angle = getHeadingAngle(); + m_x += Math.cos(angle) * distance / 2; + m_y += Math.sin(angle) * distance / 2; + + m_previousLeft = leftDistance; + m_previousRight = rightDistance; + m_previousAngle = angle; + + // calculate velocity + double currentTime = RobotProvider.getTimeProvider().get(); + m_velocity = distance / (currentTime - m_previousTime); + m_previousTime = currentTime; + } + + public double getHeadingAngle() { + return m_gyro.getAngle() % 360; + } + + public void resetGyro() { + m_gyro.reset(); + } + + public double getXPosition() { + return m_x; + } + + public double getYPosition() { + return m_y; + } + + public void setDrivePower(double leftPower, double rightPower) { + checkContextOwnership(); + m_leftMotor.set(leftPower); + m_rightMotor.set(rightPower); + } + + public void setArcadeDrive(double fwdPower, double turnPower) { + double maximum = Math.max(Math.abs(fwdPower), Math.abs(turnPower)); + double total = fwdPower + turnPower; + double difference = fwdPower - turnPower; + + if (fwdPower >= 0) { + if (turnPower >= 0) { + setDrivePower(maximum, difference); + } else { + setDrivePower(total, maximum); + } + } else { + if (turnPower >= 0) { + setDrivePower(total, -maximum); + } else { + setDrivePower(-maximum, difference); + } + } + } +} diff --git a/src/main/java/com/team766/robot/mechanisms/Intake.java b/src/main/java/com/team766/robot/mechanisms/Intake.java new file mode 100644 index 00000000..073a9b85 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Intake.java @@ -0,0 +1,26 @@ +package com.team766.robot.mechanisms; + +import com.team766.framework.Mechanism; +import com.team766.hal.RobotProvider; +import com.team766.hal.SolenoidController; +import com.team766.hal.MotorController; + +public class Intake extends Mechanism { + private SolenoidController m_intakeArm; + private MotorController m_intakeWheels; + + public Intake() { + m_intakeArm = RobotProvider.instance.getSolenoid("intakeArm"); + m_intakeWheels = RobotProvider.instance.getMotor("intake"); + } + + public void setIntakePower(double intakePower) { + checkContextOwnership(); + m_intakeWheels.set(intakePower); + } + + public void setIntakeArm(boolean state) { + checkContextOwnership(); + m_intakeArm.set(state); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/mechanisms/Launcher.java b/src/main/java/com/team766/robot/mechanisms/Launcher.java new file mode 100644 index 00000000..9212c699 --- /dev/null +++ b/src/main/java/com/team766/robot/mechanisms/Launcher.java @@ -0,0 +1,18 @@ +package com.team766.robot.mechanisms; + +import com.team766.framework.Mechanism; +import com.team766.hal.RobotProvider; +import com.team766.hal.SolenoidController; + +public class Launcher extends Mechanism { + private SolenoidController m_plungerSolenoid; + + public Launcher() { + m_plungerSolenoid = RobotProvider.instance.getSolenoid("launch"); + } + + public void setPlunger(boolean state) { + checkContextOwnership(); + m_plungerSolenoid.set(state); + } +} \ No newline at end of file diff --git a/src/main/java/com/team766/robot/procedures/ExampleDriveSequence.java b/src/main/java/com/team766/robot/procedures/ExampleDriveSequence.java new file mode 100644 index 00000000..f5349672 --- /dev/null +++ b/src/main/java/com/team766/robot/procedures/ExampleDriveSequence.java @@ -0,0 +1,36 @@ +package com.team766.robot.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Robot; +import com.team766.logging.Category; + +public class ExampleDriveSequence extends Procedure { + public ExampleDriveSequence() { + loggerCategory = Category.AUTONOMOUS; + } + + public void run(Context context) { + context.takeOwnership(Robot.drive); + context.takeOwnership(Robot.intake); + + Robot.intake.setIntakeArm(true); + Robot.intake.setIntakePower(1.0); + + for (int i = 0; i < 4; ++i) { + log("Forward movement begins"); + Robot.drive.setDrivePower(0.3, 0.3); + context.waitForSeconds(2.3); + log("Forward movement finished"); + + log("Turning movement begins"); + Robot.drive.setDrivePower(0.1, -0.1); + context.waitForSeconds(2.35); + log("Turning movement finished"); + + context.startAsync(new Launch()); + } + + Robot.drive.setDrivePower(0.0, 0.0); + } +} diff --git a/src/main/java/com/team766/robot/procedures/Launch.java b/src/main/java/com/team766/robot/procedures/Launch.java new file mode 100644 index 00000000..873e1c2e --- /dev/null +++ b/src/main/java/com/team766/robot/procedures/Launch.java @@ -0,0 +1,16 @@ +package com.team766.robot.procedures; + +import com.team766.framework.Context; +import com.team766.framework.Procedure; +import com.team766.robot.Robot; + +public class Launch extends Procedure { + public void run(Context context) { + context.takeOwnership(Robot.launcher); + + log("Launching ball"); + Robot.launcher.setPlunger(true); + context.waitForSeconds(0.5); + Robot.launcher.setPlunger(false); + } +}