TimeCrafters/UltimateGoal
1
0
Fork 0
mirror of https://github.com/TimeCrafters/UltimateGoal.git synced 2025-12-13 05:02:34 +00:00
Code Issues Projects Releases Wiki Activity

Synced directions of vision and Odometry localizers

Browse Source
This commit is contained in:
Nathaniel Palme
2020-10-27 20:11:51 -05:00
parent a982c6c93e
commit 8ca11b3b25
5 changed files with 69 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/LauncherTest.java Normal file
View File

@@ -0,0 +1,28 @@
package org.timecrafters.UltimateGoal.HardwareTesting;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.cyberarm.engine.V2.CyberarmState;
import org.timecrafters.UltimateGoal.Robot;
public class LauncherTest extends CyberarmState {
DcMotor LaunchMotor;
@Override
public void init() {
LaunchMotor = engine.hardwareMap.dcMotor.get("launcher");
}
@Override
public void exec() {
LaunchMotor.setPower(1);
}
@Override
public void telemetry() {
}
}

14
TeamCode/src/main/java/org/timecrafters/UltimateGoal/HardwareTesting/TestingEngine.java
View File

@@ -10,18 +10,18 @@ public class TestingEngine extends CyberarmEngine {
private Robot robot;
@Override
public void init() {
robot = new Robot(hardwareMap);
robot.initHardware();
super.init();
}
// @Override
// public void init() {
// robot = new Robot(hardwareMap);
// robot.initHardware();
// super.init();
// }
@Override
public void setup() {
addState(new EncoderTest(robot));
addState(new LauncherTest());
}

3
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/LocalizerTestingEngine.java
View File

@@ -1,11 +1,12 @@
package org.timecrafters.UltimateGoal.LocalizerTesting;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.cyberarm.engine.V2.CyberarmEngine;
import org.timecrafters.UltimateGoal.Robot;
@Autonomous (name = "Localizer Test")
@TeleOp(name = "Localizer Test")
public class LocalizerTestingEngine extends CyberarmEngine {
private Robot robot;

9
TeamCode/src/main/java/org/timecrafters/UltimateGoal/LocalizerTesting/VuforiaNavTesting.java
View File

@@ -17,6 +17,7 @@ public class VuforiaNavTesting extends CyberarmState {
@Override
public void exec() {
robot.setDrivePower(-0.2 * engine.gamepad1.left_stick_y, -0.2 * engine.gamepad1.right_stick_y);
robot.updateLocation();
X = robot.getLocationX();
Y = robot.getLocationY();
@@ -27,8 +28,12 @@ public class VuforiaNavTesting extends CyberarmState {
public void telemetry() {
engine.telemetry.addData("Target Visible", robot.trackableVisible);
engine.telemetry.addData("X", X);
engine.telemetry.addData("Y", Y);
engine.telemetry.addData("Odo X", robot.ticksToInches(X));
engine.telemetry.addData("Odo Y", robot.ticksToInches(Y));
engine.telemetry.addData("Vis X", robot.visionX);
engine.telemetry.addData("Vis Y", robot.visionY);
engine.telemetry.addData("Angle", angle);
}
}

34
TeamCode/src/main/java/org/timecrafters/UltimateGoal/Robot.java
View File

@@ -57,17 +57,21 @@ public class Robot {
static final float mmPerInch = 25.4f;
// Inches Forward of axis of rotation
static final float CAMERA_FORWARD_DISPLACEMENT = 4.0f;
static final float CAMERA_FORWARD_DISPLACEMENT = 4.25f;
// Inches above Ground
static final float CAMERA_VERTICAL_DISPLACEMENT = 8.0f;
static final float CAMERA_VERTICAL_DISPLACEMENT = 4.5f;
// Inches Left of axis of rotation
static final float CAMERA_LEFT_DISPLACEMENT = 0;
static final float CAMERA_LEFT_DISPLACEMENT = 2f;
//Robot Localizatoin
private double locationX;
private double locationY;
private float rotation;
public double visionX;
public double visionY;
public double traveledLeft;
public double traveledRight;
@@ -183,10 +187,10 @@ public class Robot {
}
public void updateLocation(){
// IMU orientation is inverted to have clockwise be positive.
rotation = -imu.getAngularOrientation().firstAngle;
// orientation is inverted to have clockwise be positive.
float imuAngle = -imu.getAngularOrientation().firstAngle;
float rotationChange = rotation - rotationPrevious;
float rotationChange = imuAngle - rotationPrevious;
int encoderLeftCurrent = encoderLeft.getCurrentPosition();
int encoderRightCurrent = encoderRight.getCurrentPosition();
double encoderLeftChange = encoderLeftCurrent - encoderLeftPrevious;
@@ -194,10 +198,11 @@ public class Robot {
traveledLeft += Math.abs(encoderLeftChange);
traveledRight += Math.abs(encoderRightChange);
rotation += rotationChange;
encoderLeftPrevious = encoderLeftCurrent;
encoderRightPrevious = encoderRightCurrent;
rotationPrevious = rotation;
rotationPrevious = imuAngle;
double average = (encoderLeftChange+encoderRightChange)/2;
@@ -212,7 +217,7 @@ public class Robot {
if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
OpenGLMatrix robotLocation = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
//TODO : make rotation update with Vuforia Nav.
//this is used for debugging purposes.
trackableVisible = true;
@@ -223,8 +228,19 @@ public class Robot {
VectorF translation = lastConfirmendLocation.getTranslation();
locationX = inchesToTicks(translation.get(1) / mmPerInch);
locationX = -inchesToTicks(translation.get(1) / mmPerInch);
locationY = inchesToTicks( translation.get(0) / mmPerInch);
// visionX = -translation.get(1) / mmPerInch;
// visionY = translation.get(0) / mmPerInch;
//For our tornament, it makes sence to make zero degrees towards the goal, orientation is inverted to have clockwise be positive.
Orientation rotation = Orientation.getOrientation(lastConfirmendLocation, EXTRINSIC, XYZ, DEGREES);
this.rotation = 90-rotation.thirdAngle;
if (this.rotation > 180) {
this.rotation -= -180;
}
break;
}