mirror of
https://github.com/TimeCrafters/FTC_2022
synced 2025-12-15 21:22:33 +00:00
I completely forgot to commit any of the new stuff I have done. Basically it is just states and engines for my not so mini "minibot" with a couple TeleOp states and engines and a new autonomous drive state and engine. and theres a distance sensor test to give me telemetry on distance from each ball amount when the hopper is filled up or empty or filled with one or two or three balls. which the logic was very in efficient the way i chose to do it and then when implememnting caused problems in my standard TeleOp (Fancy Drive TeleOp) and then came up with a better solution. with proper comparisons and now it works amazingly
This commit is contained in:
SpencerPiha
@@ -0,0 +1,20 @@
|
||||
package org.timecrafters.minibots.cyberarm.engines;
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.minibots.cyberarm.states.DemoPingPongState;
|
||||
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
|
||||
|
||||
@TeleOp(name = "DemoTeleOp")
|
||||
|
||||
public class DemoPingPongEngine extends CyberarmEngine {
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
MecanumRobot robot;
|
||||
|
||||
robot = new MecanumRobot(this);
|
||||
|
||||
addState(new DemoPingPongState(robot));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,25 @@
|
||||
package org.timecrafters.minibots.cyberarm.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.minibots.cyberarm.states.DemoPingPongState;
|
||||
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
|
||||
|
||||
@TeleOp( name = "Distance Sensor Test")
|
||||
|
||||
public class DistanceSensorTest extends CyberarmEngine {
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
MecanumRobot robot;
|
||||
|
||||
robot = new MecanumRobot(this);
|
||||
|
||||
addState(new org.timecrafters.minibots.cyberarm.states.DistanceSensorTest(robot));
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,11 @@
|
||||
package org.timecrafters.minibots.cyberarm.engines;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
public class DriveAutonomous extends CyberarmEngine {
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package org.timecrafters.minibots.cyberarm.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
|
||||
import org.timecrafters.minibots.cyberarm.states.Mecanum_Robot_State;
|
||||
|
||||
@TeleOp(name = "Mecanum Robot TeleOp")
|
||||
|
||||
public class Mecanum_Robot_Engine extends CyberarmEngine {
|
||||
MecanumRobot robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
robot = new MecanumRobot(this);
|
||||
|
||||
addState(new Mecanum_Robot_State(robot));
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,22 @@
|
||||
package org.timecrafters.minibots.cyberarm.engines;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
import org.timecrafters.minibots.cyberarm.states.MecanumRobot;
|
||||
import org.timecrafters.minibots.cyberarm.states.Mecanum_Fancy_Drive_State;
|
||||
|
||||
@TeleOp(name = "Fancy Drive TeleOp")
|
||||
|
||||
public class Mecanum_fancy_drive_TeleOp extends CyberarmEngine {
|
||||
MecanumRobot robot;
|
||||
|
||||
@Override
|
||||
public void setup() {
|
||||
|
||||
robot = new MecanumRobot(this);
|
||||
|
||||
addState(new Mecanum_Fancy_Drive_State(robot));
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,89 @@
|
||||
package org.timecrafters.minibots.cyberarm.states;
|
||||
|
||||
import com.qualcomm.robotcore.hardware.Gamepad;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class DemoPingPongState extends CyberarmState {
|
||||
|
||||
private final MecanumRobot robot;
|
||||
private boolean A;
|
||||
private boolean B;
|
||||
private boolean X;
|
||||
private boolean Y;
|
||||
|
||||
public DemoPingPongState(MecanumRobot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
//Gamepad Read
|
||||
|
||||
A = engine.gamepad2.a;
|
||||
B = engine.gamepad2.b;
|
||||
X = engine.gamepad2.x;
|
||||
Y = engine.gamepad2.y;
|
||||
|
||||
Gamepad gamepad = engine.gamepad1;
|
||||
if (engine.gamepad2.right_trigger != 0) {
|
||||
gamepad = engine.gamepad2;
|
||||
}
|
||||
|
||||
double y = -gamepad.left_stick_y; // Remember, this is reversed! because of the negative
|
||||
double x = gamepad.left_stick_x * 1.1; // Counteract imperfect strafing
|
||||
double rx = gamepad.right_stick_x;
|
||||
|
||||
// Denominator is the largest motor power (absolute value) or 1
|
||||
// This ensures all the powers maintain the same ratio, but only when
|
||||
// at least one is out of the range [-1, 1]
|
||||
|
||||
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (y + x + rx) / denominator;
|
||||
double backLeftPower = (y - x + rx) / denominator;
|
||||
double frontRightPower = (y - x - rx) / denominator;
|
||||
double backRightPower = (y + x - rx) / denominator;
|
||||
|
||||
// As I programed this and ran it, I realized everything was backwards
|
||||
// in direction so to fix that i either went in the robot object state and reversed
|
||||
// directions on drive motors or put a negative in behind the joystick power to reverse it.
|
||||
// I put negatives in to reverse it because it was the easiest at the moment.
|
||||
|
||||
robot.frontLeftDrive.setPower(-frontLeftPower);
|
||||
robot.backLeftDrive.setPower(-backLeftPower);
|
||||
robot.frontRightDrive.setPower(-frontRightPower);
|
||||
robot.backRightDrive.setPower(-backRightPower);
|
||||
|
||||
//--------------------------------------------------------------------------------------------------------------------------------
|
||||
// GamePad 2 Controls (GamePad 1 Has no control with these)
|
||||
|
||||
if (A){
|
||||
robot.shooterWheel.setPower(1);
|
||||
robot.collectorRight.setPower(0);
|
||||
}
|
||||
|
||||
if (Y){
|
||||
robot.shooterWheel.setPower(0);
|
||||
robot.collectorRight.setPower(1);
|
||||
}
|
||||
|
||||
if (X){
|
||||
robot.shooterWheel.setPower(0);
|
||||
robot.collectorRight.setPower(0);
|
||||
}
|
||||
|
||||
// Ball blocker Servo Program
|
||||
// This says if the trigger is pressed down 50% or more the servo brings the blocker down
|
||||
if (engine.gamepad2.left_trigger >= 0.5){
|
||||
robot.ballBlocker.setPosition(0.45);
|
||||
}
|
||||
// this else statement says if nothing is pressed then it stays in the up position
|
||||
else {
|
||||
robot.ballBlocker.setPosition(0.52);
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,139 @@
|
||||
package org.timecrafters.minibots.cyberarm.states;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
public class DistanceSensorTest extends CyberarmState {
|
||||
|
||||
private final MecanumRobot robot;
|
||||
public String ballAmount;
|
||||
public DistanceSensorTest(MecanumRobot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.5
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.6
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.7
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.8
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 5.9
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.5
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.6
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.7
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.8
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 6.9
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 7.5) {
|
||||
|
||||
ballAmount = "3 balls";
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.5
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.6
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.7
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.8
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 10.9
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.5
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.6
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.7
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.8
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 11.9) {
|
||||
|
||||
ballAmount = "2 balls";
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.5
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.6
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.7
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.8
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 14.9
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.5
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.6
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.7
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.8
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 15.9
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.5
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.6
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.7
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.8
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 16.9) {
|
||||
|
||||
ballAmount = "1 ball";
|
||||
|
||||
}
|
||||
|
||||
|
||||
if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.4
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.5
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.6
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.7
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.8
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 19.9
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.0
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.1
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.2
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.3
|
||||
|| robot.ballPositionSensor.getDistance(DistanceUnit.CM) == 20.4){
|
||||
|
||||
ballAmount = "no balls";
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
|
||||
engine.telemetry.addData("distance", robot.ballPositionSensor.getDistance(DistanceUnit.CM));
|
||||
engine.telemetry.addData("Amount", ballAmount);
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
package org.timecrafters.minibots.cyberarm.states;
|
||||
|
||||
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
|
||||
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
|
||||
import com.qualcomm.robotcore.hardware.CRServo;
|
||||
import com.qualcomm.robotcore.hardware.DcMotor;
|
||||
import com.qualcomm.robotcore.hardware.DcMotorSimple;
|
||||
import com.qualcomm.robotcore.hardware.Servo;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
public class MecanumRobot {
|
||||
|
||||
private CyberarmEngine engine;
|
||||
|
||||
public DcMotor frontLeftDrive, frontRightDrive, backLeftDrive, backRightDrive, shooterWheel, collectorRight;
|
||||
|
||||
public Servo ballBlocker;
|
||||
|
||||
public RevBlinkinLedDriver ledDriver;
|
||||
public Rev2mDistanceSensor ballPositionSensor;
|
||||
|
||||
public MecanumRobot(CyberarmEngine engine) {
|
||||
this.engine = engine;
|
||||
|
||||
setupRobot();
|
||||
}
|
||||
|
||||
private void setupRobot() {
|
||||
|
||||
//motors configuration
|
||||
frontLeftDrive = engine.hardwareMap.dcMotor.get("front left");
|
||||
frontRightDrive = engine.hardwareMap.dcMotor.get("front right");
|
||||
backRightDrive = engine.hardwareMap.dcMotor.get("back left");
|
||||
backLeftDrive = engine.hardwareMap.dcMotor.get("back right");
|
||||
shooterWheel = engine.hardwareMap.dcMotor.get("shooter wheel");
|
||||
collectorRight = engine.hardwareMap.dcMotor.get("collector motor");
|
||||
ledDriver = engine.hardwareMap.get(RevBlinkinLedDriver.class, "lights");
|
||||
|
||||
//motors direction and encoders
|
||||
frontLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
frontRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
backLeftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
backRightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
|
||||
|
||||
|
||||
|
||||
// Sensor Configuration
|
||||
|
||||
ballPositionSensor = engine.hardwareMap.get(Rev2mDistanceSensor.class, "ball position");
|
||||
|
||||
//Servo configuration
|
||||
ballBlocker = engine.hardwareMap.servo.get("ball blocker");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,143 @@
|
||||
package org.timecrafters.minibots.cyberarm.states;
|
||||
|
||||
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
|
||||
|
||||
|
||||
public class Mecanum_Fancy_Drive_State extends CyberarmState {
|
||||
|
||||
private final MecanumRobot robot;
|
||||
public String ballAmount;
|
||||
public boolean A;
|
||||
public boolean X;
|
||||
public boolean Y;
|
||||
|
||||
|
||||
public Mecanum_Fancy_Drive_State(MecanumRobot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void init() {
|
||||
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
//Gamepad Read
|
||||
|
||||
A = engine.gamepad1.a;
|
||||
X = engine.gamepad1.x;
|
||||
Y = engine.gamepad1.y;
|
||||
|
||||
|
||||
//Drivetrain Variables
|
||||
double y = -engine.gamepad1.left_stick_y; // Remember, this is reversed! because of the negative
|
||||
double x = engine.gamepad1.left_stick_x * 1.1; // Counteract imperfect strafing
|
||||
double rx = engine.gamepad1.right_stick_x;
|
||||
|
||||
|
||||
//Shooter Variables
|
||||
|
||||
//Gamepad variables
|
||||
|
||||
// Denominator is the largest motor power (absolute value) or 1
|
||||
// This ensures all the powers maintain the same ratio, but only when
|
||||
// at least one is out of the range [-1, 1]
|
||||
|
||||
double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
|
||||
double frontLeftPower = (y + x + rx) / denominator;
|
||||
double backLeftPower = (y - x + rx) / denominator;
|
||||
double frontRightPower = (y - x - rx) / denominator;
|
||||
double backRightPower = (y + x - rx) / denominator;
|
||||
|
||||
// As I programed this and ran it, I realized everything was backwards
|
||||
// in direction so to fix that i either went in the robot object state and reversed
|
||||
// directions on drive motors or put a negative in behind the joystick power to reverse it.
|
||||
// I put negatives in to reverse it because it was the easiest at the moment.
|
||||
|
||||
robot.frontLeftDrive.setPower(-frontLeftPower);
|
||||
robot.backLeftDrive.setPower(-backLeftPower);
|
||||
robot.frontRightDrive.setPower(-frontRightPower);
|
||||
robot.backRightDrive.setPower(-backRightPower);
|
||||
|
||||
//-----------------------------------------------------------------
|
||||
// Shooter program
|
||||
|
||||
if (A) {
|
||||
robot.shooterWheel.setPower(1);
|
||||
robot.collectorRight.setPower(0);
|
||||
}
|
||||
|
||||
if (Y) {
|
||||
robot.shooterWheel.setPower(0);
|
||||
robot.collectorRight.setPower(1);
|
||||
}
|
||||
|
||||
if (X) {
|
||||
robot.shooterWheel.setPower(0);
|
||||
robot.collectorRight.setPower(0);
|
||||
}
|
||||
|
||||
// Ball blocker Servo Program
|
||||
// This says if the trigger is pressed down 50% or more the servo brings the blocker down
|
||||
if (engine.gamepad1.left_trigger >= 0.5) {
|
||||
robot.ballBlocker.setPosition(0.45);
|
||||
}
|
||||
// this else statement says if nothing is pressed then it stays in the up position
|
||||
else {
|
||||
robot.ballBlocker.setPosition(0.52);
|
||||
}
|
||||
|
||||
//--------------------------------------------------------------------------------
|
||||
// ball Amount Sensor Program
|
||||
|
||||
if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) < 9) {
|
||||
ballAmount = "3 balls";
|
||||
} else if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) < 13){
|
||||
ballAmount = "2 balls";
|
||||
} else if (robot.ballPositionSensor.getDistance(DistanceUnit.CM) < 17){
|
||||
ballAmount = "1 balls";
|
||||
} else {
|
||||
ballAmount = "no balls";
|
||||
}
|
||||
|
||||
|
||||
|
||||
//-------------------------------------------------------------------------------------------------------------------
|
||||
// LIGHT CONTROLS
|
||||
|
||||
if (robot.ballBlocker.getPosition() == 0.45 && robot.shooterWheel.getPower() == 1) {
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.STROBE_RED);
|
||||
} else if (robot.shooterWheel.getPower() == 1) {
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.RED);
|
||||
} else if (ballAmount.equals("3 balls")) {
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.GREEN);
|
||||
} else if (ballAmount.equals("2 balls")) {
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE_GREEN);
|
||||
} else if (ballAmount.equals("1 balls")) {
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.CP1_2_END_TO_END_BLEND);
|
||||
} else {
|
||||
robot.ledDriver.setPattern(RevBlinkinLedDriver.BlinkinPattern.BLUE);
|
||||
}
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------------------------------------------------------------
|
||||
// Telemetry Data
|
||||
|
||||
|
||||
@Override
|
||||
public void telemetry() {
|
||||
|
||||
engine.telemetry.addData("Distance", robot.ballPositionSensor.getDistance(DistanceUnit.CM));
|
||||
engine.telemetry.addData("Amount", ballAmount);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,114 @@
|
||||
package org.timecrafters.minibots.cyberarm.states;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
|
||||
public class Mecanum_Robot_State extends CyberarmState {
|
||||
|
||||
private final MecanumRobot robot;
|
||||
private float maxSpeed = 1;
|
||||
private double halfSpeed = 0.5;
|
||||
|
||||
public Mecanum_Robot_State(MecanumRobot robot) {
|
||||
this.robot = robot;
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
if (engine.gamepad1.left_trigger > 0.5){
|
||||
|
||||
if (engine.gamepad1.left_bumper) {
|
||||
|
||||
robot.backRightDrive.setPower(halfSpeed);
|
||||
robot.frontRightDrive.setPower(-halfSpeed);
|
||||
robot.backLeftDrive.setPower(-halfSpeed);
|
||||
robot.frontLeftDrive.setPower(halfSpeed);
|
||||
|
||||
} else if (engine.gamepad1.right_bumper) {
|
||||
|
||||
robot.backRightDrive.setPower(-halfSpeed);
|
||||
robot.frontRightDrive.setPower(halfSpeed);
|
||||
robot.backLeftDrive.setPower(halfSpeed);
|
||||
robot.frontLeftDrive.setPower(-halfSpeed);
|
||||
|
||||
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_up) {
|
||||
|
||||
robot.frontLeftDrive.setPower(-halfSpeed);
|
||||
robot.backRightDrive.setPower(-halfSpeed);
|
||||
|
||||
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_up) {
|
||||
|
||||
robot.frontRightDrive.setPower(-halfSpeed);
|
||||
robot.backLeftDrive.setPower(-halfSpeed);
|
||||
|
||||
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_down) {
|
||||
|
||||
robot.backLeftDrive.setPower(halfSpeed);
|
||||
robot.frontRightDrive.setPower(halfSpeed);
|
||||
|
||||
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_down) {
|
||||
|
||||
robot.backRightDrive.setPower(halfSpeed);
|
||||
robot.frontLeftDrive.setPower(halfSpeed);
|
||||
|
||||
}
|
||||
|
||||
else {
|
||||
|
||||
robot.backRightDrive.setPower(engine.gamepad1.right_stick_y * halfSpeed) ;
|
||||
robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y * halfSpeed);
|
||||
robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y * halfSpeed);
|
||||
robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y * halfSpeed);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
else {
|
||||
|
||||
if (engine.gamepad1.left_bumper) {
|
||||
|
||||
robot.backRightDrive.setPower(maxSpeed);
|
||||
robot.frontRightDrive.setPower(-maxSpeed);
|
||||
robot.backLeftDrive.setPower(-maxSpeed);
|
||||
robot.frontLeftDrive.setPower(maxSpeed);
|
||||
|
||||
} else if (engine.gamepad1.right_bumper) {
|
||||
|
||||
robot.backRightDrive.setPower(-maxSpeed);
|
||||
robot.frontRightDrive.setPower(maxSpeed);
|
||||
robot.backLeftDrive.setPower(maxSpeed);
|
||||
robot.frontLeftDrive.setPower(-maxSpeed);
|
||||
|
||||
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_up) {
|
||||
|
||||
robot.frontLeftDrive.setPower(-maxSpeed);
|
||||
robot.backRightDrive.setPower(-maxSpeed);
|
||||
|
||||
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_up) {
|
||||
|
||||
robot.frontRightDrive.setPower(-maxSpeed);
|
||||
robot.backLeftDrive.setPower(-maxSpeed);
|
||||
|
||||
} else if (engine.gamepad1.dpad_right && engine.gamepad1.dpad_down) {
|
||||
|
||||
robot.backLeftDrive.setPower(maxSpeed);
|
||||
robot.frontRightDrive.setPower(maxSpeed);
|
||||
|
||||
} else if (engine.gamepad1.dpad_left && engine.gamepad1.dpad_down) {
|
||||
|
||||
robot.backRightDrive.setPower(maxSpeed);
|
||||
robot.frontLeftDrive.setPower(maxSpeed);
|
||||
|
||||
} else {
|
||||
|
||||
robot.backRightDrive.setPower(engine.gamepad1.right_stick_y * maxSpeed) ;
|
||||
robot.frontRightDrive.setPower(engine.gamepad1.right_stick_y * maxSpeed);
|
||||
robot.backLeftDrive.setPower(engine.gamepad1.left_stick_y * maxSpeed);
|
||||
robot.frontLeftDrive.setPower(engine.gamepad1.left_stick_y * maxSpeed);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user