Skip to content
This repository has been archived by the owner on Jan 19, 2025. It is now read-only.

Commit

Permalink
Navigation wokring with gyroscope and correction now.
Browse files Browse the repository at this point in the history
  • Loading branch information
@Akachukwuegbosimba
Akachukwuegbosimba committed Jan 12, 2024
1 parent 81b6ccc commit 6f61ba2
Show file tree
Hide file tree
Showing 5 changed files with 114 additions and 36 deletions.
24 changes: 13 additions & 11 deletions Autonomous.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -52,20 +52,22 @@ public void runOpMode() throws InterruptedException {
intakeWheel = hardwareMap.get(DcMotor.class, "intakeWheel");
// firstPixel = hardwareMap.get(Servo.class, "FirstPixel");
// secondPixel = hardwareMap.get(Servo.class, "SecondPixel");
randomizationWebcam = new PlaceDetectionWebcam();
randomizationWebcam.init(hardwareMap, "Blue.tflite");
int loc = 0;
while (!opModeIsActive()){
if (isStopRequested()){
break;
}
loc = detectTape(randomizationWebcam);
// randomizationWebcam = new PlaceDetectionWebcam();
// randomizationWebcam.init(hardwareMap, "Blue.tflite");
// int loc = 0;
// while (opModeInInit()){
// loc = detectTape(randomizationWebcam);
// }
// randomizationWebcam.stop();
navigation = new Navigation(new double[]{12, 60}, 0, this, hardwareMap);
while(opModeInInit()){
TelemetryWrapper.setLine(7, "x: " + navigation.getCurrentPos()[0] + " y: " +navigation.getCurrentPos()[1]);
TelemetryWrapper.setLine(8, "Gyro bearing: " + navigation.getGyroBearing());
}
randomizationWebcam.stop();
navigation = new Navigation(new double[]{12, 50}, 270, this, hardwareMap);

navigation = new Navigation(new double[]{12, 60}, 270, this, hardwareMap);
navigation.MoveToPosDirect(new double[]{12, 36});
navigation.MoveToPosDirect(new double[]{12, 12});
navigation.MoveToPosDirect(new double[]{36, 36});


// // Turn around
Expand Down
2 changes: 1 addition & 1 deletion TeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,7 @@ public void runOpMode() {
intakeWheel.setPower((gamepad2.right_trigger - gamepad2.left_trigger) * 0.8);

if (Math.abs(gamepad2.left_stick_y) > 0.001) {
outputSlide.startMoveToRelativePos((int) -gamepad2.left_stick_y * 100);
outputSlide.startMoveToRelativePos((int) -gamepad2.left_stick_y * 500);
}
if (gp2.pressing(ButtonHelper.b)) {
outputSlide.startMoveToPos(1430);
Expand Down
104 changes: 89 additions & 15 deletions modules/Navigation.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,14 @@
import org.firstinspires.ftc.teamcode.modules.Webcams.AprilTagWebcam;
import org.firstinspires.ftc.teamcode.util.TelemetryWrapper;

import com.qualcomm.hardware.bosch.BNO055IMU;
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;

public class Navigation {
private double[] currentPos;
private double currentBearing;
Expand All @@ -20,14 +28,30 @@ public class Navigation {

private AprilTagWebcam tagCam;

private double lastAprilTagBearing;
BNO055IMU imu;
private double initGyroReading;
public Navigation (double[] initPos, double initBearing, LinearOpMode opMode, HardwareMap map){
currentPos = initPos;
lastAprilTagPos = initPos;
lastDriveTrainPos = initPos;
lastDriveTrainBearing = initBearing;
currentBearing = initBearing;
driveTrain = new DriveTrain(opMode);
driveTrain.init(map);
tagCam = new AprilTagWebcam();
tagCam.init(map);
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;
imu = map.get(BNO055IMU.class, "imu");
imu.initialize(parameters);
while (opMode.opModeInInit() && !imu.isGyroCalibrated()){
TelemetryWrapper.setLine(2, "Calibrating gyro...");
}
TelemetryWrapper.setLine(2, "Gyro calibrated");
initGyroReading = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle - initBearing;
}

public double[] getCurrentPosition(){
Expand All @@ -38,39 +62,89 @@ public double getCurrentBearing() {
return currentBearing;
}

public double getGyroBearing(){
return ((imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle - initGyroReading%360)+360)%360;
}

public double magnitude(double[] vector){
double sum = 0;
for (double component:vector){
sum += Math.pow(component, 2);
}
return Math.sqrt(sum);
}
public double[] getCurrentPos(){
lastAprilTagPos = tagCam.detectIter(currentBearing);
if (tagCam.isDetecting){
return lastAprilTagPos;
}
return lastDriveTrainPos;
}

public void setBearing(double targetBearing){
targetBearing = (targetBearing%360)+360;
double dir =((targetBearing-currentBearing < 180)? -1 : 1);
driveTrain.move(0, 0, 0.1*dir, 1);
while (Math.abs(targetBearing - currentBearing) > 0.5){
TelemetryWrapper.setLine(6, "Target Bearing: " + targetBearing + " | " + "Current Bearing: " + currentBearing);
currentBearing = getGyroBearing();
}
driveTrain.stopStayInPlace();
}


public void MoveToPosDirect(double[] targetPos){
double targetBearing = Math.toDegrees(Math.atan2(targetPos[1]-currentPos[1], targetPos[0]-currentPos[0]));
targetBearing = (360+targetBearing)%360;
TelemetryWrapper.setLine(6, "Target Bearing: " + targetBearing + " | " + "Delta Bearing: " + -(targetBearing - currentBearing));
driveTrain.translate(0.2, 0,0, -(targetBearing-currentBearing), 10);
currentBearing = targetBearing;
setBearing(targetBearing);
double[] lastEncoderPos = driveTrain.getEncPos();
double[] displacement = new double[]{targetPos[0]- currentPos[0], targetPos[1] - currentPos[1]};
driveTrain.move(0,0.2, 0, 1);
while (magnitude(displacement) > 0.5){
driveTrain.move(0,0.7, 0, 1);
while (magnitude(displacement) > 2){
TelemetryWrapper.setLine(5, "x: "+ currentPos[0] + "| y: " + currentPos[1] + "| theta: " + currentBearing);
// if (tagCam.isDetecting){
// currentPos = tagCam.detectIter();
// TelemetryWrapper.setLine(7, "CAM");
// }else {
double[] currentEncPos = driveTrain.getEncPos();
currentBearing = getGyroBearing();
lastAprilTagPos = tagCam.detectIter(currentBearing);
if (tagCam.isDetecting){
currentPos = lastAprilTagPos;
TelemetryWrapper.setLine(7, "CAM");
}else {
TelemetryWrapper.setLine(7, "ENC");
double delta = 0;
for (int i = 0; i < lastEncoderPos.length; i++) {
delta += Math.abs(driveTrain.getEncPos()[i] - lastEncoderPos[i]) / driveTrain.COUNTS_PER_INCH;
delta += Math.abs(currentEncPos[i] - lastEncoderPos[i]) / driveTrain.COUNTS_PER_INCH;
}
delta /= lastEncoderPos.length;
currentPos = new double[]{currentPos[0] + Math.cos(Math.toRadians(currentBearing)) * delta, currentPos[1] + Math.sin(Math.toRadians(currentBearing)) * delta};
// }
}
displacement = new double[]{targetPos[0]- currentPos[0], targetPos[1] - currentPos[1]};
lastEncoderPos = driveTrain.getEncPos();
lastEncoderPos = currentEncPos;
}
driveTrain.stopStayInPlace();
strafeToPos(targetPos);
}

public void strafeToPos(double[] targetPos){
double[] displacement = new double[]{targetPos[0]- currentPos[0], targetPos[1] - currentPos[1]};
double dx = Math.cos(Math.toRadians(currentBearing-90))*(displacement[0]) - Math.sin(Math.toRadians(currentBearing-90))*(displacement[1]);
double dy = Math.sin(Math.toRadians(currentBearing-90))*(displacement[0]) + Math.cos(Math.toRadians(currentBearing-90))*(displacement[1]);

driveTrain.translate(0.4, dx, 0, 0, 10);
driveTrain.translate(0.4, 0, dy, 0, 10);
lastAprilTagPos = tagCam.detectIter(currentBearing);
if(tagCam.isDetecting){
currentPos = lastAprilTagPos;
TelemetryWrapper.setLine(7, "CAM");
}else{
currentPos = targetPos;
TelemetryWrapper.setLine(7, "ENC");
}
driveTrain.stop();
driveTrain.stopStayInPlace();
}

public void MoveToPosAtAngle(double[] targetPos, double angle){
double directAngle = Math.toDegrees(Math.atan2(targetPos[1]-currentPos[1], targetPos[0]-currentPos[0]));
directAngle = (360+directAngle)%360;

}

}
16 changes: 9 additions & 7 deletions modules/Webcams/AprilTagWebcam.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,8 @@ public class AprilTagWebcam {
*/
public boolean isDetecting = false;

private final double[] tagCamOffset = new double[] {0, -8};

/**
* Last seen position
*/
Expand Down Expand Up @@ -144,30 +146,30 @@ public void toggle(){

/**
* Add telemetry about AprilTag detections.
* @param bearing the bearing of the webcam in euler angles (x, y, z)
*/
public double[] detectIter() {
public double[] detectIter(double bearing) {

List<org.firstinspires.ftc.vision.apriltag.AprilTagDetection> currentDetections = aprilTag.getDetections();

double avg_z = 0;
double avg_y = 0;
double avg_x = 0;
isDetecting = false;
// Step through the list of detections and display info for each one.
for (AprilTagDetection detection : currentDetections) {
if (detection.metadata != null) {
isDetecting = true;
avg_z += detection.metadata.fieldPosition.get(2) - detection.ftcPose.z;
avg_y += detection.metadata.fieldPosition.get(1) - detection.ftcPose.y;
avg_x += detection.metadata.fieldPosition.get(0) - detection.ftcPose.x;
avg_y += detection.metadata.fieldPosition.get(1)-(Math.sin(Math.toRadians(bearing-90))*(detection.ftcPose.x-tagCamOffset[0])+ Math.cos(Math.toRadians(bearing-90))*(detection.ftcPose.y-tagCamOffset[1]));
avg_x += detection.metadata.fieldPosition.get(0)-(Math.cos(Math.toRadians(bearing-90))*(detection.ftcPose.x-tagCamOffset[0]) - Math.sin(Math.toRadians(bearing-90))*(detection.ftcPose.y-tagCamOffset[1]));
}
} // end for() loop

avg_z /= currentDetections.size();
avg_y /= currentDetections.size();
avg_x /= currentDetections.size();


if (isDetecting) {
lastPosition = new double[]{avg_x, avg_y, avg_z};
lastPosition = new double[]{avg_x, avg_y};
}
return lastPosition;

Expand Down
4 changes: 2 additions & 2 deletions modules/output/LinearSlide.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,15 +85,15 @@ public void startRetraction() {
*/
@Override
public void tick() {
if (elevatorBtnLeft.isPressed()){
if (elevatorBtnLeft.isPressed() && elevatorLeft.isBusy()){
int targetPos = elevatorLeft.getTargetPosition();
double power = elevatorLeft.getPower();
elevatorLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
elevatorLeft.setTargetPosition(Math.max(targetPos, 0));
elevatorLeft.setMode(DcMotor.RunMode.RUN_TO_POSITION);
elevatorLeft.setPower(power);
}
if (elevatorBtnRight.isPressed()){
if (elevatorBtnRight.isPressed() && elevatorRight.isBusy()){
int targetPos = elevatorRight.getTargetPosition();
double power = elevatorRight.getPower();
elevatorRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
Expand Down

0 comments on commit 6f61ba2

Please sign in to comment.