mirror of
https://github.com/TimeCrafters/FreightFrenzy.git
synced 2025-12-13 05:02:34 +00:00
Fixed up tele op and robot object
This commit is contained in:
Spencer
@@ -1,82 +1,15 @@
|
||||
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;
|
||||
|
||||
@@ -84,121 +17,14 @@ public class MecanumRobot {
|
||||
|
||||
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");
|
||||
|
||||
driveFrontRight = engine.hardwareMap.dcMotor.get("driveFrontRight");
|
||||
driveFrontLeft = engine.hardwareMap.dcMotor.get("driveFrontLeft");
|
||||
driveBackRight = engine.hardwareMap.dcMotor.get("driveBackRight");
|
||||
driveBackLeft = engine.hardwareMap.dcMotor.get("driveBackLeft");
|
||||
|
||||
driveFrontLeft.setDirection(DcMotorSimple.Direction.FORWARD);
|
||||
driveFrontRight.setDirection(DcMotorSimple.Direction.REVERSE);
|
||||
@@ -206,47 +32,6 @@ public class MecanumRobot {
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -18,12 +18,12 @@ public class TeleOpStateMecanumRobot extends CyberarmState {
|
||||
|
||||
if (engine.gamepad1.left_trigger > 0.5){
|
||||
// TankDrive
|
||||
// Left joystick foward; Left side foward;
|
||||
// Left joystick forward; Left side forward;
|
||||
|
||||
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;
|
||||
// Right joystick forward; Right side forward;
|
||||
robot.driveFrontRight.setPower(engine.gamepad1.right_stick_y * 0.5);
|
||||
robot.driveBackRight.setPower(engine.gamepad1.right_stick_y * 0.5);
|
||||
|
||||
@@ -40,19 +40,20 @@ public class TeleOpStateMecanumRobot extends CyberarmState {
|
||||
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);
|
||||
}
|
||||
// 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){
|
||||
if (engine.gamepad1.right_stick_button) {
|
||||
|
||||
// left stick foward; right side foward
|
||||
// left stick forward; right side foward
|
||||
robot.driveFrontLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
robot.driveBackLeft.setPower(engine.gamepad1.left_stick_y * 1);
|
||||
|
||||
@@ -60,7 +61,7 @@ public class TeleOpStateMecanumRobot extends CyberarmState {
|
||||
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) {
|
||||
@@ -94,23 +95,23 @@ public class TeleOpStateMecanumRobot extends CyberarmState {
|
||||
|
||||
// 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);
|
||||
|
||||
}
|
||||
// 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