test auto drive to position

has known bugs => dont use

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/Autonomous/Examples/TestCoordinateDriving.java

@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.OpModes.Autonomous.Examples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+import org.firstinspires.ftc.teamcode.OpModes.Autonomous.BaseAutonomous;
+import org.firstinspires.ftc.teamcode.Tools.Chassis.Chassi;
+import org.firstinspires.ftc.teamcode.Tools.Chassis.MecanumChassi;
+import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;
+import org.firstinspires.ftc.teamcode.Tools.FieldNavigation;
+import org.firstinspires.ftc.teamcode.Tools.Robot;
+
+@Autonomous(name="Test Coordinates Driving", group="Examples")
+//@Disabled
+public class TestCoordinateDriving extends BaseAutonomous {
+    private Robot robot;
+    private Chassi chassi;
+    private FieldNavigation navi;
+
+    @Override
+    public void initialize() {
+        navi = new FieldNavigation();
+        chassi = new MecanumChassi();
+        chassi.setRotationAxis(1);
+        chassi.populateMotorArray(hardwareMap);
+
+        robot = new Robot(navi, chassi);
+    }
+
+    @Override
+    public void run() {
+        robot.drive(new Position2D(100.0, 0.0));
+
+        while (opModeIsActive()) {
+            robot.step();
+            telemetry.addLine(chassi.debug());
+            telemetry.addLine(navi.debug());
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/TeleOp/Examples/FullControl.java

@@ -1,5 +1,6 @@
 package org.firstinspires.ftc.teamcode.OpModes.TeleOp.Examples;
 
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
 import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
@@ -13,6 +14,7 @@
 import java.util.Date;
 
 @TeleOp(name="FullControl", group="Examples")
+//@Disabled
 public class FullControl extends BaseTeleOp {
     private Robot robot;
     private FieldNavigation navi;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tools/DTypes/Position2D.java

@@ -115,12 +115,14 @@ public Position2D getNormalization() {
 
         // one dim
         else if (this.x == 0.0)
-            return new Position2D(0.0,1.0);
+            return new Position2D(0.0,this.y>0?1.0:-1.0);
         else if (this.y == 0.0)
-            return new Position2D(1.0,0.0);
+            return new Position2D(this.x>0?1.0:-1.0,0.0);
 
         // normalize
         double alpha = Math.atan(this.y / this.x);
-        return new Position2D(Math.cos(alpha),Math.sin(alpha));
+        boolean x = this.x > 0;
+        boolean y = this.y > 0;
+        return new Position2D((x?1:.1)*Math.cos(alpha),(y?1:-1)*Math.sin(alpha));
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tools/FieldNavigation.java

@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.Tools;
 
+import android.annotation.SuppressLint;
+
 import org.firstinspires.ftc.teamcode.Tools.DTypes.Velocity;
 import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;
 
@@ -13,6 +15,8 @@ public class FieldNavigation {
     private double driving_accuracy;
     private Velocity velocity;
 
+    public Position2D distance;
+
     /**
      * create new FieldNavigation object with given position and rotation
      * @param position position of the robot in CM
@@ -129,14 +133,25 @@ public void stop(){
 
 
     public String debug() {
-        return String.format("--- FieldNavigation Debug ---\nposition :: x=%+3.1f y=%+3.1f\n",position.getX(),position.getY());
+        String ret = "--- FieldNavigation Debug ---\n";
+        ret += String.format("driving :: %s\ntarget position :: x=%+3.1f y=%+3.1f\n",
+                (this.driving?"True":"False"), target_position.getX(), target_position.getY());
+        ret += String.format("position :: x=%+3.1f y=%+3.1f\n",
+                position.getX(), position.getY());
+        ret += String.format("velocity :: x=%+1.2f y=%+1.2f wz=%+1.2f\n",
+                velocity.getVX(), velocity.getVY(), velocity.getWZ());
+
+        ret += String.format("DIST :: %f %f\n", distance.getX(), distance.getY());
+
+        return ret;
     }
 
     /**
      * refresh
      */
     public void step() {
         Position2D distance;
+        double quad = 0;
 
         distance = target_position.copy();
         distance.subract(position);
@@ -146,8 +161,12 @@ public void step() {
                 stop();
 
             distance = distance.getNormalization();
-            velocity.setVX(distance.getX());
-            velocity.setVY(distance.getY());
+
+            distance.rotate(-this.rotation);
+            this.distance = distance;
+            velocity.set(distance.getX()*0.3, distance.getY()*0.3, 0.0);
+        } else {
+            velocity.set(0.0,0.0,0.0); // TODO remove this
         }
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Tools/Robot.java

@@ -3,6 +3,8 @@
 import org.firstinspires.ftc.teamcode.Tools.Chassis.Chassi;
 import org.firstinspires.ftc.teamcode.Tools.DTypes.Position2D;
 
+// TODO isb for driving etc
+
 public class Robot {
     public FieldNavigation navi;
     public Chassi chassi;