manual control complete, and states started again

HARDWARE.md

@@ -18,10 +18,10 @@ Below are the following configurations for our robots
 | motor3        | control   | UltraPlanetary HD hex motor | left back leg frame           | Drive back lt              |
 | I2C B0        | control   | Color sensor V3            | Left outside leg frame        | color left                 |
  | I2C B1        | control   | Color sensor V3            | Right outside leg frame       | color right                |
-| digital01     | control   | Digital device             | arm frame back right          | axle encoder               |
  | I2C B0        | expansion | 2m distance sensor         | Middle Back outside leg frame | distance                   |
 | motor0        | expansion | UltraPlanetary HD hex motor | left back arm frame           | arm raise                  |
 | motor1        | expansion | Core Hex Motor             | right back arm frame          | hang                       |
+| motor3        | expansion | Digital device             | arm frame back right          | axle encoder               |
 | Servo 0       | expansion | Servo                      | on arm                        | wrist                      |
 | Servo 1       | expansion | Servo                      | on arm                        | gripper                    |
 

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

@@ -0,0 +1,186 @@
+package org.firstinspires.ftc.teamcode;
+
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+
+
+@TeleOp( name = "ArmState")
+public class ArmStates extends OpMode {
+
+    DcMotor arm;
+    DcMotor armThroughBoreEncoder;
+    static final double TICKS_TO_DEGREES = 0.07462686567;
+    static final double COUNTS_PER_MOTOR_REV = 537.6;
+    static final double DRIVE_GEAR_REDUCTION = 1.0;
+    static final double COUNTS_PER_ARM_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / (2.7 * Math.PI);
+
+
+    /**
+     * this function takes a long milliseconds parameter and sleeps
+     * @param millis milliseconds to sleep
+     */
+    public void sleepmillis(long millis) {
+        try {
+            Thread.sleep(millis);
+        } catch (Exception e) {
+        }
+    }
+
+    /**
+     * stops all drive motors
+     */
+    public void off() {
+        arm.setPower(0);
+
+    }
+    /**
+     * User defined init method
+     * This method will be called once when the INIT button is pressed.
+     */
+
+    public void init() {
+
+
+        telemetry.addData("Status","In Init()");
+        telemetry.update();
+        arm = hardwareMap.dcMotor.get("arm raise");
+        armThroughBoreEncoder = hardwareMap.dcMotor.get("armThroughBoreEncoder");
+
+    }
+
+    /**
+     * User defined init_loop method
+     * This method will be called repeatedly when the INIT button is pressed.
+     * This method is optional. By default this method takes no action.
+     */
+    public void init_loop(){
+        // Wait for the game to start (driver presses PLAY)
+        telemetry.addData("Status", "Initialized");
+        telemetry.update();
+    }
+    /**
+     * User defined start method.
+     * This method will be called once when the PLAY button is first pressed.
+     * This method is optional. By default this method takes not action. Example usage: Starting another thread.
+     */
+    public void start() {
+
+    }
+
+    /**
+     * User defined stop method
+     * This method will be called when this op mode is first disabled.
+     * The stop method is optional. By default this method takes no action.
+     */
+    public void stop(){
+
+    }
+
+
+    /**
+     * User defined loop method.
+     * This method will be called repeatedly in a loop while this op mode is running
+     */
+    public void loop() {
+if (gamepad1.right_bumper)
+{
+    raisearm(200);
+}
+
+
+//        int armState = 0;
+//        int armPos = 0;
+//        if(gamepad2.x)
+//        {
+//            armState = 1;
+//          raisearm(0 - armPos);
+//          armPos = 0;
+//        }
+//        if(gamepad2.y)
+//        {
+//             armState = 2;
+//            raisearm(1000);
+//            armPos = 30;
+//        }
+//        if(gamepad2.b)
+//        {
+//             armState = 2;
+//            raisearm(200 - armPos);
+//            armPos = 200;
+//        }
+//        if(gamepad2.right_bumper)
+//        {
+//            armState =+ 1;
+//        }
+//        if(gamepad2.left_bumper)
+//        {
+//            armState =- 1;
+//        }
+//        if(armState == 1)
+//        {
+//            raisearm(0 - armPos);
+//            armPos = 0;
+//        }
+//        if(armState == 2)
+//        {
+//            raisearm(30 - armPos);
+//            armPos = 30;
+//        }
+//        if(armState == 3)
+//        {
+//            raisearm(200 - armPos);
+//            armPos = 200;
+//
+//        }
+
+
+
+
+
+//         Show the elapsed game time and wheel power
+//        telemetry.addData("Status", "Run Time: " + runtime.toString());
+//        telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
+//        telemetry.addData("Back  left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
+//        telemetry.addData("Speed", speed);
+
+
+        telemetry.update();
+    }
+    public void raisearm(int degrees)
+    {
+        arm.setDirection(DcMotor.Direction.REVERSE);
+        armDriveWithEncoder(.2, degrees);
+
+    }
+    public void armDriveWithEncoder(double speed,
+                                    double Inches) {
+        int newarmTarget;
+
+
+        // Determine new target position, and pass to motor controller
+        newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
+        armThroughBoreEncoder.setTargetPosition(newarmTarget);
+
+        // Turn On RUN_TO_POSITION
+        arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+        // reset the timeout time and start motion.
+        arm.setPower(Math.abs(speed));
+
+
+        arm.setPower(0);
+
+
+        // Turn off RUN_TO_POSITION
+        armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+    }
+}
+

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

@@ -92,34 +92,34 @@ public class ChassisControl extends OpMode {
      * User defined loop method.
      * This method will be called repeatedly in a loop while this op mode is running
      */
-    double num = 1;
+    double num = 3;
-    String speed = "";
+    String speed = "slow";
     public void loop() {
 
         frontLeft.setDirection(DcMotor.Direction.REVERSE);
         backLeft.setDirection(DcMotor.Direction.REVERSE);
         frontRight.setDirection(DcMotor.Direction.FORWARD);
         backRight.setDirection(DcMotor.Direction.REVERSE);
-        if(gamepad1.a){
+//        if(gamepad1.a){
-            num = 3;
+//            num = 3;
-            speed = "slow";
+//            speed = "slow";
-        }
+//        }
-        if(gamepad1.b){
+////        if(gamepad1.b){
-            num = 2.5;
+////            num = 2.5;
-            speed = "medium";
+////            speed = "medium";
-        }
+////        }
-        if(gamepad1.y){
+////        if(gamepad1.y){
-            num = 2;
+////            num = 2;
-            speed = "fast";
+////            speed = "fast";
-        }
+////        }
-        if(gamepad1.x){
+////        if(gamepad1.x){
-            num = 1.5;
+////            num = 1.5;
-            speed = "Ludicrous";
+////            speed = "Ludicrous";
-        }
+////        }
-        if(gamepad1.x && gamepad1.y){
+////        if(gamepad1.x && gamepad1.y){
-            num = 1;
+////            num = 1;
-            speed = "plaid";
+////            speed = "plaid";
-        }
+////        }
         axial   = -gamepad1.left_stick_y/num;  // Note: pushing stick forward gives negative value
         lateral =  gamepad1.left_stick_x/num;
         yaw     =  gamepad1.right_stick_x/(num+0.5);

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

@@ -20,10 +20,11 @@ public class FullRobotControl extends OpMode {
     DcMotor backRight;
     DcMotor frontLeft;
     DcMotor backLeft;
-    DcMotor armMotor;
+    //DcMotor armMotor;
     Servo gripper;
     Servo wrist;
     DcMotor arm;
+    DcMotor armThroughBoreEncoder;
     public ElapsedTime runtime = new ElapsedTime();
     static final double TICKS_TO_DEGREES = 0.07462686567;
     static final double COUNTS_PER_MOTOR_REV = 537.6;
@@ -50,6 +51,7 @@ public class FullRobotControl extends OpMode {
         backRight.setPower(0);
         frontLeft.setPower(0);
         backLeft.setPower(0);
+
     }
     /**
      * User defined init method
@@ -65,11 +67,12 @@ public class FullRobotControl extends OpMode {
         backRight = hardwareMap.dcMotor.get("Drive back rt");
         frontLeft = hardwareMap.dcMotor.get("Drive front lt");
         backLeft = hardwareMap.dcMotor.get("Drive back lt");
-        armMotor = hardwareMap.dcMotor.get("armMotor");
+        //armMotor = hardwareMap.dcMotor.get("armMotor");
         gripper = hardwareMap.servo.get("gripper");
         gripper.setPosition(1);
         wrist = hardwareMap.servo.get("wrist");
         arm = hardwareMap.dcMotor.get("arm raise");
+        armThroughBoreEncoder = hardwareMap.dcMotor.get("arm raise");
         wrist.setPosition(1);
 
     }
@@ -108,7 +111,7 @@ public class FullRobotControl extends OpMode {
      * This method will be called repeatedly in a loop while this op mode is running
      */
     double num = 2;
-    String speed = "plaid";
+    String speed = "medium";
     public void loop() {
 
         frontLeft.setDirection(DcMotor.Direction.REVERSE);
@@ -156,57 +159,76 @@ public class FullRobotControl extends OpMode {
             leftBackPower   /= max;
             rightBackPower  /= max;
         }
-        if(gamepad1.dpad_down){
+        if(gamepad2.dpad_down){
             wrist.setPosition(1);
 
         }
-        if(gamepad1.dpad_up){
+        if(gamepad2.dpad_up){
 
             wrist.setPosition(0.5);
         }
-        if(gamepad1.left_bumper){
+        if(gamepad2.left_trigger > 0.35)
+        {
             gripper.setPosition(0.25);
         }
-        if(gamepad1.right_bumper){
+        if(gamepad2.right_trigger > 0.35){
             gripper.setPosition(1);
         }
 
-        int armState = 0;
+//        int armState = 0;
-        if(gamepad2.x)
+//        int armPos = 0;
+//        if(gamepad2.x)
+//        {
+//            armState = 1;
+//          raisearm(0 - armPos);
+//          armPos = 0;
+//        }
+//        if(gamepad2.y)
+//        {
+//             armState = 2;
+//            raisearm(1000);
+//            armPos = 30;
+//        }
+//        if(gamepad2.b)
+//        {
+//             armState = 2;
+//            raisearm(200 - armPos);
+//            armPos = 200;
+//        }
+//        if(gamepad2.right_bumper)
+//        {
+//            armState =+ 1;
+//        }
+//        if(gamepad2.left_bumper)
+//        {
+//            armState =- 1;
+//        }
+//        if(armState == 1)
+//        {
+//            raisearm(0 - armPos);
+//            armPos = 0;
+//        }
+//        if(armState == 2)
+//        {
+//            raisearm(30 - armPos);
+//            armPos = 30;
+//        }
+//        if(armState == 3)
+//        {
+//            raisearm(200 - armPos);
+//            armPos = 200;
+//
+//        }
+        if(gamepad2.right_stick_y > 0)
         {
-            armState = 1;
+            arm.setPower(armPower);
-          raisearm(0);
         }
-        if(gamepad2.y)
+        if(gamepad2.left_stick_y < 0)
         {
-             armState = 2;
+            arm.setPower(armPower);
-            raisearm(30);
-        }
-        if(gamepad2.y)
-        {
-             armState = 2;
-            raisearm(200);
-        }
-        if(gamepad2.right_bumper)
-        {
-            armState =+ 1;
-        }
-        if(gamepad2.left_bumper)
-        {
-            armState =- 1;
-        }
-        if(armState == 1)
-        {
-            raisearm(0);
-        }
-        if(armState == 2)
-        {
-            raisearm(30);
-        }
-        if(armState == 1)
-        {
-            raisearm(200);
         }
+        telemetry.addData("arm is at", " %7d", armThroughBoreEncoder.getCurrentPosition());
+        telemetry.update();
 
 
 
@@ -215,29 +237,30 @@ public class FullRobotControl extends OpMode {
         frontRight.setPower(rightFrontPower);
         backLeft.setPower(leftBackPower);
         backRight.setPower(rightBackPower);
-        armMotor.setPower(armPower);
+        //arm.setPower(armPower);
-        // Show the elapsed game time and wheel power
+//         Show the elapsed game time and wheel power
-        telemetry.addData("Status", "Run Time: " + runtime.toString());
+//        telemetry.addData("Status", "Run Time: " + runtime.toString());
-        telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
+//        telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
-        telemetry.addData("Back  left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
+//        telemetry.addData("Back  left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
-        telemetry.addData("Speed", speed);
+//        telemetry.addData("Speed", speed);
 
 
         telemetry.update();
     }
     public void raisearm(int degrees)
     {
-        armEncoder(.2, degrees*TICKS_TO_DEGREES);
+        arm.setDirection(DcMotor.Direction.REVERSE);
+        armDriveWithEncoder(.2, degrees*TICKS_TO_DEGREES);
 
     }
-    public void armEncoder(double speed,
+    public void armDriveWithEncoder(double speed,
                            double Inches) {
         int newarmTarget;
 
 
         // Determine new target position, and pass to motor controller
-        newarmTarget = arm.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
+        newarmTarget = armThroughBoreEncoder.getCurrentPosition() + (int) (Inches * COUNTS_PER_ARM_INCH);
-        arm.setTargetPosition(newarmTarget);
+        armThroughBoreEncoder.setTargetPosition(newarmTarget);
 
         // Turn On RUN_TO_POSITION
         arm.setMode(DcMotor.RunMode.RUN_TO_POSITION);
@@ -251,7 +274,7 @@ public class FullRobotControl extends OpMode {
 
 
         // Turn off RUN_TO_POSITION
-        arm.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+        armThroughBoreEncoder.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
 
         }
     }

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

@@ -0,0 +1,181 @@
+package org.firstinspires.ftc.teamcode;
+
+
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Gamepad;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+
+
+@TeleOp( name = "manual control")
+public class arm extends OpMode {
+
+    DcMotor arm;
+    Servo gripper;
+    Servo wrist;
+    public double axial;
+    public double lateral;
+    public double yaw;
+    DcMotor frontRight;
+    DcMotor backRight;
+    DcMotor frontLeft;
+    DcMotor backLeft;
+
+
+    /**
+     * this function takes a long milliseconds parameter and sleeps
+     * @param millis milliseconds to sleep
+     */
+    public void sleepmillis(long millis) {
+        try {
+            Thread.sleep(millis);
+        } catch (Exception e) {
+        }
+    }
+
+    /**
+     * stops all drive motors
+     */
+    public void off() {
+        arm.setPower(0);
+        frontRight.setPower(0);
+        backRight.setPower(0);
+        frontLeft.setPower(0);
+        backLeft.setPower(0);
+
+    }
+    /**
+     * User defined init method
+     * This method will be called once when the INIT button is pressed.
+     */
+
+    public void init() {
+
+
+        telemetry.addData("Status","In Init()");
+        telemetry.update();
+        arm = hardwareMap.dcMotor.get("arm raise");
+        gripper = hardwareMap.servo.get("gripper");
+        wrist = hardwareMap.servo.get("wrist");
+        frontRight = hardwareMap.dcMotor.get("Drive front rt");
+        backRight = hardwareMap.dcMotor.get("Drive back rt");
+        frontLeft = hardwareMap.dcMotor.get("Drive front lt");
+        backLeft = hardwareMap.dcMotor.get("Drive back lt");
+
+
+
+    }
+
+    /**
+     * User defined init_loop method
+     * This method will be called repeatedly when the INIT button is pressed.
+     * This method is optional. By default this method takes no action.
+     */
+    public void init_loop(){
+        // Wait for the game to start (driver presses PLAY)
+        telemetry.addData("Status", "Initialized");
+        telemetry.update();
+    }
+    /**
+     * User defined start method.
+     * This method will be called once when the PLAY button is first pressed.
+     * This method is optional. By default this method takes not action. Example usage: Starting another thread.
+     */
+    public void start() {
+
+    }
+
+    /**
+     * User defined stop method
+     * This method will be called when this op mode is first disabled.
+     * The stop method is optional. By default this method takes no action.
+     */
+    public void stop(){
+
+    }
+
+    double num = 3;
+    /**
+     * User defined loop method.
+     * This method will be called repeatedly in a loop while this op mode is running
+     */
+    public void loop() {
+        frontLeft.setDirection(DcMotor.Direction.REVERSE);
+        backLeft.setDirection(DcMotor.Direction.REVERSE);
+        frontRight.setDirection(DcMotor.Direction.FORWARD);
+        backRight.setDirection(DcMotor.Direction.REVERSE);
+
+
+        double armPower = gamepad2.right_stick_y/3.5;
+        // Normalize the values so no wheel power exceeds 100%
+        // This ensures that the robot maintains the desired motion.
+
+        if(gamepad2.right_stick_y != 0)
+        {
+            arm.setPower(armPower);
+            telemetry.addData("joystick y value", gamepad2.right_stick_y);
+            telemetry.update();
+        }
+        else
+        {
+            arm.setPower(0);
+        }
+        if(gamepad2.left_trigger > 0.35)
+        {
+            gripper.setPosition(0.25);
+        }
+        if(gamepad2.right_trigger > 0.35){
+            gripper.setPosition(1);
+        }
+        if(gamepad2.dpad_up)
+        {
+            wrist.setPosition(0.4);
+        }
+        if(gamepad2.dpad_down)
+        {
+            wrist.setPosition(1);
+        }
+        if(gamepad2.dpad_right)
+        {
+            wrist.setPosition(0);
+        }
+        axial   = -gamepad1.left_stick_y/num;  // Note: pushing stick forward gives negative value
+        lateral =  gamepad1.left_stick_x/num;
+        yaw     =  gamepad1.right_stick_x/(num+0.5);
+        // Combine the joystick requests for each axis-motion to determine each wheel's power.
+        // Set up a variable for each drive wheel to save the power level for telemetry.
+        double leftFrontPower  = axial + lateral + yaw;
+        double rightFrontPower = axial - lateral - yaw;
+        double leftBackPower   = axial - lateral + yaw;
+        double rightBackPower  = axial + lateral - yaw;
+        // Normalize the values so no wheel power exceeds 100%
+        // This ensures that the robot maintains the desired motion.
+        double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
+        max = Math.max(max, Math.abs(leftBackPower));
+        max = Math.max(max, Math.abs(rightBackPower));
+        if (max > 1.0) {
+            leftFrontPower  /= max;
+            rightFrontPower /= max;
+            leftBackPower   /= max;
+            rightBackPower  /= max;
+        }
+        frontLeft.setPower(leftFrontPower);
+        frontRight.setPower(rightFrontPower);
+        backLeft.setPower(leftBackPower);
+        backRight.setPower(rightBackPower);
+        // Show the elapsed game time and wheel power
+        telemetry.addData("Front left, Right", "%4.2f, %4.2f", leftFrontPower, rightFrontPower);
+        telemetry.addData("Back  left, Right", "%4.2f, %4.2f", leftBackPower, rightBackPower);
+        telemetry.update();
+
+
+
+
+
+
+    }
+}