Josh: GyroLock and DriveTrain

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java

@@ -74,7 +74,7 @@ public void init(HardwareMap ahwMap){
         servos[2] = ahwMap.get(Servo.class, "intake_right_bottom");
         servos[3] = ahwMap.get(Servo.class, "intake_left_bottom");
 
-        lift=new GlyphIntake2(servos);
+        lift=new GlyphIntake2(ahwMap);
         sensorColor = hwMap.get(ColorSensor.class, "colorSensor");
         limitSwitch = hwMap.get(DigitalChannel.class, "limit_switch_1");
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java

@@ -1,10 +1,10 @@
 package org.firstinspires.ftc.teamcode;
 
+import com.qualcomm.hardware.bosch.BNO055IMU;
 import com.qualcomm.robotcore.eventloop.opmode.OpMode;
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.hardware.DigitalChannel;
-import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.teamcode.Components.DriveTrain;
 import org.firstinspires.ftc.teamcode.Components.GlyphIntake2;
@@ -15,54 +15,56 @@
  * */
 @TeleOp(name="Teleop: PLEASE DON'T DELETE THIS WILL")
 public class CompTeleop extends OpMode {
-    public DriveTrain driveTrain;
-    DcMotor frontRight, backRight, frontLeft, backLeft;
-    Servo[] servos;
-
+    DriveTrain driveTrain;
     Gyro gyro;
 
-
     GlyphIntake2 intake;
     DcMotor liftA, liftB;
     DigitalChannel limitSwitch;
 
     @Override
     public void init() {
-        gyro = new Gyro(hardwareMap);
-        this.driveTrain = new DriveTrain(hardwareMap, gyro);
+        BNO055IMU imu = hardwareMap.get(BNO055IMU.class, Name.SENSOR_GYRO);
+        this.gyro = new Gyro(imu);
 
-        liftA = hardwareMap.get(DcMotor.class,"lift_left");
-        liftB = hardwareMap.get(DcMotor.class,"lift_right");
+        this.driveTrain = new DriveTrain(hardwareMap);
 
-        servos = new Servo[4];
-        servos[0] = hardwareMap.get(Servo.class, "intake_right_top");
-        servos[1] = hardwareMap.get(Servo.class, "intake_left_top");
-        servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom");
-        servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom");
+        liftA = hardwareMap.get(DcMotor.class, Name.LIFT_L);
+        liftB = hardwareMap.get(DcMotor.class, Name.LIFT_R);
 
-        intake = new GlyphIntake2(servos);
-        gyro = new Gyro(hardwareMap);
+        intake = new GlyphIntake2(hardwareMap);
         limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1");
     }
 
     @Override
     public void loop() {
-        driveTrain.driveMechanum(gamepad1);
+        gyro.update();
+        driveTrain.halfPower(gamepad1.left_bumper);
+        driveTrain.driveMechanum(gamepad1.left_stick_x, gamepad1.left_stick_y, gamepad1.right_stick_x);
 
-        if (gamepad2.a)
+        if (gamepad2.a) {
             intake.openBottom();
-        else
+        } else {
             intake.closeBottom();
+        }
 
-        if (gamepad2.b)
+        if (gamepad2.b) {
             intake.openTop();
-        else
+        } else {
             intake.closeTop();
+        }
 
         liftA.setPower(gamepad2.left_stick_y);
         liftB.setPower(-gamepad2.left_stick_y);
 
-        telemetry.addData("Orientation", gyro.x + "°");
+        telemetry.addData("Gyroscope", gyro);
         telemetry.addData("Limit Switch", limitSwitch);
+        telemetry.update();
+    }
+
+
+    @Override
+    public void stop() {
+        driveTrain.stop();
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java

@@ -1,30 +1,38 @@
 package org.firstinspires.ftc.teamcode.Components;
 
 import com.qualcomm.robotcore.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.Gamepad;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
 import org.firstinspires.ftc.teamcode.GyroLock;
+import org.firstinspires.ftc.teamcode.Name;
 import org.firstinspires.ftc.teamcode.PID;
-import org.firstinspires.ftc.teamcode.Gyro;
 
 /**
- * Created by Joshua Rapoport on 11/20/17.
+ * Created by Montclair Robotics on 11/27/17
+ * @author Joshua Rapoport
+ * @version 12/2/17.
  */
 
 public class DriveTrain {
-    public static final double TURN_ERROR = 0.1;
+    public static final double TURN_ERROR = 0.05;
 
     GyroLock lock;
+//    GyroBalance balance;
+
+    private double power;
     DcMotor frontLeft, frontRight, backLeft, backRight;
 
-    public DriveTrain(HardwareMap map, Gyro gyro) {
-        lock = new GyroLock(new PID(0.01, 0, 0.005), gyro);
+    public DriveTrain(HardwareMap map) {
+        this.lock = new GyroLock(new PID(0.04, 0, 0.02)); // TODO: Calibrate GyroLock
+
+//        this.balance = new GyroBalance(new PID(0, 0, 0), new PID(0, 0, 0)); // TODO: Calibrate GyroBalance
 
-        frontRight = map.get(DcMotor.class, "right_front");
-        backRight = map.get(DcMotor.class, "right_back");
-        backLeft = map.get(DcMotor.class, "left_back");
-        frontLeft = map.get(DcMotor.class, "left_front");
+        this.power = 0;
+
+        frontRight = map.get(DcMotor.class, Name.DRIVETRAIN_FR);
+        backRight = map.get(DcMotor.class, Name.DRIVETRAIN_BR);
+        backLeft = map.get(DcMotor.class, Name.DRIVETRAIN_BL);
+        frontLeft = map.get(DcMotor.class, Name.DRIVETRAIN_FL);
 
         frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
         backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT);
@@ -33,31 +41,38 @@ public DriveTrain(HardwareMap map, Gyro gyro) {
     }
 
     public void stop() {
-        frontLeft.setPower(0);
-        frontRight.setPower(0);
-        backLeft.setPower(0);
-        backRight.setPower(0);
+        driveMechanum(0, 0, 0);
     }
 
-    public void driveMechanum(Gamepad g) {
-        double pow = 1.0;
+    public void halfPower(boolean h) {
+        power = 0.8;
 
-        if (g.left_bumper) {
-            pow = 0.5;
+        if (h) {
+            power = 0.4;
         }
+    }
 
-        double x = g.left_stick_x * pow;
-        double y = -g.left_stick_y * pow;
-        double turn = g.right_stick_x * pow;
+    /**
+     * A single loop.
+     * @param x going along x-axis, from -1 to 1.
+     * @param y going along x-axis, from -1 to 1.
+     * @param turn rotating clockwise, from -1 to 1.
+     */
+    public void driveMechanum(double x, double y, double turn) {
+        if (Math.abs(turn) < TURN_ERROR) {
+            if (lock.init()) {
+                lock.setTarget();
+            }
 
-        if (turn < TURN_ERROR) {
-            lock.teleopLoop(g);
+            lock.setInput();
             turn = lock.correction();
+        } else {
+            lock.stop();
         }
 
-        frontRight.setPower(x - y + turn);
-        backRight.setPower(-x - y + turn);
-        backLeft.setPower(-x + y + turn);
-        frontLeft.setPower(x + y + turn);
+        frontRight.setPower(power * (x + y + turn));
+        backRight.setPower(power * (-x + y + turn));
+        backLeft.setPower(power * (-x - y + turn));
+        frontLeft.setPower(power * (x - y + turn));
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java

@@ -1,12 +1,14 @@
 package org.firstinspires.ftc.teamcode.Components;
 
+import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
+import org.firstinspires.ftc.teamcode.Name;
 import org.firstinspires.ftc.teamcode.Robot;
 
 /**
  * Created by Montclair Robotics on 11/13/17.
- * @Author:Rafi
+ * @author Rafi
  * */
 
 public class GlyphIntake2 {
@@ -19,8 +21,14 @@ public class GlyphIntake2 {
 
     private Servo[] servos;
 
-    public GlyphIntake2(Servo... servos) {
-        this.servos = servos;
+    public GlyphIntake2(HardwareMap map) {
+        this.servos = new Servo[] {
+                map.get(Servo.class, Name.INTAKE_TR),
+                map.get(Servo.class, Name.INTAKE_TL),
+                map.get(Servo.class, Name.INTAKE_BR),
+                map.get(Servo.class, Name.INTAKE_BL)
+        };
+
         servoOpen=new double[4];
         servoClose=new double[4];
 

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphLift.java

@@ -2,15 +2,12 @@
 
 import com.qualcomm.robotcore.hardware.DcMotor;
 
-import org.montclairrobotics.sprocket.core.IMotor;
 import org.montclairrobotics.sprocket.ftc.FTCMotor;
 import org.montclairrobotics.sprocket.motors.Module;
-import org.montclairrobotics.sprocket.motors.SEncoder;
-import org.montclairrobotics.sprocket.utils.PID;
 
 /**
  * Created by Montclair Robotics on 11/13/17.
- * @Author:Garrett
+ * @author Garrett
  * */
 
 public class GlyphLift extends Module{

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java

@@ -1,28 +1,21 @@
 package org.firstinspires.ftc.teamcode;
 
 import com.qualcomm.hardware.bosch.BNO055IMU;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-
-import org.montclairrobotics.sprocket.loop.Priority;
-import org.montclairrobotics.sprocket.loop.Updatable;
-import org.montclairrobotics.sprocket.loop.Updater;
-import org.montclairrobotics.sprocket.utils.Input;
 
 /**
  * Created by Montclair Robotics on 11/13/17.
- * @Author:Jack
+ * @author Jack
  * */
 
-public class Gyro implements Input<Double>, Updatable {
-    double x, y, z;
+public class Gyro {
+    public static Gyro current;
 
     private RRQuaternion quat; // An angle object to store the gyro angles
     private BNO055IMU imu; // Gyroscope
 
-    public Gyro(HardwareMap map) {
-        x = y = z = 0;
-
-        imu = map.get(BNO055IMU.class, "gyro");
+    public Gyro(BNO055IMU imu) {
+        this.imu = imu;
+        this.quat = new RRQuaternion();
 
         BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); // Create a new parameter object for the gyro
         parameters.angleUnit            = BNO055IMU.AngleUnit.DEGREES; // set the angle unit parameter to
@@ -31,22 +24,25 @@ public Gyro(HardwareMap map) {
         parameters.loggingTag           = "IMU"; // set the logging tag
 
         imu.initialize(parameters);
-        imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, 6);
 
-        update();
-        Updater.add(this, Priority.INPUT);
+        current = this;
     }
 
-    @Override
     public void update() {
         quat = new RRQuaternion(imu.getQuaternionOrientation());
-        x = (double) ((int) (10*quat.getX())) / 10;
-        y = (double) ((int) (10*quat.getY())) / 10;
-        z = (double) ((int) (10*quat.getZ())) / 10;
     }
 
-    @Override
-    public Double get() {
-        return quat.getX();
+    public Double getX() {
+        return -quat.getX();
+    }
+    public Double getY() {
+        return quat.getY();
+    }
+    public Double getZ() {
+        return quat.getZ();
+    }
+
+    public String toString() {
+        return "{x: " + getX().intValue() + "°,\ty: " + getY().intValue() + "°,\tz: " + getZ().intValue() + "°}";
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroBalance.java

@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode;
+
+/**
+ * @author Joshua Rapoport
+ * @version 11/27/17.
+ */
+
+public class GyroBalance {
+    private Gyro gyro;
+    private PID xPID, yPID;
+
+    public GyroBalance(PID x, PID y) {
+        this.gyro = Gyro.current;
+        this.xPID = x;
+        this.yPID = y;
+
+        xPID.setTarget(0.0);
+        yPID.setTarget(0.0);
+
+        xPID.setInputRange(-45, 45);
+        yPID.setInputRange(-45, 45);
+
+        xPID.setOutputRange(-1.0, 1.0);
+        yPID.setOutputRange(-1.0, 1.0);
+    }
+
+    public double correctionX() { return xPID.getOutput(); }
+    public double correctionY() { return yPID.getOutput(); }
+
+    public void update() {
+        xPID.setInput(gyro.getY()); // TODO: double check please
+        yPID.setInput(gyro.getZ()); // TODO: double check please
+    }
+}