-
Notifications
You must be signed in to change notification settings - Fork 1
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #14 from ftc16072/armCode
Adding Teleop code and new positions
- Loading branch information
Showing
8 changed files
with
165 additions
and
18 deletions.
There are no files selected for viewing
75 changes: 75 additions & 0 deletions
75
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Arm.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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) | ||
| ); | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
46 changes: 46 additions & 0 deletions
46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/ScrimmageTeleop.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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();} | ||
| } | ||
| } |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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> |