add gyro rotation to chassi base

RobotIGS · Sep 19, 2023 · 6078b14 · 6078b14
1 parent 7bbb187
commit 6078b14
Show file tree

Hide file tree

Showing 4 changed files with 51 additions and 7 deletions.
diff --git a/...ode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/TeleOp/Examples/FullControl.java b/...ode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/TeleOp/Examples/FullControl.java
@@ -2,13 +2,16 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
 import org.firstinspires.ftc.teamcode.OpModes.TeleOp.BaseTeleOp;
 import org.firstinspires.ftc.teamcode.Tools.FieldNavigation;
 import org.firstinspires.ftc.teamcode.Tools.Chassis.Chassi;
 import org.firstinspires.ftc.teamcode.Tools.Chassis.NormalChassi;
 import org.firstinspires.ftc.teamcode.Tools.Chassis.MecanumChassi;
 import org.firstinspires.ftc.teamcode.Tools.Robot;
 
+import java.util.Date;
+
 @TeleOp(name="FullControl", group="Examples")
 public class FullControl extends BaseTeleOp {
     private Robot robot;
@@ -18,8 +21,8 @@ public class FullControl extends BaseTeleOp {
     @Override
     public void initialize() {
         navi = new FieldNavigation();
-        //chassi = new NormalChassi();
         chassi = new MecanumChassi();
+        //chassi.setRotation(0.0f);
         chassi.populateMotorArray(hardwareMap);
 
         robot = new Robot(navi, chassi);
@@ -29,9 +32,9 @@ public void initialize() {
     public void run() {
         robot.setSpeed(-gamepad1.left_stick_y*0.5, -gamepad1.left_stick_x*0.5, -gamepad1.right_stick_x*0.5);
         robot.step();
+
         telemetry.addLine(chassi.debug());
         telemetry.addLine(navi.debug());
         telemetry.update();
-
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/Testing/GyroTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/Testing/GyroTest.java
@@ -4,10 +4,14 @@
 import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
+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.teamcode.OpModes.TeleOp.BaseTeleOp;
 @TeleOp
 public class GyroTest extends BaseTeleOp {
     private BNO055IMU imu;
+
     @Override
     public void initialize() {
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
@@ -24,7 +28,9 @@ public void initialize() {
 
     @Override
     public void run() {
-        telemetry.addData("AngularOrientation",imu.getAngularOrientation());
+        telemetry.addData("AngularOrientation (XYZ) in deg", imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES));
+        telemetry.addData("getAcceleration", imu.getAcceleration());
+        telemetry.addData("Position", imu.getPosition());
         telemetry.update();
     }
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tools/Chassis/Chassi.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tools/Chassis/Chassi.java
@@ -2,6 +2,7 @@
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
 import org.firstinspires.ftc.teamcode.Tools.DTypes.Velocity;
 import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;
 //TODO order
@@ -14,5 +15,6 @@ public interface Chassi {
 	Position2D getDrivenDistance();
 	void stopMotors();
 	String debug();
-	double getRotation();
+	float getRotation();
+	void setRotation(float rotation);
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tools/Chassis/ChassiBase.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tools/Chassis/ChassiBase.java
@@ -1,8 +1,13 @@
 package org.firstinspires.ftc.teamcode.Tools.Chassis;
 
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
+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.teamcode.Tools.DTypes.Velocity;
 import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;
 
@@ -18,18 +23,33 @@
 
 //TODO add l_x, l_y
 public abstract class ChassiBase implements Chassi {
+    protected BNO055IMU imu;
+    protected BNO055IMU.Parameters imu_prameters;
+
     protected  Velocity velocity;
     protected Position2D drivenDistance;
     protected double[] wheelSpeeds;
     protected double[] wheelSpeedsFactors;
     private DcMotor[] wheelMotors;
     private int[] wheelMotorSteps;
     protected int[] deltaWheelMotorSteps;
+    private float start_rotation;
+    private float rotation;
 
     /**
      * create chassi
      */
     public ChassiBase(int numWheels) {
+        imu_prameters = new BNO055IMU.Parameters();
+        imu_prameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+        imu_prameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+        imu_prameters.calibrationDataFile = "BNO055IMUCalibration.jason";
+        imu_prameters.loggingEnabled = true;
+        imu_prameters.loggingTag = "IMU";
+        imu_prameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
+        start_rotation = 0;
+        rotation = 0;
+
         drivenDistance = new Position2D();
         wheelMotors = new DcMotor[numWheels];
         wheelSpeeds = new double[numWheels];
@@ -49,6 +69,10 @@ public void populateMotorArray(HardwareMap hw_map) {
             wheelMotorSteps[i] = wheelMotors[i].getCurrentPosition();
             deltaWheelMotorSteps[i] = wheelMotorSteps[i];
         }
+
+        imu = hw_map.get(BNO055IMU.class,"imu");
+        imu.initialize(imu_prameters);
+        start_rotation = imu.getAngularOrientation().firstAngle;
     }
 
     public void setFactor(int wheelIndex, double factor) {
@@ -101,17 +125,21 @@ private void updateMotorSteps() {
         }
     }
 
-    public double getRotation() {
+    public void setRotation(float rotation) {
+        // TODO calculate and set offset
+    }
 
+    public float getRotation() {
+        return rotation;
     }
 
     /**
      * debug info
      */
     public String debug() {
         String ret = String.format(
-                "--- Chassi Debug ---\nvelocity :: vx=%+1.2f vy=%+1.2f wz=%+1.2f\ndrivenDistance :: x=%+2.2f y=%+2.2f\n",
-                velocity.getVX(), velocity.getVY(), velocity.getWZ(), drivenDistance.getX(), drivenDistance.getY());
+                "--- Chassi Debug ---\nvelocity :: vx=%+1.2f vy=%+1.2f wz=%+1.2f\ndrivenDistance :: x=%+2.2f y=%+2.2f\nrotation :: %+3.2f\n",
+                velocity.getVX(), velocity.getVY(), velocity.getWZ(), drivenDistance.getX(), drivenDistance.getY(), getRotation());
 
         // add wheel debug
         for (int i=0; i<wheelMotors.length; i++) {
@@ -121,10 +149,15 @@ public String debug() {
         return ret;
     }
 
+    private void calculateRotation() {
+        rotation = imu.getAngularOrientation().firstAngle; // Todo use starting rotation
+    }
+
     /**
      * update
      */
     public void step() {
+        calculateRotation();
         setMotorSpeeds();
         updateMotorSteps();
     }