From 0a1ad23dd457cf58b71348fc73238d06555c3bea Mon Sep 17 00:00:00 2001
From: RobotIGS Laptop <robot.igs@igs-goettingen.de>
Date: Thu, 21 Sep 2023 17:05:19 +0200
Subject: [PATCH] fixed rotation

---
 .../teamcode/Tools/Chassis/ChassiBase.java    | 33 +++++++++++++------
 1 file changed, 23 insertions(+), 10 deletions(-)

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
index 2979083..03baff3 100644
--- 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
@@ -5,9 +5,7 @@
 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.robotcore.external.navigation.Orientation;
 import org.firstinspires.ftc.teamcode.Tools.DTypes.Velocity;
 import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;
 
@@ -33,8 +31,9 @@ public abstract class ChassiBase implements Chassi {
     private DcMotor[] wheelMotors;
     private int[] wheelMotorSteps;
     protected int[] deltaWheelMotorSteps;
-    private float start_rotation;
+    private float rotation_offset;
     private float rotation;
+    private int rotation_axis;
 
     /**
      * create chassi
@@ -47,8 +46,9 @@ public ChassiBase(int numWheels) {
         imu_prameters.loggingEnabled = true;
         imu_prameters.loggingTag = "IMU";
         imu_prameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
-        start_rotation = 0;
+        rotation_offset = 0;
         rotation = 0;
+        rotation_axis = 1;
 
         drivenDistance = new Position2D();
         wheelMotors = new DcMotor[numWheels];
@@ -72,7 +72,7 @@ public void populateMotorArray(HardwareMap hw_map) {
 
         imu = hw_map.get(BNO055IMU.class,"imu");
         imu.initialize(imu_prameters);
-        start_rotation = imu.getAngularOrientation().firstAngle;
+        setRotation(0.0f);
     }
 
     public void setFactor(int wheelIndex, double factor) {
@@ -126,7 +126,7 @@ private void updateMotorSteps() {
     }
 
     public void setRotation(float rotation) {
-        // TODO calculate and set offset
+        rotation_offset = -getRawRotation() + rotation;
     }
 
     public float getRotation() {
@@ -138,8 +138,8 @@ public float getRotation() {
      */
     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\nrotation :: %+3.2f\n",
-                velocity.getVX(), velocity.getVY(), velocity.getWZ(), drivenDistance.getX(), drivenDistance.getY(), getRotation());
+                "--- Chassi Debug ---\nvelocity :: vx=%+1.2f vy=%+1.2f wz=%+1.2f\ndrivenDistance :: x=%+2.2f y=%+2.2f\nrotation :: %+3.2f\nrawation :: %+3.2f + %+3.2f\n",
+                velocity.getVX(), velocity.getVY(), velocity.getWZ(), drivenDistance.getX(), drivenDistance.getY(), getRotation(), getRawRotation(), rotation_offset);
 
         // add wheel debug
         for (int i=0; i<wheelMotors.length; i++) {
@@ -149,8 +149,21 @@ public String debug() {
         return ret;
     }
 
+    private float getRawRotation() {
+        switch(rotation_axis) {
+            case 1: return imu.getAngularOrientation().firstAngle;
+            case 2: return imu.getAngularOrientation().secondAngle;
+            case 3: return imu.getAngularOrientation().thirdAngle;
+            default: return 0.0f;
+        }
+    }
+
     private void calculateRotation() {
-        rotation = imu.getAngularOrientation().firstAngle; // Todo use starting rotation
+        rotation = getRawRotation();
+        rotation += 180; // make positive
+        rotation += rotation_offset; // add offset
+        rotation %= 360;
+        rotation -= 180;
     }
 
     /**