diff --git a/HARDWARE.md b/HARDWARE.md index 878acd1..dd27e58 100644 --- a/HARDWARE.md +++ b/HARDWARE.md @@ -10,22 +10,22 @@ There are two robots: 14493-DS, and FTC-992M. Below are the following configurations for our robots -| physical port | hub | robot part | robot part location | robot software config name | -|---------------|-----------|-----------------------------|-------------------------------|----------------------------| -| motor0 | control | UltraPlanetary HD hex motor | right front leg frame | Drive front rt | -| motor1 | control | UltraPlanetary HD hex motor | right back leg frame | Drive back rt | -| motor2 | control | UltraPlanetary HD hex motor | left front leg frame | Drive front lt | -| 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 | - | 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 | +| physical port | hub | robot part | robot part location | robot software config name | +|---------------|-----------|------------------------------|-------------------------------|----------------------------| +| motor0 | control | UltraPlanetary HD hex motor | right front leg frame | Drive front rt | +| motor1 | control | UltraPlanetary HD hex motor | right back leg frame | Drive back rt | +| motor2 | control | UltraPlanetary HD hex motor | left front leg frame | Drive front lt | +| 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 | + | 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 | UltraPlanetary HD hex motor* | arm frame back right | axle encoder | +| Servo 0 | expansion | Servo | on arm | wrist | +| Servo 1 | expansion | Servo | on arm | gripper | - +** * **: The device plugged into motor3 is actually a Digital Device encoder but behaves like a UltraPlanetary HD hex motor encoder. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/manualChasis.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/manualChasis.java new file mode 100644 index 0000000..0c9a2b6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/manualChasis.java @@ -0,0 +1,205 @@ +package org.firstinspires.ftc.teamcode; + + +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.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; + + +@TeleOp( name = "manual Chasis") +public class manualChasis 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; + DcMotor hang; + + + /** + * 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"); + hang = hardwareMap.dcMotor.get("hang"); + + + + } + + /** + * 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 = 2.25; + final static double MOTOR_HI_SPEED_RATIO = 2.25; + final static double MOTOR_LO_SPEED_RATIO = 3.5; + final static double ARM_POWER = 3.5; + double num = MOTOR_HI_SPEED_RATIO; + /** + * 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.FORWARD); + frontRight.setDirection(DcMotor.Direction.REVERSE); + backRight.setDirection(DcMotor.Direction.FORWARD); + + + double armPower = gamepad2.right_stick_y/ARM_POWER; + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + if(gamepad1.a) + { + num = MOTOR_HI_SPEED_RATIO; + } + if (gamepad1.b) + { + num = MOTOR_LO_SPEED_RATIO; + } + if(gamepad2.right_stick_y != 0) + { + arm.setPower(armPower); + telemetry.addData("joystick y value", gamepad2.right_stick_y); + telemetry.update(); + } + else + { + arm.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + arm.setPower(0); + } + if(gamepad2.left_trigger > 0.35) + { + gripper.setPosition(0); + } + 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); + } + if (gamepad1.dpad_up) + { + hang.setPower(1); + } + hang.setPower(0); + if (gamepad1.dpad_down) + { + hang.setPower(-.5); + } + else { + hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + hang.setPower(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); + // 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(); + + + + + + + } +}