mirror of
https://github.com/TimeCrafters/FreightFrenzy.git
synced 2025-12-13 05:02:34 +00:00
I added a new folder/ directory for My Mecanum Wheel Robot, and created a engine for it, and new robot object... followed by a tele op state and this can be uploaded to the robot when finished being built and all the wires are attached and expansion hub is attached
This commit is contained in:
Spencer
2
.idea/misc.xml
generated
2
.idea/misc.xml
generated
@@ -1,7 +1,7 @@
|
||||
<?xml version="1.0" encoding="UTF-8"?>
|
||||
<project version="4">
|
||||
<component name="ExternalStorageConfigurationManager" enabled="true" />
|
||||
<component name="ProjectRootManager" version="2" languageLevel="JDK_11" project-jdk-name="11" project-jdk-type="JavaSDK">
|
||||
<component name="ProjectRootManager" version="2" languageLevel="JDK_11" default="true" project-jdk-name="11" project-jdk-type="JavaSDK">
|
||||
<output url="file://$PROJECT_DIR$/build/classes" />
|
||||
</component>
|
||||
<component name="ProjectType">
|
||||
|
||||
@@ -28,7 +28,6 @@ public class TeleOpState extends CyberarmState {
|
||||
@Override
|
||||
public void start() {
|
||||
super.start();
|
||||
// FIXME: Don't reset when doing autonomous stuff
|
||||
robot.whiteArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
robot.whiteArmBobbin.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
|
||||
robot.orangeArmBobbin.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
|
||||
@@ -74,11 +73,11 @@ public class TeleOpState extends CyberarmState {
|
||||
if (engine.gamepad1.dpad_up || engine.gamepad1.dpad_down) {
|
||||
|
||||
if (engine.gamepad1.dpad_up) {
|
||||
robot.orangeArmRiser.setPower(0.8);
|
||||
robot.orangeArmRiser.setPower(1);
|
||||
}
|
||||
|
||||
if (engine.gamepad1.dpad_down) {
|
||||
robot.orangeArmRiser.setPower(-0.5);
|
||||
robot.orangeArmRiser.setPower(-1);
|
||||
}
|
||||
}
|
||||
// nothing pressed nothing move...
|
||||
@@ -130,11 +129,11 @@ public class TeleOpState extends CyberarmState {
|
||||
if (engine.gamepad2.dpad_down || engine.gamepad2.dpad_up) {
|
||||
|
||||
if (engine.gamepad2.dpad_up) {
|
||||
robot.whiteArmRiser.setPower(8);
|
||||
robot.whiteArmRiser.setPower(1);
|
||||
}
|
||||
|
||||
if (engine.gamepad2.dpad_down) {
|
||||
robot.whiteArmRiser.setPower(-0.5);
|
||||
robot.whiteArmRiser.setPower(-1);
|
||||
}
|
||||
}
|
||||
// nothing pressed nothing move...
|
||||
|
||||
@@ -0,0 +1,252 @@
|
||||
package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
|
||||
|
||||
import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
|
||||
import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
|
||||
import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
|
||||
|
||||
import com.qualcomm.hardware.bosch.BNO055IMU;
|
||||
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
|
||||
import com.qualcomm.hardware.rev.RevTouchSensor;
|
||||
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;
|
||||
import org.firstinspires.ftc.robotcore.external.ClassFactory;
|
||||
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
|
||||
import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Position;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
|
||||
import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
|
||||
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
|
||||
import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
|
||||
import org.timecrafters.FreightFrenzy.Competition.Common.RobotLocation;
|
||||
import org.timecrafters.TimeCraftersConfigurationTool.library.TimeCraftersConfiguration;
|
||||
|
||||
import java.util.ArrayList;
|
||||
import java.util.List;
|
||||
|
||||
public class MecanumRobot {
|
||||
private static final float mmPerInch = 25.4f;
|
||||
private static final float mmTargetHeight = 6 * mmPerInch; // the height of the center of the target image above the floor
|
||||
private static final float halfField = 72 * mmPerInch;
|
||||
private static final float halfTile = 12 * mmPerInch;
|
||||
private static final float oneAndHalfTile = 36 * mmPerInch;
|
||||
|
||||
private static final String TENSORFLOW_MODEL_ASSET = "FreightFrenzy_BCDM.tflite";
|
||||
private static final String[] TENSORFLOW_MODEL_LABELS = {
|
||||
"Ball",
|
||||
"Cube",
|
||||
"Duck",
|
||||
"Marker"
|
||||
};
|
||||
private static final String VUFORIA_KEY = "Abmu1jv/////AAABmYzrcgDEi014nv+wD6PkEPVnOlV2pI3S9sGUMMR/X7hF72x20rP1JcVtsU0nI6VK0yUlYbCSA2k+yMo4hQmPDBvrqeqAgXKa57ilPhW5e1cB3BEevP+9VoJ9QYFhKA3JJTiuFS50WQeuFy3dp0gOPoqHL3XClRFZWbhzihyNnLXgXlKiq+i5GbfONECucQU2DgiuuxYlCaeNdUHl1X5C2pO80zZ6y7PYAp3p0ciXJxqfBoVAklhd69avaAE5Z84ctKscvcbxCS16lq81X7XgIFjshLoD/vpWa300llDG83+Y777q7b5v7gsUCZ6FiuK152Rd272HLuBRhoTXAt0ug9Baq5cz3sn0sAIEzSHX1nah";
|
||||
private static final float VUFORIA_CAMERA_FORWARD_DISPLACEMENT = 0.0f * mmPerInch; // Camera lens forward distance from robot center
|
||||
private static final float VUFORIA_CAMERA_VERTICAL_DISPLACEMENT = 0.0f * mmPerInch; // Camera lens height above ground
|
||||
private static final float VUFORIA_CAMERA_LEFT_DISPLACEMENT = 0.0f * mmPerInch; // Camera lens left distance from robot center
|
||||
|
||||
private final CyberarmEngine engine;
|
||||
public TimeCraftersConfiguration configuration;
|
||||
|
||||
// Shiny
|
||||
public RevBlinkinLedDriver leds;
|
||||
public TFObjectDetector tensorflow;
|
||||
private List<Recognition> tensorflowRecognitions = new ArrayList<>();
|
||||
public VuforiaLocalizer vuforia;
|
||||
private VuforiaTrackables vuforiaTargets;
|
||||
private List<VuforiaTrackable> vuforiaTrackables;
|
||||
private OpenGLMatrix vuforiaCameraLocationOnRobot = OpenGLMatrix
|
||||
.translation(VUFORIA_CAMERA_FORWARD_DISPLACEMENT, VUFORIA_CAMERA_LEFT_DISPLACEMENT, VUFORIA_CAMERA_VERTICAL_DISPLACEMENT)
|
||||
.multiplied(Orientation.getRotationMatrix(AxesReference.EXTRINSIC, AxesOrder.XZY, AngleUnit.DEGREES, 90, 90, 0));
|
||||
private RobotLocation vuforiaLastLocation;
|
||||
private Orientation orientation;
|
||||
|
||||
// Sensors
|
||||
public BNO055IMU imu;
|
||||
public RevTouchSensor whiteMag;
|
||||
public RevTouchSensor orangeMag;
|
||||
|
||||
// Drivetrain
|
||||
public static final double WHEEL_CIRCUMFERENCE = Math.PI * 8;
|
||||
public static final int COUNTS_PER_REVOLUTION = 1000;
|
||||
|
||||
public DcMotor driveFrontLeft, driveFrontRight, driveBackLeft, driveBackRight;
|
||||
|
||||
|
||||
|
||||
public MecanumRobot(CyberarmEngine engine) {
|
||||
this.engine = engine;
|
||||
initMagnetSwitches();
|
||||
initConfiguration();
|
||||
initBlinkin();
|
||||
initIMU();
|
||||
initDrivetrain();
|
||||
initVuforia();
|
||||
initTensorflow();
|
||||
}
|
||||
|
||||
private void initMagnetSwitches() {
|
||||
whiteMag = engine.hardwareMap.get(RevTouchSensor.class, "whiteMag");
|
||||
orangeMag = engine.hardwareMap.get(RevTouchSensor.class, "orangeMag");
|
||||
}
|
||||
|
||||
public double heading() {
|
||||
return orientation.firstAngle;
|
||||
}
|
||||
|
||||
public double roll() {
|
||||
return orientation.secondAngle;
|
||||
}
|
||||
|
||||
public double pitch() {
|
||||
return orientation.thirdAngle;
|
||||
}
|
||||
|
||||
public void updateRobotOrientation(){
|
||||
orientation = imu.getAngularOrientation();
|
||||
}
|
||||
|
||||
public void activateTensorflow() {
|
||||
tensorflow.activate();
|
||||
}
|
||||
|
||||
public List<Recognition> tensorflowDetections() {
|
||||
List<Recognition> updateRecognitions = tensorflow.getUpdatedRecognitions();
|
||||
|
||||
if (updateRecognitions != null) {
|
||||
tensorflowRecognitions = updateRecognitions;
|
||||
}
|
||||
|
||||
return tensorflowRecognitions;
|
||||
}
|
||||
|
||||
public void deactivateTensorflow() {
|
||||
tensorflow.deactivate();
|
||||
}
|
||||
|
||||
public void activateVuforia() {
|
||||
vuforiaTargets.activate();
|
||||
}
|
||||
|
||||
public RobotLocation vuforiaLocation() {
|
||||
for (VuforiaTrackable trackable : vuforiaTrackables) {
|
||||
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
|
||||
OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
|
||||
if (robotLocationTransform != null) {
|
||||
vuforiaLastLocation = new RobotLocation(robotLocationTransform);
|
||||
}
|
||||
|
||||
// Recycle old position since there is no new data
|
||||
return vuforiaLastLocation;
|
||||
}
|
||||
}
|
||||
|
||||
// Return null if there is no visible trackable
|
||||
return null;
|
||||
}
|
||||
|
||||
private void vuforiaIdentifyTarget(int targetIndex, String targetName, float dx, float dy, float dz, float rx, float ry, float rz) {
|
||||
VuforiaTrackable aTarget = vuforiaTargets.get(targetIndex);
|
||||
aTarget.setName(targetName);
|
||||
aTarget.setLocation(OpenGLMatrix.translation(dx, dy, dz)
|
||||
.multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, rx, ry, rz)));
|
||||
}
|
||||
|
||||
public void deactivateVuforia() {
|
||||
vuforiaTargets.deactivate();
|
||||
}
|
||||
|
||||
public double ticksToInches(int ticks) {
|
||||
return ticks * (WHEEL_CIRCUMFERENCE / COUNTS_PER_REVOLUTION);
|
||||
}
|
||||
|
||||
public double inchesToTicks(double inches) {
|
||||
return inches * (COUNTS_PER_REVOLUTION / WHEEL_CIRCUMFERENCE);
|
||||
}
|
||||
|
||||
private void initConfiguration() {
|
||||
this.configuration = new TimeCraftersConfiguration();
|
||||
}
|
||||
|
||||
private void initBlinkin() {
|
||||
// engine.hardwareMap.get(RevBlinkinLedDriver.class, "leds");
|
||||
}
|
||||
|
||||
private void initIMU() {
|
||||
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
|
||||
|
||||
parameters.mode = BNO055IMU.SensorMode.IMU;
|
||||
parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
|
||||
parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
|
||||
parameters.loggingEnabled = false;
|
||||
|
||||
this.imu = engine.hardwareMap.get(BNO055IMU.class, "imu");
|
||||
imu.initialize(parameters);
|
||||
|
||||
imu.startAccelerationIntegration(new Position(), new Velocity(), 10);
|
||||
}
|
||||
|
||||
private void initDrivetrain() {
|
||||
driveFrontRight = engine.hardwareMap.dcMotor.get("driveWarehouseRight");
|
||||
driveFrontLeft = engine.hardwareMap.dcMotor.get("driveWarehouseLeft");
|
||||
driveBackRight = engine.hardwareMap.dcMotor.get("driveGoalRight");
|
||||
driveBackLeft = engine.hardwareMap.dcMotor.get("driveGoalLeft");
|
||||
|
||||
driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
|
||||
driveBackLeft.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
driveBackRight.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
}
|
||||
|
||||
|
||||
private void initVuforia() {
|
||||
int cameraMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
|
||||
"cameraMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
|
||||
VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
|
||||
|
||||
parameters.vuforiaLicenseKey = VUFORIA_KEY;
|
||||
parameters.cameraName = engine.hardwareMap.get(WebcamName.class, "Webcam 1");
|
||||
parameters.useExtendedTracking = false;
|
||||
|
||||
vuforia = ClassFactory.getInstance().createVuforia(parameters);
|
||||
|
||||
vuforiaTargets = vuforia.loadTrackablesFromAsset("FreightFrenzy");
|
||||
|
||||
vuforiaTrackables = new ArrayList<>(vuforiaTargets);
|
||||
|
||||
// Name and locate each trackable object
|
||||
vuforiaIdentifyTarget(0, "Blue Storage", -halfField, oneAndHalfTile, mmTargetHeight, 90, 0, 90);
|
||||
vuforiaIdentifyTarget(1, "Blue Alliance Wall", halfTile, halfField, mmTargetHeight, 90, 0, 0);
|
||||
vuforiaIdentifyTarget(2, "Red Storage", -halfField, -oneAndHalfTile, mmTargetHeight, 90, 0, 90);
|
||||
vuforiaIdentifyTarget(3, "Red Alliance Wall", halfTile, -halfField, mmTargetHeight, 90, 0, 180);
|
||||
|
||||
for (VuforiaTrackable trackable : vuforiaTrackables) {
|
||||
((VuforiaTrackableDefaultListener) trackable.getListener())
|
||||
.setCameraLocationOnRobot(parameters.cameraName, vuforiaCameraLocationOnRobot);
|
||||
}
|
||||
}
|
||||
|
||||
private void initTensorflow() {
|
||||
int tfodMonitorViewId = engine.hardwareMap.appContext.getResources().getIdentifier(
|
||||
"tfodMonitorViewId", "id", engine.hardwareMap.appContext.getPackageName());
|
||||
|
||||
TFObjectDetector.Parameters parameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
|
||||
parameters.minResultConfidence = 0.8f;
|
||||
parameters.isModelTensorFlow2 = true;
|
||||
parameters.inputSize = 320;
|
||||
|
||||
tensorflow = ClassFactory.getInstance().createTFObjectDetector(parameters, vuforia);
|
||||
tensorflow.loadModelFromAsset(TENSORFLOW_MODEL_ASSET, TENSORFLOW_MODEL_LABELS);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
|
||||
|
||||
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmEngine;
|
||||
|
||||
@TeleOp(name = "TeleOp Mecanum Robot")
|
||||
|
||||
public class Mecanum_Drive_Engine extends CyberarmEngine {
|
||||
@Override
|
||||
public void setup() { addState(new TeleOpStateMecanumRobot(new MecanumRobot(this)));}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,116 @@
|
||||
package org.timecrafters.FreightFrenzy.MecanumRobot_Spencer;
|
||||
|
||||
import org.cyberarm.engine.V2.CyberarmState;
|
||||
import org.timecrafters.FreightFrenzy.MecanumRobot_Spencer.MecanumRobot;
|
||||
|
||||
public class TeleOpStateMecanumRobot extends CyberarmState {
|
||||
|
||||
MecanumRobot robot;
|
||||
|
||||
public TeleOpStateMecanumRobot(MecanumRobot mecanumRobot) {
|
||||
|
||||
}
|
||||
|
||||
@Override
|
||||
public void exec() {
|
||||
|
||||
// Slow Mode
|
||||
|
||||
if (engine.gamepad1.left_trigger > 0.5){
|
||||
// TankDrive
|
||||
// Left joystick foward; Left side foward;
|
||||
|
||||
robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
|
||||
robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
|
||||
|
||||
// Right joystick foward; Right side foward;
|
||||
robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5);
|
||||
robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5);
|
||||
|
||||
|
||||
// Strafing left and right
|
||||
|
||||
if (engine.gamepad1.left_bumper) {
|
||||
robot.driveFrontLeft.setPower(0.5);
|
||||
robot.driveBackLeft.setPower(-0.5);
|
||||
robot.driveFrontRight.setPower(-0.5);
|
||||
robot.driveBackRight.setPower(0.5);
|
||||
} else if (engine.gamepad1.right_bumper) {
|
||||
robot.driveFrontLeft.setPower(-0.5);
|
||||
robot.driveBackLeft.setPower(0.5);
|
||||
robot.driveFrontRight.setPower(0.5);
|
||||
robot.driveBackRight.setPower(-0.5);
|
||||
} else {
|
||||
robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
|
||||
robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 0.5);
|
||||
robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5);
|
||||
robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5);
|
||||
|
||||
}
|
||||
}
|
||||
// Normal Speed
|
||||
|
||||
else if (engine.gamepad1.right_stick_button){
|
||||
|
||||
// left stick foward; right side foward
|
||||
robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
|
||||
// right stick foward; right side foward;
|
||||
robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
|
||||
robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
|
||||
|
||||
|
||||
// Strafing left and right
|
||||
|
||||
if (engine.gamepad1.left_bumper) {
|
||||
robot.driveFrontLeft.setPower(1);
|
||||
robot.driveBackLeft.setPower(-1);
|
||||
robot.driveFrontRight.setPower(-1);
|
||||
robot.driveBackRight.setPower(1);
|
||||
} else if (engine.gamepad1.right_bumper) {
|
||||
robot.driveFrontLeft.setPower(-1);
|
||||
robot.driveBackLeft.setPower(1);
|
||||
robot.driveFrontRight.setPower(1);
|
||||
robot.driveBackRight.setPower(-1);
|
||||
} else {
|
||||
robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
|
||||
robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
else {
|
||||
// left stick foward; right side foward
|
||||
robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
|
||||
// right stick foward; right side foward;
|
||||
robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
|
||||
robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
|
||||
|
||||
|
||||
// Strafing left and right
|
||||
|
||||
if (engine.gamepad1.left_bumper) {
|
||||
robot.driveFrontLeft.setPower(1);
|
||||
robot.driveBackLeft.setPower(-1);
|
||||
robot.driveFrontRight.setPower(-1);
|
||||
robot.driveBackRight.setPower(1);
|
||||
} else if (engine.gamepad1.right_bumper) {
|
||||
robot.driveFrontLeft.setPower(-1);
|
||||
robot.driveBackLeft.setPower(1);
|
||||
robot.driveFrontRight.setPower(1);
|
||||
robot.driveBackRight.setPower(-1);
|
||||
} else {
|
||||
robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 1);
|
||||
robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 1);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user