Merge pull request #8 from rh-robotics/nextGenTeleop

Lots of TeleOp Changes, Time-based Auton - From Comp

FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java

@@ -29,8 +29,8 @@
 
 package org.firstinspires.ftc.robotcontroller.external.samples;
 
-import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 import com.qualcomm.robotcore.hardware.DcMotor;
 import com.qualcomm.robotcore.util.ElapsedTime;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/GoodAuton.java

@@ -0,0 +1,82 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystems.HWC;
+
+/*
+ * This OpMode illustrates the concept of driving a path based on time.
+ * The code is structured as a LinearOpMode
+ *
+ * The code assumes that you do NOT have encoders on the wheels,
+ *   otherwise you would use: RobotAutoDriveByEncoder;
+ *
+ *   The desired path in this example is:
+ *   - Drive forward for 3 seconds
+ *   - Spin right for 1.3 seconds
+ *   - Drive Backward for 1 Second
+ *
+ *  The code is written in a simple form with no optimizations.
+ *  However, there are several ways that this type of sequence could be streamlined,
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+
+@Autonomous(name="Back Auton", group="Robot")
+
+public class GoodAuton extends LinearOpMode {
+
+    /* Declare OpMode members. */
+  HWC robot;
+
+
+    static final double     FORWARD_SPEED = 0.3;
+   // static final double     TURN_SPEED    = 0.5;
+
+    @Override
+    public void runOpMode() {
+
+        telemetry.addData("Status", "Initializing");
+
+        // Do all init stuff
+        // TODO: ADD INITS THAT YOU NEED
+        robot = new HWC(hardwareMap, telemetry);
+        ElapsedTime     runtime = new ElapsedTime();
+
+        // Tell the driver the robot is ready
+        telemetry.addData("Status", "Initialized");
+        // Wait for the game to start (driver presses START)
+        waitForStart();
+        sleep(5000);
+        // Step through each leg of the path, ensuring that the OpMode has not been stopped along the way.
+
+        // Step 1:  Drive forward for 3 seconds
+        robot.rightRear.setPower(FORWARD_SPEED);
+        robot.rightFront.setPower(FORWARD_SPEED);
+        robot.leftRear.setPower(-FORWARD_SPEED);
+        robot.leftFront.setPower(-FORWARD_SPEED);
+
+        runtime.reset();
+        while (opModeIsActive() && (runtime.seconds() < 2)) {
+            telemetry.addData("Path", "Leg 1: %4.1f S Elapsed", runtime.seconds());
+            telemetry.update();
+        }
+
+       runtime.reset();
+
+        // Step 4:  Stop
+        robot.rightRear.setPower(0);
+        robot.rightFront.setPower(0);
+        robot.leftRear.setPower(0);
+        robot.leftFront.setPower(0);
+
+        telemetry.addData("Path", "Complete");
+        telemetry.update();
+        sleep(1000);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/GoodAutonForward.java

@@ -0,0 +1,80 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.subsystems.HWC;
+
+/*
+ * This OpMode illustrates the concept of driving a path based on time.
+ * The code is structured as a LinearOpMode
+ *
+ * The code assumes that you do NOT have encoders on the wheels,
+ *   otherwise you would use: RobotAutoDriveByEncoder;
+ *
+ *   The desired path in this example is:
+ *   - Drive forward for 3 seconds
+ *   - Spin right for 1.3 seconds
+ *   - Drive Backward for 1 Second
+ *
+ *  The code is written in a simple form with no optimizations.
+ *  However, there are several ways that this type of sequence could be streamlined,
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+
+@Autonomous(name="Forward Auton", group="Robot")
+
+public class GoodAutonForward extends LinearOpMode {
+
+    /* Declare OpMode members. */
+  HWC robot;
+
+
+    static final double     FORWARD_SPEED = 0.3;
+   // static final double     TURN_SPEED    = 0.5;
+
+    @Override
+    public void runOpMode() {
+
+        telemetry.addData("Status", "Initializing");
+
+        // Do all init stuff
+        // TODO: ADD INITS THAT YOU NEED
+        robot = new HWC(hardwareMap, telemetry);
+        ElapsedTime     runtime = new ElapsedTime();
+
+        // Tell the driver the robot is ready
+        telemetry.addData("Status", "Initialized");
+        // Wait for the game to start (driver presses START)
+        waitForStart();
+        sleep(5000);
+        // Step through each leg of the path, ensuring that the OpMode has not been stopped along the way.
+
+        // Step 1:  Drive forward for 3 seconds
+        robot.rightRear.setPower(-FORWARD_SPEED);
+        robot.rightFront.setPower(-FORWARD_SPEED);
+        robot.leftRear.setPower(FORWARD_SPEED);
+        robot.leftFront.setPower(FORWARD_SPEED);
+
+        runtime.reset();
+        while (opModeIsActive() && (runtime.seconds() < 2)) {
+            telemetry.addData("Path", "Leg 1: %4.1f S Elapsed", runtime.seconds());
+            telemetry.update();
+        }
+
+       runtime.reset();
+
+        // Step 4:  Stop
+        robot.rightRear.setPower(0);
+        robot.rightFront.setPower(0);
+        robot.leftRear.setPower(0);
+        robot.leftFront.setPower(0);
+
+        telemetry.addData("Path", "Complete");
+        telemetry.update();
+        sleep(1000);
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/HWC.java

@@ -19,9 +19,13 @@
 public class HWC {
     // Declare empty variables for robot hardware
     public DcMotorEx leftFront, rightFront, leftRear, rightRear, rightSlide, leftSlide;
-    //TODO: CHANGE TO NORMAL SERVO
     public Servo claw, joint, arm;
 
+    // Position Variables
+
+
+
+
 
     // Other Variables
     // ------ Declare Gamepads ------ //
@@ -41,6 +45,20 @@ public class HWC {
      */
     public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
         this.telemetry = telemetry;
+        //TODO: FIND ACTUAL VALUES
+        double clawOpenPos = 0.5;
+        double clawClosePos = 0;
+
+        double armDefaultPos = 0;
+        double armPos1 = 0.25;
+        double armPos2 = 0.5;
+        double armPos3 = 0.75;
+        int lowBasketPos = 0;
+        int highBasketPos = 0;
+        int lowBarPos = 0;
+        int highBarPos = 0;
+        int climbOnePos = 0;
+        int climbTwoPos=0;
 
         // Declare Driving motors
         leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
@@ -67,9 +85,10 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
         leftRear.setDirection(DcMotorEx.Direction.FORWARD);
         rightRear.setDirection(DcMotorEx.Direction.FORWARD);
 
-        //Set direction of other motors
-       // leftSlide.setDirection(DcMotorEx.Direction.FORWARD);
-        //rightSlide.setDirection(DcMotorEx.Direction.FORWARD);
+        leftSlide.setDirection(DcMotorEx.Direction.FORWARD);
+        rightSlide.setDirection(DcMotorEx.Direction.REVERSE);
+
+
 
         // Set motors to break when power = 0
         // TODO: REMOVE IF THIS BEHAVIOUR IS NOT DESIRED ON NEW BOT
@@ -95,6 +114,11 @@ public HWC(@NonNull HardwareMap hardwareMap, Telemetry telemetry) {
     // TODO: ADD ANY HARDWARE RELATED FUNCTIONS BELOW
 
     public static void betterSleep(double secs){
-
+        ElapsedTime sleepTimer = new ElapsedTime();
+        sleepTimer.reset();
+        while (sleepTimer.seconds() < secs){
+
+        }
+
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/pid/MotorPIDTuning.java

@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.teamcode.subsystems.pid;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.arcrobotics.ftclib.controller.PIDController;
+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.DcMotorEx;
+
+import org.firstinspires.ftc.teamcode.subsystems.HWC;
+
+@Config
+@TeleOp(name = "Motor PID Tuning", group = "Testing")
+public class MotorPIDTuning extends OpMode {
+    // ------ Declare PID Variables ------ //
+    public static double p = 0, i = 0, d = 0, f = 0;
+    public static int target = 0;
+    private double motorPos = 0;
+    private double pid = 0;
+    private double ff = 0;
+
+    // ------ Declare Others ------ //
+    private PIDController controller;
+    private HWC robot;
+    private DcMotorEx slideLeft;
+    //private Motor motorSelection = Motor.SLIDE_L;
+    private DcMotorEx slideRight;
+    private final double TICKS_IN_DEGREES = 751.8 / 360;
+
+    @Override
+    public void init() {
+        // ------ Initialize Hardware ------ //
+        robot = new HWC(hardwareMap, telemetry);
+
+        slideLeft = robot.leftSlide;
+        slideRight = robot.rightSlide;
+
+        slideLeft.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        slideRight.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+        // ------ Initialize PID Controller ------ //
+        controller = new PIDController(p, i, d);
+
+        // ------ Make telemetry FTC Dashboard Compatible ------ //
+        telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
+
+        slideLeft.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        slideRight.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+    }
+
+    @Override
+    public void loop() {
+        // ------ Update Gamepads ------ //
+        robot.previousGamepad1.copy(robot.currentGamepad1);
+        robot.previousGamepad2.copy(robot.currentGamepad2);
+        robot.currentGamepad1.copy(gamepad1);
+        robot.currentGamepad2.copy(gamepad2);
+
+        // ------ Set PID ------ //
+        controller.setPID(p, i, d);
+
+        // ------ Get Motor Position ------ //
+        motorPos = (slideLeft.getCurrentPosition() + slideRight.getCurrentPosition()) / 2.0;
+
+        // ------ Calculate PID ------ //
+        pid = controller.calculate(motorPos, target);
+
+        // ------ Calculate Feed Forward ------ //
+        ff = Math.cos(Math.toRadians(target / TICKS_IN_DEGREES)) * f;
+
+        // ------ Set Motor Power ------ //
+        slideLeft.setPower(pid + ff);
+        slideRight.setPower(pid + ff);
+
+        // ------ Telemetry ------ //
+        telemetry.addData("Motor Position", motorPos);
+        telemetry.addData("Motor Target", target);
+        telemetry.addData("Motor Power", slideLeft.getPower());
+        telemetry.addData("P", p);
+        telemetry.addData("I", i);
+        telemetry.addData("D", d);
+        telemetry.addData("F", f);
+        telemetry.addData("Calculated PID", pid);
+        telemetry.addData("Calculated FF", ff);
+        telemetry.update();
+    }
+}