diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/TeleOp/Examples/FullControl.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/TeleOp/Examples/FullControl.java index 7b40a9b..2d0e46d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/TeleOp/Examples/FullControl.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OpModes/TeleOp/Examples/FullControl.java @@ -2,6 +2,7 @@ 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; @@ -9,6 +10,8 @@ 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 index 3a11df7..7c6108c 100644 --- 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(); } } \ No newline at end of file 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 index 09c2cd6..18576b1 100644 --- 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 index 19166a1..2979083 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 @@ -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,6 +23,9 @@ //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; @@ -25,11 +33,23 @@ public abstract class ChassiBase implements Chassi { 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,8 +125,12 @@ private void updateMotorSteps() { } } - public double getRotation() { + public void setRotation(float rotation) { + // TODO calculate and set offset + } + public float getRotation() { + return rotation; } /** @@ -110,8 +138,8 @@ public double 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\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