diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/DoubleReverse4Bar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/DoubleReverse4Bar.java new file mode 100644 index 0000000..70b8c73 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/DoubleReverse4Bar.java @@ -0,0 +1,64 @@ +package org.firstinspires.ftc.teamcode.ftc16072.Mechanisms; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.teamcode.ftc16072.Tests.QQTest; +import org.firstinspires.ftc.teamcode.ftc16072.Tests.TestTwoMotors; +import org.firstinspires.ftc.teamcode.ftc16072.Util.PIDFController; + +import java.util.Arrays; +import java.util.List; + +public class DoubleReverse4Bar extends QQMechanism{ + public static final double TEST_SPEED = 0.2; + DcMotor leftMotor; + DcMotor rightMotor; + public static double kP = 0.001; + public static double kI = 0.0; + public static double kD = 0.0; + public static double kF = 0.0; + public static double max = 0.8; + public static double min = 0.0; + + int currentPos; + int desiredPos; + + + PIDFController pidfController = new PIDFController(kP,kI,kD,kF,max,min); + + public int INTAKE_POS = 0;//TODO: make real + + public int PLACE_POS = 600; //TODO: make real :) + + @Override + public void init(HardwareMap hwMap) { + leftMotor = hwMap.get(DcMotor.class, "left_4bar_motor"); + rightMotor = hwMap.get(DcMotor.class, "right_4bar_motor"); + leftMotor.setDirection(DcMotorSimple.Direction.REVERSE); + leftMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + leftMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + public void setPosition(int desiredPos){ + this.desiredPos = desiredPos; + } + public void manualPositionChange(int changeAmount){ + desiredPos += changeAmount; + } + @Override + public void update(){ + currentPos = (leftMotor.getCurrentPosition() + rightMotor.getCurrentPosition())/2;//average left and right speeds + double motorPower = pidfController.calculate(desiredPos,currentPos); + leftMotor.setPower(motorPower); + rightMotor.setPower(motorPower); + } + + @Override + public List getTests() { + return Arrays.asList(new TestTwoMotors("fourbar", TEST_SPEED,leftMotor,rightMotor)); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java index 1aef3fb..7e15556 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Robot.java @@ -4,6 +4,7 @@ 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; @@ -18,6 +19,7 @@ public class Robot { public MecanumDrive mecanumDrive; public OpticalTrackingOdometrySensor otos; public Claw claw; + public DoubleReverse4Bar doubleReverse4Bar; public Slides slides; List mechanisms; @@ -26,6 +28,7 @@ public Robot() { controlHub = new ControlHub(); otos = new OpticalTrackingOdometrySensor(); claw = new Claw(); + doubleReverse4Bar = new DoubleReverse4Bar(); slides = new Slides(); mechanisms = Arrays.asList( @@ -33,8 +36,9 @@ public Robot() { mecanumDrive, otos, claw, - slides); - + slides, + claw, + doubleReverse4Bar); } public void init(HardwareMap hwMap) { for (QQMechanism mechanism : mechanisms) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestTwoMotors.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestTwoMotors.java new file mode 100644 index 0000000..371a48e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Tests/TestTwoMotors.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.ftc16072.Tests; + +import com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public class TestTwoMotors extends QQTest{ + DcMotor leftMotor; + DcMotor rightMotor; + double speed; + public TestTwoMotors(String name, double speed, DcMotor leftMotor, DcMotor rightMotor){ + super(name); + this.speed = speed; + this.leftMotor = leftMotor; + this.rightMotor = rightMotor; + } + @Override + public void run(boolean on, Telemetry telemetry) { + telemetry.addData("encoder position",leftMotor.getCurrentPosition()); + telemetry.addData("encoder position",rightMotor.getCurrentPosition()); + if(on){ + leftMotor.setPower(speed); + rightMotor.setPower(speed); + + }else{ + leftMotor.setPower(0); + rightMotor.setPower(0); + + } + + } + +}