Adding Teleop code and new positions

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java

@@ -0,0 +1,75 @@
+package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
+import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestMotor;
+import org.firstinspires.ftc.teamcode.ftc16072.Util.PIDFController;
+
+import java.util.Arrays;
+import java.util.List;
+@Config
+public class Arm extends QQMechanism{
+    public static final double TEST_SPEED = 0.35;
+    DcMotor armMotor;
+    public static double kP = 0.01;
+    public static double kI = 0.0;
+    public static double kD = 0.0;
+    public static double kF = 0;
+    public static double max =  0.4;
+    public static double min = -max;
+
+    public int currentPos;
+    public int desiredPos;
+    public double motorPower;
+    public static int PLACEMENT_POSITION = 920;
+    public static int INTAKE_POSITION = 10;
+
+    public Telemetry telemetry;
+
+
+    PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min);
+
+    @Override
+    public void init(HardwareMap hwMap) {
+        armMotor = hwMap.get(DcMotor.class, "arm_motor");
+        armMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+        armMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+        armMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+        armMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+    }
+
+    public void manualPositionChange(int changeAmount){
+        desiredPos += changeAmount;
+    }
+
+    public void goToIntake(){
+        desiredPos = INTAKE_POSITION;
+    }
+    public void goToPlacement(){
+        desiredPos = PLACEMENT_POSITION;
+    }
+
+    @Override
+    public void update(){
+        currentPos = (armMotor.getCurrentPosition());
+        double motorPower = pidfController.calculate(desiredPos,currentPos);
+        telemetry.addData("Current", currentPos);
+        telemetry.addData("Desired", desiredPos);
+        telemetry.addData("Motor Power", motorPower);
+        this.motorPower = motorPower;
+        armMotor.setPower(motorPower);
+    }
+
+    @Override
+    public List<QQTest> getTests() {
+        return Arrays.asList(
+                new TestMotor("arm up", TEST_SPEED, armMotor),
+                new TestMotor("arm down", -TEST_SPEED, armMotor)
+        );
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Claw.java

@@ -1,18 +1,18 @@
 package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms;
 
+import com.acmerobotics.dashboard.config.Config;
 import com.qualcomm.robotcore.hardware.HardwareMap;
 import com.qualcomm.robotcore.hardware.Servo;
 
 import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest;
 import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestServo;
 
-import java.util.Arrays;
 import java.util.Collections;
 import java.util.List;
-
+@Config
 public class Claw extends QQMechanism {
-    public static double CLAW_OPEN_POSITION = 1;
-    public static double CLAW_CLOSE_POSITION = 0.7;
+    public static double CLAW_OPEN_POSITION = 0.3;
+    public static double CLAW_CLOSE_POSITION = 0;
 
 
     Servo clawServo;
@@ -24,7 +24,7 @@ public void init(HardwareMap hwMap) {
 
     @Override
     public List<QQTest> getTests() {
-        return Arrays.asList(
+        return Collections.singletonList(
                 new TestServo("claw_movement", CLAW_OPEN_POSITION, CLAW_CLOSE_POSITION, clawServo)
         );
     }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/MecanumDrive.java

@@ -47,7 +47,7 @@ public void setSpeed(Speed speed) {
                 speedMultiplier= 0.5;
                 break;
             case SLOW :
-                speedMultiplier= 0.25;
+                speedMultiplier= 0.1;
                 break;
             case FAST:
                 speedMultiplier= 0.75;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RI3WTeleOp.java

@@ -2,8 +2,6 @@
 
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
 
-import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
-
 @TeleOp
 @SuppressWarnings("unused")
 public class RI3WTeleOp extends QQOpMode{
@@ -14,7 +12,7 @@ public class RI3WTeleOp extends QQOpMode{
 
     @Override
     public void loop() {
-        super.loop();
+       /* super.loop();
         double forward = -gamepad1.left_stick_y;
         double left = gamepad1.left_stick_x;
         double rotate = gamepad1.right_stick_x;
@@ -56,6 +54,6 @@ else if (gamepad1.right_bumper){
             robot.controlHub.resetGyro();
         }
 
-
+*/
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java

@@ -0,0 +1,46 @@
+package org.firstinspires.ftc.teamcode.ftc16072.OpModes;
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
+@TeleOp
+public class ScrimmageTeleop extends QQOpMode{
+
+    public static final double TRIGGER_THRESHOLD = 0.5;
+    public static final int MANUAL_CHANGE = 5;
+
+    public void init(){
+        super.init();
+        robot.arm.telemetry = telemetry;
+    }
+    public void loop(){
+        super.loop();
+        double forward = -gamepad1.left_stick_y;
+        double left = gamepad1.left_stick_x;
+        double rotate = gamepad1.right_stick_x;
+
+        nav.driveFieldRelative(forward, left, rotate);
+
+        if (gamepad1.left_trigger > TRIGGER_THRESHOLD ){
+            robot.mecanumDrive.setSpeed(MecanumDrive.Speed.TURBO);
+        } else if (gamepad1.left_bumper) {
+            robot.mecanumDrive.setSpeed(MecanumDrive.Speed.SLOW);
+        } else robot.mecanumDrive.setSpeed(MecanumDrive.Speed.NORMAL);
+
+        if (gamepad1.a){
+        robot.arm.goToIntake();
+        }else if (gamepad1.b) {
+        robot.arm.goToPlacement();}
+        else if (gamepad1.dpad_up){
+            robot.arm.manualPositionChange(MANUAL_CHANGE);
+        }else if (gamepad1.dpad_down){
+            robot.arm.manualPositionChange(-MANUAL_CHANGE);
+        }else if (gamepad1.y){
+            robot.controlHub.resetGyro();
+        }
+        if (gamepad1.right_bumper) {
+            robot.claw.close();
+        }else if (gamepad1.right_trigger > TRIGGER_THRESHOLD){
+        robot.claw.open();}
+    }
+}

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

@@ -2,13 +2,12 @@
 
 import com.qualcomm.robotcore.hardware.HardwareMap;
 
+import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Arm;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Claw;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.ControlHub;
-import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.DoubleReverse4Bar;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.MecanumDrive;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.OpticalTrackingOdometrySensor;
 import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.QQMechanism;
-import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Slides;
 
 import java.util.Arrays;
 import java.util.List;
@@ -19,26 +18,29 @@ public class Robot {
     public MecanumDrive mecanumDrive;
     public OpticalTrackingOdometrySensor otos;
     public Claw claw;
-    public DoubleReverse4Bar doubleReverse4Bar;
-    public Slides slides;
+    //public DoubleReverse4Bar doubleReverse4Bar;
+   // public Slides slides;
+    public Arm arm;
     List<QQMechanism> mechanisms;
 
     public Robot() {
         mecanumDrive = new MecanumDrive();
         controlHub = new ControlHub();
         otos = new OpticalTrackingOdometrySensor();
         claw = new Claw();
-        doubleReverse4Bar = new DoubleReverse4Bar();
-        slides = new Slides();
+        //doubleReverse4Bar = new DoubleReverse4Bar();
+        //slides = new Slides();
+        arm = new Arm();
 
         mechanisms = Arrays.asList(
                 controlHub,
                 mecanumDrive,
                 otos,
                 claw,
-                slides,
+               // slides,
                 claw,
-                doubleReverse4Bar);
+                //doubleReverse4Bar,
+                arm);
     }
     public void init(HardwareMap hwMap) {
         for (QQMechanism mechanism : mechanisms) {

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/PIDFController.java

@@ -12,6 +12,7 @@ public class PIDFController {
 
     double sumErrors;
     double lastError;
+    double lastDesired;
     ElapsedTime timer = new ElapsedTime();
 
     public PIDFController(double kP,double kI,double kD,double kF, double max, double min){
@@ -28,6 +29,10 @@ public void reset(){
         timer.reset();
     }
     public double calculate(double desired, double current){
+        if (lastDesired != desired){
+            reset();
+        }
+
         double error = desired - current;
         double derivative = (error-lastError)/timer.seconds();
         sumErrors += error * timer.seconds();
@@ -36,6 +41,8 @@ public double calculate(double desired, double current){
 
         timer.reset();
         lastError = error;
+        lastDesired = desired;
+
 
         result = Math.min(max,result);
         result = Math.max(min,result);

TeamCode/src/main/res/xml/ri6w.xml

@@ -0,0 +1,19 @@
+<?xml version='1.0' encoding='UTF-8' standalone='yes' ?>
+<Robot type="FirstInspires-FTC">
+    <LynxUsbDevice name="Control Hub Portal" serialNumber="(embedded)" parentModuleAddress="173">
+        <LynxModule name="Expansion Hub 3" port="3">
+            <goBILDA5202SeriesMotor name="arm_motor" port="0" />
+            <Servo name="claw_servo" port="2" />
+        </LynxModule>
+        <LynxModule name="Control Hub" port="173">
+            <goBILDA5202SeriesMotor name="back_left_motor" port="0" />
+            <goBILDA5202SeriesMotor name="front_left_motor" port="1" />
+            <goBILDA5202SeriesMotor name="back_right_motor" port="2" />
+            <goBILDA5202SeriesMotor name="front_right_motor" port="3" />
+            <Servo name="" port="0" />
+            <LynxEmbeddedIMU name="imu" port="0" bus="0" />
+            <SparkFunOTOS name="sensor_otos" port="0" bus="3" />
+        </LynxModule>
+    </LynxUsbDevice>
+    <Webcam name="Webcam" serialNumber="9041D230" />
+</Robot>