package com.qualcomm.ftcrobotcontroller.opmodes.custom.util;

import com.qualcomm.robotcore.hardware.DcMotorController;
import com.qualcomm.robotcore.hardware.GyroSensor;
import com.rmrobotics.library.core.RMOpMode;
import com.rmrobotics.library.hardware.Motor;

/**
 * Created by Simon on 3/11/2016.
 */
public class GyroSelfCorrect5421 extends RMOpMode {

    GyroSensor gyro;
    Motor mL;
    Motor sL;
    Motor mR;
    Motor sR;

    @Override
    public void init() {
        super.setTeam(5421);
        super.init();
        gyro = hardwareMap.gyroSensor.get("gyro");
        gyro.calibrate();
        mL = motorMap.get("mL");
        mL.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
        sL = motorMap.get("sL");
        mR = motorMap.get("mR");
        mR.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS);
        sR = motorMap.get("sR");
    }

    public void init_loop() {
        telemetry.addData("CALIBRATION DONE:", !gyro.isCalibrating());
    }

    @Override
    protected void calculate() {
        if (gyro.getHeading() > 2) {
            drive(0.1, 0.3);
        } else if (gyro.getHeading() < 358) {
            drive(0.3, 0.1);
        } else {
            drive(0.2);
        }
        updateSlave();
        addTelemetry();
    }

    private void drive(double p) {
        mL.setDesiredPower(-p);
        mR.setDesiredPower(-p);
    }

    private void drive(double p1, double p2) {
        mL.setDesiredPower(-p1);
        mR.setDesiredPower(-p2);
    }

    private void updateSlave() {
        sL.setDesiredPower(mL.getDesiredPower());
        sR.setDesiredPower(mR.getDesiredPower());
    }

    private void addTelemetry() {
        telemetry.addData("h", String.format("%03d", gyro.getHeading()));
        telemetry.addData("mL", mL.getPower());
        telemetry.addData("sL", sL.getPower());
        telemetry.addData("mR", mR.getPower());
        telemetry.addData("sR", sR.getPower());
    }
}

@@ -55,16 +55,119 @@
 .util;%0A%0A
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;%0Aimport com.qualcomm.robotcore.hardware.DcMotor;%0A
 import c
@@ -359,16 +359,75 @@
 Motor;%0A%0A
+import java.util.concurrent.locks.ReentrantReadWriteLock;%0A%0A
 /**%0A * C
@@ -501,18 +501,16 @@
 extends 
-RM
 OpMode %7B
@@ -532,24 +532,26 @@
 r gyro;%0A    
+Dc
 Motor mL;%0A  
@@ -556,22 +556,10 @@
     
-Motor sL;%0A    
+Dc
 Moto
@@ -572,16 +572,19 @@
     
-Motor sR
+String skew
 ;%0A%0A 
@@ -625,59 +625,8 @@
 ) %7B%0A
-        super.setTeam(5421);%0A        super.init();%0A
     
@@ -707,32 +707,43 @@
        mL = 
-m
+hardwareMap.dcM
 otor
-Map
 .get(%22mL%22);%0A
@@ -783,37 +783,39 @@
 ler.RunMode.RUN_
-USING
+WITHOUT
 _ENCODERS);%0A    
@@ -822,54 +822,84 @@
     
-sL = motorMap.get(%22sL%22);%0A        mR = motorMap
+mL.setDirection(DcMotor.Direction.REVERSE);%0A        mR = hardwareMap.dcMotor
 .get
@@ -959,13 +959,15 @@
 RUN_
-USING
+WITHOUT
 _ENC
@@ -978,41 +978,8 @@
 S);%0A
-        sR = motorMap.get(%22sR%22);%0A
     
@@ -1112,31 +1112,23 @@
    p
-rotected void calculate
+ublic void loop
 () %7B
@@ -1161,16 +1161,43 @@
 ng() %3E 2
+ && gyro.getHeading() %3C 180
 ) %7B%0A    
@@ -1216,17 +1216,45 @@
 e(0.
-1
+2
 , 0.
-3
+5
 );%0A
+            skew = %22RIGHT%22;%0A
     
@@ -1291,16 +1291,43 @@
 () %3C 358
+ && gyro.getHeading() %3E 180
 ) %7B%0A    
@@ -1346,17 +1346,44 @@
 e(0.
-3
+5
 , 0.
-1
+2
 );%0A
+            skew = %22LEFT%22;%0A
     
@@ -1415,17 +1415,17 @@
 drive(0.
-2
+5
 );%0A     
@@ -1431,32 +1431,40 @@
     
-%7D%0A        updateSlave();
+    skew = %22STRAIGHT%22;%0A        %7D
 %0A   
@@ -1536,38 +1536,30 @@
       mL.set
-Desired
 Power(
--
 p);%0A        
@@ -1564,30 +1564,22 @@
   mR.set
-Desired
 Power(
--
 p);%0A    
@@ -1642,30 +1642,22 @@
   mL.set
-Desired
 Power(
--
 p1);%0A   
@@ -1671,22 +1671,14 @@
 .set
-Desired
 Power(
--
 p2);
@@ -1706,19 +1706,20 @@
 oid 
-updateSlave
+addTelemetry
 () %7B
@@ -1731,140 +1731,40 @@
     
-sL.setDesiredPower(mL.getDesiredPower());%0A        sR.setDesiredPower(mR.getDesiredPower());%0A    %7D%0A%0A    private void addTelemetry() %7B
+telemetry.addData(%22SKEW%22, skew);
 %0A   
@@ -1913,110 +1913,14 @@
 ta(%22
-sL%22, sL.getPower());%0A        telemetry.addData(%22mR%22, mR.getPower());%0A        telemetry.addData(%22sR%22, s
+mR%22, m
 R.ge

Update GyroSelfCorrection to work with 1:1 gear ratio on drive train instead of 3:1