diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultAuto.java index 06a816c..8f352bc 100755 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultAuto.java @@ -7,7 +7,7 @@ * @Author:Will * */ -//85 pts auto +//possible 85 pts auto public class DefaultAuto extends DefaultAutoMode { @Override public void init() { @@ -17,63 +17,70 @@ public void init() { @Override public void loop() { switch (state){ - - case 0: //grab glyph, lower arm and get pictogram + case 0: hardware.lift.closeAll(); - hardware.jewelArm.setPosition(JEWEL_ARM_DOWN_POS); - nextState(getPictogram()); + nextState(pause(0.5)); break; - case 1: // raise glyph - nextState(setGlyphLiftPos(5,1)); - break; + hardware.lift.closeAll(); + nextState(raiseGlyph()); + break; case 2: //get jewel - nextState(getJewel()); + hardware.lift.closeAll(); + nextState(/*getJewel()*/true); break; - - case 3: // drive forward 24 + 2 (to get off balancing stone) - nextState(autoDrive(new XY(0,26),1)); + case 3: //drive forward or backward 24 + 2(to get off balancing stone) + hardware.lift.closeAll(); + switch (allianceColor){ + case RED: + nextState(autoDrive(new XY(0,26),1)); + break; + + case BLUE: + nextState(autoDrive(new XY(0,-26),1)); + break; + } break; - case 4: //turn based on position + case 4: // turn based on color + hardware.lift.closeAll(); nextState(driveTurn()); break; - case 5: //drive forward 12 - nextState(autoDrive(new XY(0,12),1)); + case 5: // drive to safezone + hardware.lift.closeAll(); + nextState(safeZoneDrive()); break; - case 6: //2nd turn based on position + case 6: // turn at cryptobox + hardware.lift.closeAll(); nextState(cryptoBoxTurn()); break; - case 7: // move to the correct cryptobox slot - nextState(pictogramDrive(pictogram)); - break; - - case 8: // drive into crypto box slot slowly - nextState(autoDrive(new XY(0,12),0.25)); + case 7: //input glyph + hardware.lift.closeAll(); + nextState(autoDrive(new XY(0,6),1)); break; - case 9: // lower glyph - nextState(setGlyphLiftPos(-5,1)); + case 8: // lower glyph + hardware.lift.closeAll(); + nextState(lowerGlyph()); break; - case 10: //release glyph + case 9: //release glyph hardware.lift.openAll(); nextState(pause(1)); break; - case 11: // back away from glyph + case 10: //back away nextState(autoDrive(new XY(0,-2),1)); break; - case 12: // telemetry + case 11 : //Telemetry telemetry.addData("INFO",LSA); break; - } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultAutoMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultAutoMode.java index 4c8ba06..90f576a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultAutoMode.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultAutoMode.java @@ -9,21 +9,24 @@ import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable; import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables; -import org.firstinspires.ftc.teamcode.Auto.Enums.AllianceColor; -import org.firstinspires.ftc.teamcode.Auto.Enums.PictogramResults; -import org.firstinspires.ftc.teamcode.Auto.Enums.StartPosition; +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; +import org.firstinspires.ftc.teamcode.Enums.JewelColor; +import org.firstinspires.ftc.teamcode.Enums.PictogramResults; +import org.firstinspires.ftc.teamcode.Enums.StartPosition; import org.firstinspires.ftc.teamcode.Gyro; import org.montclairrobotics.sprocket.drive.DTTarget; import org.montclairrobotics.sprocket.geometry.Vector; import org.montclairrobotics.sprocket.geometry.XY; +import static org.firstinspires.ftc.teamcode.Enums.JewelColor.BLUE; + /** * Created by Montclair Robotics on 11/13/17. * @Author:Will * */ -public class DefaultAutoMode extends OpMode{ +public class DefaultAutoMode extends OpMode { //Vuforia Vars VuforiaLocalizer vuforia; @@ -33,8 +36,8 @@ public class DefaultAutoMode extends OpMode{ PictogramResults pictogram; //Color Prox Vars - ColorSensor sensorColor; - AllianceColor color; + ColorSensor colorSensor; + JewelColor jewelColor; //auto mode objects Gyro gyro; @@ -45,6 +48,11 @@ public class DefaultAutoMode extends OpMode{ ElapsedTime timer; double startTime; + private int redOverBlue; + private boolean averaging=false; + private long avgTimeout; + + //final public final double JEWEL_ARM_DOWN_POS = 1; public final double JEWEL_ARM_UP_POS = 0; @@ -52,6 +60,7 @@ public class DefaultAutoMode extends OpMode{ public final double TICKS_PER_INCH = 1500/42.3; public final double TICKS_PER_DEGREE = (43.23/360)*TICKS_PER_INCH*2.5; public final double TICKS_PER_LIFT_INCH = 1400/6.5; + public final double autoTurnSpeed = 0.75; @@ -59,14 +68,13 @@ public void autoInit(){ hardware = new DefaultHardwareMap(); hardware.init(hardwareMap); hardware.lift.closeAll(); -// gyro = new Gyro(); //uncomment once josh makes it work telemetry.addData("INFO","Hardware Map Init"); mapper = new DefaultMecanumMapper(); - sensorColor = hardware.sensorColor; + colorSensor = hardware.colorSensor; setState(0); timer = new ElapsedTime(); startTime = timer.milliseconds(); - visionInit(); +// visionInit(); hardware.resetDriveEncoders(); hardware.resetLiftEncoders(); telemetry.addData("INFO", "INITIALIZED"); @@ -125,48 +133,192 @@ public boolean pictogramDrive(PictogramResults image){ } //colorProx - private boolean getJewelColor(){ - if(sensorColor.red() > sensorColor.blue()){ - color = AllianceColor.RED; - }else if(sensorColor.blue()> sensorColor.red()){ - color = AllianceColor.BLUE; - }else{ - telemetry.addData("INFO","ID FAILED"); - return false; + public boolean getJewelColor(){ + if(colorSensor.red() > colorSensor.blue()){ + jewelColor = JewelColor.RED; + + }else if(colorSensor.blue()> colorSensor.red()){ + jewelColor = JewelColor.BLUE; + }else { + jewelColor = JewelColor.UNKNOWN; } - telemetry.addData("Jewel Color", color); + telemetry.addData("Color",jewelColor); return true; - } //pt1 - private boolean moveJewel(){ - if(getJewelColor()){ - if(color != allianceColor){ - return autoTurn(-30,0.5); + } + + public boolean getJewelColorAvg() + { + if(!averaging) { + averaging=true; + avgTimeout=System.currentTimeMillis()+3000; + redOverBlue=0; + } + int red=colorSensor.red(); + int blue=colorSensor.blue(); + if(red>blue) { + redOverBlue++; + } + if(blue>red) { + redOverBlue--; + } + if(Math.abs(redOverBlue)>10) { + if(redOverBlue>0) { + jewelColor=JewelColor.RED; } else { - return autoTurn(30,0.5); + jewelColor=JewelColor.BLUE; } - } else { - return false; + telemetry.addData("Jewel Color", jewelColor); + averaging=false; + return true; } - } //pt2 - private boolean jewelArmUp(){ - if(moveJewel()){ - hardware.jewelArm.setPosition(JEWEL_ARM_UP_POS); - return (pause(2)); - } else { - return false; + if(System.currentTimeMillis()>avgTimeout) { + jewelColor=JewelColor.UNKNOWN; + telemetry.addData("Jewel Color", jewelColor); + averaging=false; + return true; } - } //pt3 + return false; + } + + int jewelState = 0; public boolean getJewel(){ - if(jewelArmUp()){ - if(color != allianceColor){ - return autoTurn(30,0.5); - } else { - return autoTurn(-30,0.5); - } - } else { - return false; + switch (jewelState){ + case 0: //lower arm + hardware.jewelArm.setPosition(JEWEL_ARM_DOWN_POS); + if(pause(2)){ + jewelState++; + } + break; + + case 1: // get reading + if(getJewelColor()){ + jewelState++; + } + break; + + case 2: //react accordingly +// double reactDegrees=(allianceColor==AllianceColor.RED)?-30:30; +// if(jewelColor==BLUE) { +// reactDegrees*=-1; +// } +// if(autoTurn(reactDegrees,autoTurnSpeed)){ +// jewelState++; +// } + switch (allianceColor){ + + case RED: + switch (jewelColor){ + case RED: + if(autoTurn(30,1)){ + jewelState++; + } + break; + + case BLUE: + if(autoTurn(-30,1)){ + jewelState++; + } + break; + + case UNKNOWN: + jewelState++; + break; + } + break; + + case BLUE: + switch (jewelColor){ + case RED: + if(autoTurn(-30,1)){ + + jewelState++; + } + break; + + case BLUE: + if(autoTurn(30,1)){ + jewelState++; + } + break; + + case UNKNOWN: + jewelState++; + break; + } + break; + } + break; + + case 3: //reset encoders + hardware.resetDriveEncoders(); + jewelState++; + break; + + case 4: // raise arm + hardware.jewelArm.setPosition(JEWEL_ARM_UP_POS); + if(pause(2)){ + jewelState++; + } + break; + + case 5: //reset robot accordingly +// double resetDegrees=(allianceColor==AllianceColor.RED)?30:-30; +// if(jewelColor==BLUE) +// { +// resetDegrees*=-1; +// } +// if(autoTurn(resetDegrees,autoTurnSpeed)){ +// jewelState++; +// } + switch (allianceColor){ + case RED: + switch (jewelColor){ + case RED: + if(autoTurn(-30,1)){ + jewelState++; + } + break; + + case BLUE: + if(autoTurn(30,1)){ + jewelState++; + } + break; + + case UNKNOWN: + jewelState++; + break; + } + break; + + case BLUE: + switch (jewelColor){ + case RED: + if(autoTurn(30,1)){ + jewelState++; + } + break; + + case BLUE: + if(autoTurn(-30,1)){ + jewelState++; + } + break; + + case UNKNOWN: + jewelState++; + break; + } + break; + } + break; + + case 6: // return true + return true; } - } //pt4 + return false; + } + //driving /* @@ -175,7 +327,6 @@ public boolean getJewel(){ public void drive(Vector v, double turn) { mapper.map(new DTTarget(v, turn), hardware.driveModules); } - public boolean autoDrive(Vector v, double speed) { if(Math.abs(hardware.getTicks()) < v.getMagnitude()*TICKS_PER_INCH) { drive(v.normalize().scale(speed), 0); @@ -186,7 +337,6 @@ public boolean autoDrive(Vector v, double speed) { } return false; } - public boolean autoTurn(double degrees, double speed) { int ticks = Math.abs(hardware.frontLeft.getCurrentPosition()); if(degrees < 0) { @@ -208,17 +358,24 @@ public boolean autoTurn(double degrees, double speed) { } return false; } - public boolean driveTurn(){ switch (startPosition){ case CLOSE: - return autoTurn(90,0.5); + return autoTurn(90,autoTurnSpeed); case FAR: - return autoTurn(-90,0.5); + return autoTurn(-90,autoTurnSpeed); } + return false; + } + public boolean safeZoneDrive(){ + switch (startPosition){ + case CLOSE: + return true; - telemetry.addData("INFO","Alliance Color or Start Position NOT defined"); + case FAR: + return autoDrive(new XY(0,12),1); + } return false; } public boolean cryptoBoxTurn(){ @@ -229,7 +386,7 @@ public boolean cryptoBoxTurn(){ return true; case FAR: - return autoTurn(90,0.5); + return autoTurn(90,autoTurnSpeed); } break; @@ -239,11 +396,10 @@ public boolean cryptoBoxTurn(){ return true; case FAR: - return autoTurn(-90,0.5); + return autoTurn(-90,autoTurnSpeed); } break; } - telemetry.addData("INFO","Alliance Color or Start Position NOT defined"); return false; } @@ -252,8 +408,7 @@ public void glyphLiftPower(double power){ hardware.glyphLeft.setPower(power); hardware.glyphRight.setPower(power); } - - public boolean setGlyphLiftPos(double inch, double power){ + private boolean setGlyphLiftPos(double inch, double power){ if(inch>0){ if (hardware.getLiftTicks()0) { + telemetry.addData("Avg BLUE MINUS RED", (float) total / samples); + } + else { + telemetry.addData("Avg BLUE MINUS RED", 0); + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestGlyphInfo.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestGlyphInfo.java new file mode 100644 index 0000000..c6cb817 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestGlyphInfo.java @@ -0,0 +1,87 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.Servo; + +/** + * Created by MHS Robotics on 11/9/2017. + */ + +@TeleOp(name = "Info: Glyph Test") +public class TestGlyphInfo extends DefaultAutoMode { + + Gamepad controller; + Servo[] servos; + int index; + boolean wasPressed; + + + @Override + public void init() { + controller = this.gamepad1; + servos = new Servo[5]; + servos[0] = hardwareMap.get(Servo.class, "intake_right_top"); + servos[1] = hardwareMap.get(Servo.class, "intake_left_top"); + servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom"); + servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom"); + servos[4] = hardwareMap.get(Servo.class, "jewel_arm"); + index = 0; + wasPressed = false; + + autoInit(); + } + + @Override + public void loop() { + if(!wasPressed && (controller.dpad_left || controller.dpad_right)) { + wasPressed = true; + if(controller.dpad_left) { + index--; + if(index == -1) { + index = servos.length - 1; + } + } else if(controller.dpad_right) { + index++; + if(index == servos.length) { + index = 0; + } + } + } else if(wasPressed && !controller.dpad_left && !controller.dpad_right) { + wasPressed = false; + } + + telemetry.addData("index", index); + + if(controller.dpad_up) { + servos[index].setPosition(servos[index].getPosition()+.005); + } + + if(controller.dpad_down) { + servos[index].setPosition(servos[index].getPosition()-.005); + } + + if(controller.a) { + if(controller.left_trigger > 0.5) { + hardware.lift.openBottom(); + } else if(controller.right_trigger > 0.5) { + hardware.lift.openTop(); + } else { + hardware.lift.openAll(); + } + } + + if(controller.b) { + if(controller.left_trigger > 0.5) { + hardware.lift.closeBottom(); + } else if(controller.right_trigger > 0.5) { + hardware.lift.closeTop(); + } else { + hardware.lift.closeAll(); + } + } + + telemetry.addData("pos", servos[index].getPosition()); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestJewelID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestJewelID.java index 613fe34..669b277 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestJewelID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestJewelID.java @@ -1,17 +1,16 @@ package org.firstinspires.ftc.teamcode.Auto; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import org.firstinspires.ftc.teamcode.Auto.Enums.AllianceColor; -import org.montclairrobotics.sprocket.actions.StateMachine; -import org.montclairrobotics.sprocket.auto.states.DriveTime; -import org.montclairrobotics.sprocket.geometry.XY; +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; /** * Created by Montclair Robotics on 11/13/17. * @Author:Will * */ @Autonomous(name = "Test: Jewel ID") +@Disabled public class TestJewelID extends DefaultAutoMode { @Override @@ -25,16 +24,12 @@ public void init() { public void loop() { switch (state) { case 0: - hardware.jewelArm.setPosition(JEWEL_ARM_DOWN_POS); - nextState(pause(1)); - break; - - case 1: nextState(getJewel()); break; - case 2: + case 1: telemetry.addData("INFO",LSA); + break; } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestVuforia.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestVuforia.java index df22a44..06569a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestVuforia.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestVuforia.java @@ -8,6 +8,7 @@ * @Author:Will * */ @Autonomous(name = "Test: Vision") +@Disabled public class TestVuforia extends DefaultAutoMode { @Override public void init() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java index 2af92c0..f76f87b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -6,21 +6,23 @@ import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.Servo; -import org.firstinspires.ftc.teamcode.Components.DriveTrain; +import org.firstinspires.ftc.teamcode.Auto.DefaultAutoMode; +//import org.firstinspires.ftc.teamcode.Components.DriveTrain; import org.firstinspires.ftc.teamcode.Components.GlyphIntake2; /** * Created by Montclair Robotics on 11/13/17. * @Author:Garrett * */ -@TeleOp(name="Teleop: PLEASE DON'T DELETE THIS WILL") +@TeleOp(name="ANEEKAH Teleop: Competition") public class CompTeleop extends OpMode { - public DriveTrain driveTrain; + //public DriveTrain driveTrain; DcMotor frontRight, backRight, frontLeft, backLeft; Servo[] servos; - Gyro gyro; +// Gyro gyro; + public static final int SERVORT=0,SERVOLT=1,SERVORB=2,SERVOLB=3; GlyphIntake2 intake; DcMotor liftA, liftB; @@ -28,12 +30,22 @@ public class CompTeleop extends OpMode { @Override public void init() { - gyro = new Gyro(hardwareMap); - this.driveTrain = new DriveTrain(hardwareMap, gyro); + //this.driveTrain = new DriveTrain(hardwareMap); + + frontRight = hardwareMap.get(DcMotor.class, "right_front"); + backRight = hardwareMap.get(DcMotor.class, "right_back"); + backLeft = hardwareMap.get(DcMotor.class, "left_back"); + frontLeft = hardwareMap.get(DcMotor.class, "left_front"); + + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); liftA = hardwareMap.get(DcMotor.class,"lift_left"); liftB = hardwareMap.get(DcMotor.class,"lift_right"); + servos = new Servo[4]; servos[0] = hardwareMap.get(Servo.class, "intake_right_top"); servos[1] = hardwareMap.get(Servo.class, "intake_left_top"); @@ -41,20 +53,51 @@ public void init() { servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom"); intake = new GlyphIntake2(servos); - gyro = new Gyro(hardwareMap); +// gyro = new Gyro(hardwareMap); limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); } @Override - public void loop() { - driveTrain.driveMechanum(gamepad1); + public void loop(){ + + double pow = 1.0; + + if (gamepad1.left_bumper) { + pow = 0.5; + } + + double x = gamepad1.left_stick_x * pow * 2; + double y = -gamepad1.left_stick_y * pow; + double turn = gamepad1.right_stick_x * pow; + + if(gamepad1.dpad_up) + { + y=pow; + } + if(gamepad1.dpad_left) + { + x=-pow; + } + if(gamepad1.dpad_down) + { + y=-pow; + } + if(gamepad1.dpad_right) + { + x=pow; + } + + frontRight.setPower(x - y + turn); + backRight.setPower(-x - y + turn); + backLeft.setPower(-x + y + turn); + frontLeft.setPower(x + y + turn); - if (gamepad2.a) + if (gamepad2.a||gamepad2.x) intake.openBottom(); else intake.closeBottom(); - if (gamepad2.b) + if (gamepad2.b||gamepad2.x) intake.openTop(); else intake.closeTop(); @@ -62,7 +105,6 @@ public void loop() { liftA.setPower(gamepad2.left_stick_y); liftB.setPower(-gamepad2.left_stick_y); - telemetry.addData("Orientation", gyro.x + "°"); telemetry.addData("Limit Switch", limitSwitch); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java new file mode 100644 index 0000000..cf11c73 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -0,0 +1,189 @@ +package org.firstinspires.ftc.teamcode; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +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.DigitalChannel; +import com.qualcomm.robotcore.hardware.Servo; + +import org.firstinspires.ftc.teamcode.Auto.DefaultAutoMode; +//import org.firstinspires.ftc.teamcode.Components.DriveTrain; +import org.firstinspires.ftc.teamcode.Components.GlyphIntake2; +import org.montclairrobotics.sprocket.geometry.Degrees; +import org.montclairrobotics.sprocket.geometry.Vector; +import org.montclairrobotics.sprocket.geometry.XY; + +/** + * Created by Montclair Robotics on 11/13/17. + * @author Garrett + * */ +@TeleOp(name="Teleop: Gyro Enabled") +//@Disabled +public class CompTeleopWithGyro extends OpMode { + //public DriveTrain driveTrain; + DcMotor frontRight, backRight, frontLeft, backLeft; + Servo[] servos; + + Gyro gyro; + PID gyroLock; + boolean lastEnabled; + + double lastTime; + + double zeroAngle; + + enum CTRL_MODE {ROBOT,FIELD}; + + CTRL_MODE myCtrlMode; + + GlyphIntake2 intake; + DcMotor liftA, liftB; + DigitalChannel limitSwitch; + + @Override + public void init() { + //this.driveTrain = new DriveTrain(hardwareMap); + + frontRight = hardwareMap.get(DcMotor.class, "right_front"); + backRight = hardwareMap.get(DcMotor.class, "right_back"); + backLeft = hardwareMap.get(DcMotor.class, "left_back"); + frontLeft = hardwareMap.get(DcMotor.class, "left_front"); + + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + + liftA = hardwareMap.get(DcMotor.class,"lift_left"); + liftB = hardwareMap.get(DcMotor.class,"lift_right"); + + servos = new Servo[4]; + servos[0] = hardwareMap.get(Servo.class, "intake_right_top"); + servos[1] = hardwareMap.get(Servo.class, "intake_left_top"); + servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom"); + servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom"); + + intake = new GlyphIntake2(servos); + gyro = new Gyro(hardwareMap); + myCtrlMode=CTRL_MODE.ROBOT; + gyroLock=new PID(.06,0,0, -180, 180, -1.0, 1.0); + zeroAngle=gyro.get().getX(); + limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); + lastTime=System.currentTimeMillis(); + } + + @Override + public void loop(){ + double pow = 1; + + if (gamepad1.left_bumper) { + pow = 0.5; //maybe lower + } + + double x = 0.0; + double y = 0.0; + double turn = gamepad1.right_stick_x * pow; + + if (gamepad1.dpad_up) + y = pow; + else if (gamepad1.dpad_down) + y = -pow; + else + y = -gamepad1.left_stick_y * pow; + + if (gamepad1.dpad_left) + x = -pow; + else if (gamepad1.dpad_right) + x=pow; + else + x = gamepad1.left_stick_x * pow; + + if(gamepad1.x) { + zeroAngle = gyro.get().getX(); + } + double dt=(System.currentTimeMillis()-lastTime)/1000.0; + lastTime=System.currentTimeMillis(); + + if(gamepad2.dpad_right) + { + gyroLock.P+=0.2*dt; + } + if(gamepad2.dpad_left) + { + gyroLock.P-=0.2*dt; + } + if(gamepad2.dpad_up) + { + gyroLock.D+=0.2*dt; + } + if(gamepad2.dpad_down) + { + gyroLock.D-=0.2*dt; + } + + if (gamepad1.a) { + if(!lastEnabled) { + gyroLock.setTarget(-gyro.get().getX()); + } + + turn = gyroLock.get(-gyro.get().getX()); + } + else + { + gyroLock.get(-gyro.get().getX()); + } + + lastEnabled = gamepad1.a; + + if (gamepad1.left_trigger > 0.5) { + myCtrlMode = CTRL_MODE.ROBOT; + } + + if (gamepad1.right_trigger > 0.5) { + myCtrlMode = CTRL_MODE.FIELD; + } + + if(myCtrlMode == CTRL_MODE.FIELD) { + Vector ctrl = new XY(x,y); + ctrl.rotate(new Degrees(gyro.get().getX() - zeroAngle)); + x = ctrl.getX(); + y = ctrl.getY(); + } + + double fr = x - y + turn; + double br = -x - y + turn; + double bl = -x + y + turn; + double fl = x + y + turn; + + double max = Math.max(1.0, Math.max(Math.max(Math.abs(fr), Math.abs(br)), Math.max(Math.abs(bl), Math.abs(fl)))); + + frontRight.setPower(fr / max); + backRight.setPower(br / max); + backLeft.setPower(bl / max); + frontLeft.setPower(fl / max); + + if (gamepad2.a || gamepad2.x) + intake.openBottom(); + else + intake.closeBottom(); + + if (gamepad2.b||gamepad2.x) + intake.openTop(); + else + intake.closeTop(); + + liftA.setPower(gamepad2.left_stick_y); + liftB.setPower(-gamepad2.left_stick_y); + + telemetry.addData("Limit Switch", limitSwitch); + telemetry.addData("Control Mode",myCtrlMode); + telemetry.addData("Gyro Angle", (int) gyro.get().getX() + " deg"); + telemetry.addData("Turn: ", turn); + telemetry.addData("target",gyroLock.target); + telemetry.addData("P",gyroLock.P); + telemetry.addData("D",gyroLock.D); + telemetry.addData("Error",gyroLock.lastError); + telemetry.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java index f23a199..bbb85a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/DriveTrain.java @@ -4,60 +4,60 @@ import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.teamcode.GyroLock; +//import org.firstinspires.ftc.teamcode.GyroLockck; import org.firstinspires.ftc.teamcode.PID; import org.firstinspires.ftc.teamcode.Gyro; /** * Created by Joshua Rapoport on 11/20/17. */ - -public class DriveTrain { - public static final double TURN_ERROR = 0.1; - - GyroLock lock; - DcMotor frontLeft, frontRight, backLeft, backRight; - - public DriveTrain(HardwareMap map, Gyro gyro) { - lock = new GyroLock(new PID(0.01, 0, 0.005), gyro); - - frontRight = map.get(DcMotor.class, "right_front"); - backRight = map.get(DcMotor.class, "right_back"); - backLeft = map.get(DcMotor.class, "left_back"); - frontLeft = map.get(DcMotor.class, "left_front"); - - frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - } - - public void stop() { - frontLeft.setPower(0); - frontRight.setPower(0); - backLeft.setPower(0); - backRight.setPower(0); - } - - public void driveMechanum(Gamepad g) { - double pow = 1.0; - - if (g.left_bumper) { - pow = 0.5; - } - - double x = g.left_stick_x * pow; - double y = -g.left_stick_y * pow; - double turn = g.right_stick_x * pow; - - if (turn < TURN_ERROR) { - lock.teleopLoop(g); - turn = lock.correction(); - } - - frontRight.setPower(x - y + turn); - backRight.setPower(-x - y + turn); - backLeft.setPower(-x + y + turn); - frontLeft.setPower(x + y + turn); - } -} +// +//public class DriveTrain { +// public static final double TURN_ERROR = 0.1; +// +// GyroLock lock; +// DcMotor frontLeft, frontRight, backLeft, backRight; +// +// public DriveTrain(HardwareMap map, Gyro gyro) { +// lock = new GyroLock(new PID(0.01, 0, 0.005, -180, 180, -1, 1), gyro); +// +// frontRight = map.get(DcMotor.class, "right_front"); +// backRight = map.get(DcMotor.class, "right_back"); +// backLeft = map.get(DcMotor.class, "left_back"); +// frontLeft = map.get(DcMotor.class, "left_front"); +// +// frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); +// backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); +// backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); +// frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); +// } +// +// public void stop() { +// frontLeft.setPower(0); +// frontRight.setPower(0); +// backLeft.setPower(0); +// backRight.setPower(0); +// } +// +// public void driveMechanum(Gamepad g) { +// double pow = 1.0; +// +// if (g.left_bumper) { +// pow = 0.5; +// } +// +// double x = g.left_stick_x * pow; +// double y = -g.left_stick_y * pow; +// double turn = g.right_stick_x * pow; +// +// if (turn < TURN_ERROR) { +// lock.teleopLoop(g); +// turn = lock.correction(); +// } +// +// frontRight.setPower(x - y + turn); +// backRight.setPower(-x - y + turn); +// backLeft.setPower(-x + y + turn); +// frontLeft.setPower(x + y + turn); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java index 0b9cb8a..020f1ff 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java @@ -2,11 +2,11 @@ import com.qualcomm.robotcore.hardware.Servo; -import org.firstinspires.ftc.teamcode.Robot; +import org.firstinspires.ftc.teamcode.CompTeleop; /** * Created by Montclair Robotics on 11/13/17. - * @Author:Rafi + * @author Rafi * */ public class GlyphIntake2 { @@ -14,8 +14,8 @@ public class GlyphIntake2 { private double[] servoOpen;// = {, .751-.581, 0, 0}; private double[] servoClose;// = {0, 0, .926-.883, .974-.866}; - private final int[] topServos = {Robot.RIGHT_TOP,Robot.LEFT_TOP}; - private final int[] bottomServos = {Robot.RIGHT_BOTTOM,Robot.LEFT_BOTTOM}; + private final int[] topServos = {0,1}; + private final int[] bottomServos = {2,3}; private Servo[] servos; @@ -24,15 +24,15 @@ public GlyphIntake2(Servo... servos) { servoOpen=new double[4]; servoClose=new double[4]; - servoOpen[Robot.RIGHT_TOP] = 0.573; - servoOpen[Robot.LEFT_TOP] = 0.449; - servoOpen[Robot.RIGHT_BOTTOM] = 0.450; - servoOpen[Robot.LEFT_BOTTOM] = 0.600; + servoOpen[CompTeleop.SERVORT] = 0.78; //from 11/30 + servoOpen[CompTeleop.SERVOLT] = 0.4; //from 11/30 + servoOpen[CompTeleop.SERVORB] = 0.44; //from 11/30 + servoOpen[CompTeleop.SERVOLB] = 0.5; //from 11/30 - servoClose[Robot.RIGHT_TOP] = 0.440; - servoClose[Robot.LEFT_TOP] = 0.560; - servoClose[Robot.RIGHT_BOTTOM] = 0.560; - servoClose[Robot.LEFT_BOTTOM] = 0.530; + servoClose[CompTeleop.SERVORT] = 0.45; //from 11/30 + servoClose[CompTeleop.SERVOLT] = 0.5; //from 11/30 + servoClose[CompTeleop.SERVORB] = 0.55; //from 11/30 + servoClose[CompTeleop.SERVOLB] = 0.37; //from 11/30 } public void closeTop() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/AllianceColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/AllianceColor.java similarity index 64% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/AllianceColor.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/AllianceColor.java index 6e908de..3352ca0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/AllianceColor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/AllianceColor.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto.Enums; +package org.firstinspires.ftc.teamcode.Enums; /** * Created by MHS Robotics on 11/8/2017. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/JewelColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/JewelColor.java new file mode 100644 index 0000000..dd8c06d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/JewelColor.java @@ -0,0 +1,9 @@ +package org.firstinspires.ftc.teamcode.Enums; + +/** + * Created by Montclair Robotics on 11/29/2017. + */ + +public enum JewelColor { + RED,BLUE,UNKNOWN +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/PictogramResults.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/PictogramResults.java similarity index 71% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/PictogramResults.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/PictogramResults.java index 859d341..2c0d803 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/PictogramResults.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/PictogramResults.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto.Enums; +package org.firstinspires.ftc.teamcode.Enums; /** <<<<<<< HEAD diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/StartPosition.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/StartPosition.java similarity index 62% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/StartPosition.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/StartPosition.java index 20fdc85..ec0a8a6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/StartPosition.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/StartPosition.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto.Enums; +package org.firstinspires.ftc.teamcode.Enums; /** * Created by Will on 11/13/17. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java index 07c66d1..8db3d41 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -10,18 +10,13 @@ /** * Created by Montclair Robotics on 11/13/17. - * @Author:Jack * */ -public class Gyro implements Input, Updatable { - double x, y, z; - +public class Gyro { private RRQuaternion quat; // An angle object to store the gyro angles private BNO055IMU imu; // Gyroscope public Gyro(HardwareMap map) { - x = y = z = 0; - imu = map.get(BNO055IMU.class, "gyro"); BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); // Create a new parameter object for the gyro @@ -31,22 +26,11 @@ public Gyro(HardwareMap map) { parameters.loggingTag = "IMU"; // set the logging tag imu.initialize(parameters); - imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, 6); - - update(); - Updater.add(this, Priority.INPUT); - } + //imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, 6); - @Override - public void update() { - quat = new RRQuaternion(imu.getQuaternionOrientation()); - x = (double) ((int) (10*quat.getX())) / 10; - y = (double) ((int) (10*quat.getY())) / 10; - z = (double) ((int) (10*quat.getZ())) / 10; } - @Override - public Double get() { - return quat.getX(); + public RRQuaternion get() { + return new RRQuaternion(imu.getQuaternionOrientation()); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java index 1d19d78..a98abe4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java @@ -2,61 +2,57 @@ import com.qualcomm.robotcore.hardware.Gamepad; -import org.firstinspires.ftc.teamcode.Components.DriveTrain; +//import org.firstinspires.ftc.teamcode.Components.DriveTrain; import org.montclairrobotics.sprocket.utils.Debug; /** * Created by Joshua Rapoport on 11/16/17. */ - -public class GyroLock { - private Gyro gyro; - private PID pid; - private boolean active; - - public GyroLock(PID pid, Gyro gyro) { - this.gyro = gyro; - this.pid = pid; - this.active = false; - - pid.setInputRange(-180, 180); - pid.setOutputRange(-1.0, 1.0); - } - - public double correction() { - return pid.getOutput(); - } - - public void teleopLoop(Gamepad g) { - if (Math.abs(g.right_stick_x) < DriveTrain.TURN_ERROR) { - // If the gyro-lock wasn't active before... - if (!active) { - // Reset the error record. - pid.error.reset(); - // Set the target to the current trajectory. - pid.setTarget(gyro.get()); - } - - // It sure is active now. - active = true; - // Run update loop. - loop(); - } else { - // DENIED!!! - active = false; - } - } - - public void autoLoop() { - //... - } - - public void loop() { - gyro.update(); - pid.setInput(gyro.get()); - pid.update(); - - Debug.msg("PID Input", pid.getInput().intValue() + "°"); - Debug.msg("PID Output", (int) (100 * pid.getOutput()) + "%"); - } -} +// +//public class GyroLock { +// private Gyro gyro; +// private PID pid; +// private boolean active; +// +// public GyroLock(PID pid, Gyro gyro) { +// this.gyro = gyro; +// this.pid = pid; +// this.active = false; +// } +// +// public double correction() { +// return pid.getOutput(); +// } +// +// public void teleopLoop(Gamepad g) { +// if (Math.abs(g.right_stick_x) < DriveTrain.TURN_ERROR) { +// // If the gyro-lock wasn't active before... +// if (!active) { +// // Reset the error record. +// pid.error.reset(); +// // Set the target to the current trajectory. +// pid.setTarget(gyro.get().getX()); +// } +// +// // It sure is active now. +// active = true; +// // Run update loop. +// loop(); +// } else { +// // DENIED!!! +// active = false; +// } +// } +// +// public void autoLoop() { +// //... +// } +// +// public void loop() { +// pid.setInput(gyro.get().getX()); +// pid.update(); +// +// Debug.msg("PID Input", pid.getInput().intValue() + "°"); +// Debug.msg("PID Output", (int) (100 * pid.getOutput()) + "%"); +// } +//} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java index b9c7848..e688ce2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java @@ -1,177 +1,60 @@ package org.firstinspires.ftc.teamcode; +import org.montclairrobotics.sprocket.utils.Input; + /** * Created by Joshua Rapoport on 11/14/17. */ public class PID { - public class Range { - double min, max; + double P,I,D; + double minIn,maxIn; + double minOut,maxOut; - public Range() { - this.min = this.max = 0; - } - public Range(double a, double b) { - this.min = a; - this.max = b; - } + double target; + double lastError; + double currentError; + double rateError; + double totalError; + long lastUpdateTimeMillis; - public double distance() { - return Math.abs(max - min); - } - public boolean contains(double x) { - return x >= min && x <= max; - } - public int compareTo(double x) { - if (contains(x)) - return 0; - else if (x < min) - return -1; - else - return 1; - } - } - - public class Error { - double current, total; - public Error() { - reset(); - } - public void update() { - total += current * dTime(); - } - public void reset() { current = total = 0; } + public PID(double p,double i, double d,double minIn,double maxIn, double minOut,double maxOut) + { + P=p; + I=i; + D=d; + this.minIn=minIn; this.maxIn = maxIn; + this.minOut=minOut; this.maxOut = maxOut; + target=0.0; + totalError=currentError = rateError= 0.0; + lastUpdateTimeMillis=System.currentTimeMillis(); } - private double P, I, D; - /** The acceptable range of input. */ - protected Range inRange; - /** The acceptable range of output. */ - protected Range outRange; - /** An object that handles error due to input-target difference. */ - protected Error error; - - /** The input from the robot, to be compared with the target. */ - private Double input; - /** The robot's target, to be compared with the input. */ - private double target; - private double output; - - private long lastUpdateTime; - - /** - * @param p the proportionality constant. - * @param i the integral constant. - * @param d the derivative constant. - */ - public PID(double p, double i, double d) { - this.P = p; - this.I = i; - this.D = d; - - this.inRange = new Range(); - this.outRange = new Range(); - - this.error = new Error(); - - this.input = this.target = this.output = 0.0; - - update(); + public void setTarget(double t) + { + target=t; } - public double getOutput() { - return output; - } - private double setOutput(double newInput) { - error.current = target - newInput; - double dInput = newInput - input; + public double get(double in) { + double dt = System.currentTimeMillis() - lastUpdateTimeMillis; + lastUpdateTimeMillis = System.currentTimeMillis(); - double diff = inRange.distance(); - if (diff != 0) { - error.current = ((error.current - inRange.min) % diff + diff) % diff + inRange.min; - dInput = ((dInput - inRange.min) % diff + diff) % diff + inRange.min; - } - error.update(); + double diff=maxIn-minIn; + double currentError = ((target-in-minIn)%diff+diff)%diff+minIn; + double rateError = (dt > 0) ? (lastError - currentError) / dt : 0; + lastError=currentError; if (I != 0) { - double potentialI = (error.current + error.total) * I; - if (outRange.compareTo(potentialI) > 0) - error.total = outRange.max / I; - else if (outRange.compareTo(potentialI) < 0) - error.total = outRange.min / I; + double potentialI = (totalError + lastError) * I; + if (potentialI > maxOut) + totalError = maxOut / I; + else if (potentialI < minOut) + totalError = minOut / I; else - error.total += error.current; + totalError += currentError * dt; } - double out = (P * error.current * dTime()) + (I * error.total) + (D * -dInput / dTime()); - - setInput(newInput); - - if (outRange.compareTo(out) > 0) - out = outRange.max; - else if (outRange.compareTo(out) < 0) - out = outRange.min; - - return out; - } - - /** @return a reference to a different PID object with identical properties */ - public PID copy() { - PID c = new PID(P, I, D); - c.setInputRange(inRange.min, inRange.max); - c.setOutputRange(outRange.min, outRange.max); - c.setTarget(target); - - return c; - } - - public void setPID(double p, double i, double d) { - P = p; - I = i; - D = d; - } - /** @return P, the proportional constant. */ - public double getP() { - return P; - } - /** @return I, the integral constant. */ - public double getI() { - return I; - } - /** @return D, the derivative constant. */ - public double getD() { - return D; - } - - public void setInputRange(double a, double b) { - this.inRange = new Range(a, b); - } - public void setOutputRange(double a, double b) { - this.outRange = new Range(a, b); - } - - public Double getInput() { - return input; - } - - /** Use this method to get a new output value. */ - public void setInput(double i) { - this.output = setOutput(i); - this.input = i; - } - - public double getTarget() { - return target; - } - public void setTarget(double t) { this.target = target; } - - /** @return the time difference from the last update. */ - public double dTime() { - return System.currentTimeMillis() - lastUpdateTime; - } - - public void update() { - lastUpdateTime = System.currentTimeMillis(); + return (P * currentError) + (I * totalError) + (D * rateError); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java deleted file mode 100644 index 90ebf88..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java +++ /dev/null @@ -1,305 +0,0 @@ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DigitalChannel; -import com.qualcomm.robotcore.hardware.Servo; - -import org.firstinspires.ftc.teamcode.Components.GlyphIntake2; -import org.firstinspires.ftc.teamcode.Components.GlyphLift; -import org.montclairrobotics.sprocket.actions.Action; -import org.montclairrobotics.sprocket.control.BasicInput; -import org.montclairrobotics.sprocket.control.JoystickXAxis; -import org.montclairrobotics.sprocket.control.JoystickYAxis; -import org.montclairrobotics.sprocket.core.Button; -import org.montclairrobotics.sprocket.core.Sprocket; -import org.montclairrobotics.sprocket.drive.ControlledMotor; -import org.montclairrobotics.sprocket.drive.DTTarget; -import org.montclairrobotics.sprocket.drive.DriveModule; -import org.montclairrobotics.sprocket.drive.DriveTrain; -import org.montclairrobotics.sprocket.drive.NewMecanum; -import org.montclairrobotics.sprocket.drive.steps.Deadzone; -import org.montclairrobotics.sprocket.drive.steps.Sensitivity; -import org.montclairrobotics.sprocket.drive.steps.Squared; -import org.montclairrobotics.sprocket.ftc.FTCButton; -import org.montclairrobotics.sprocket.ftc.FTCJoystick; -import org.montclairrobotics.sprocket.ftc.FTCMotor; -import org.montclairrobotics.sprocket.ftc.FTCRobot; -import org.montclairrobotics.sprocket.geometry.Vector; -import org.montclairrobotics.sprocket.geometry.XY; -import org.montclairrobotics.sprocket.pipeline.Pipeline; -import org.montclairrobotics.sprocket.pipeline.Step; -import org.montclairrobotics.sprocket.utils.Debug; -import org.montclairrobotics.sprocket.utils.Input; - -import java.util.ArrayList; - -//import org.montclairrobotics.sprocket.drive.DefaultMecanumMapper; - -@TeleOp(name="Sprocket Teleop", group="147") -@Disabled -public class Robot extends FTCRobot { - Gyro gyro; - - FTCMotor frontRight, backRight, frontLeft, backLeft; - FTCMotor liftLeft, liftRight; - GlyphLift lift; - GlyphIntake2 intake; - DigitalChannel limitSwitch; - Servo[] servos; - - public static int RIGHT_TOP=0; - public static int LEFT_TOP = 1; - public static int RIGHT_BOTTOM = 2; - public static int LEFT_BOTTOM=3; - - @Override - public void setup() { - frontRight = new FTCMotor("right_front"); - frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - backRight = new FTCMotor("right_back"); - backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - backLeft = new FTCMotor("left_back"); - backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - frontLeft = new FTCMotor("left_front"); - frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); - limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); - - servos = new Servo[4]; - servos[0] = hardwareMap.get(Servo.class, "intake_right_top"); - servos[1] = hardwareMap.get(Servo.class, "intake_left_top"); - servos[2] = hardwareMap.get(Servo.class, "intake_right_bottom"); - servos[3] = hardwareMap.get(Servo.class, "intake_left_bottom"); - - liftLeft = new FTCMotor("lift_left"); - liftRight = new FTCMotor("lift_right"); - lift = new GlyphLift(liftLeft, liftRight); - intake=new GlyphIntake2(servos); - limitSwitch.setMode(DigitalChannel.Mode.INPUT); - - gyro = new Gyro(hardwareMap); - final DriveModule[] modules = new DriveModule[4]; - - //Mecanum - modules[0] = new DriveModule(new XY(1, 1), new XY(1,-1), frontRight); - modules[1] = new DriveModule(new XY(1, -1), new XY(-1,-1), backRight ); - modules[2] = new DriveModule(new XY(-1, -1), new XY(-1,1), backLeft ); - modules[3] = new DriveModule(new XY(-1, 1), new XY(1,1), frontLeft ); - - - DriveTrain driveTrain = new DriveTrain(modules); - //angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.YXZ, AngleUnit.DEGREES); - - /* - AutoBalance autoBalance = new AutoBalance( - new PID(), - new PID(), - new Input() { - @Override - public Double get() { - return (double)angles.thirdAngle; - } - }, - new Input() { - @Override - public Double get() { - return (double)angles.secondAngle; - } - } - );*/ - - ArrayList> steps = new ArrayList<>(); - steps.add(new Deadzone(0.1, 0.1)); - final Sensitivity sensitivity=new Sensitivity(1,0.6); - steps.add(sensitivity); - steps.add(new Squared()); - /* - GyroCorrection gyroCorrection = new GyroCorrection( - new Input() { - @Override - public Double get() { - return (double) -angles.thirdAngle; - } - }, - - new PID(.45, 0, 0).setMinMax(-180,180,-.5,.5), 1000, 1); - - steps.add(gyroCorrection);*/ - //steps.add(autoBalance); - driveTrain.setPipeline(new Pipeline<>(steps)); - - final Input fullJoystick=new FTCJoystick(GAMEPAD.A, FTCJoystick.STICK.LEFT); - final Input halfJoystick=new FTCJoystick(GAMEPAD.A, FTCJoystick.STICK.DPAD); - - driveTrain.setMapper(new NewMecanum()); - driveTrain.setDefaultInput(new BasicInput(new Input(){ - @Override - public Vector get() { - return halfJoystick.get().scale(0.7).add(fullJoystick.get()); - } - }, - new JoystickXAxis(new FTCJoystick(GAMEPAD.A, FTCJoystick.STICK.RIGHT)))); - - - //GyroLock lock = new GyroLock(gyroCorrection); - //new Button(new FTCButton(GAMEPAD.A, FTCButton.BUTTON.y)).setAction(autoBalance); - - //Gyro Lock - //new Button(new FTCButton(GAMEPAD.A, FTCButton.BUTTON.a)).setAction(lock); - - //Lift Up -// new Button(new FTCButton(GAMEPAD.B, FTCButton.BUTTON.dpad_up)).setAction(new Action() { -// @Override -// public void start() { -// lift.set(1); -// } -// @Override -// public void enabled() { -// -// } -// @Override -// public void stop() { -// lift.set(0); -// } -// @Override -// public void disabled() { -// -// } -// }); -// -// //Lift Down -// new Button(new FTCButton(GAMEPAD.B, FTCButton.BUTTON.dpad_down)).setAction(new Action(){ -// @Override -// public void start() { -// lift.set(-1); -// } -// @Override -// public void enabled() { -// -// } -// @Override -// public void stop() { -// lift.set(0); -// } -// @Override -// public void disabled() { -// -// } -// }); - - new ControlledMotor(liftLeft, new JoystickYAxis(new FTCJoystick(GAMEPAD.B, FTCJoystick.STICK.LEFT)).negate()); - new ControlledMotor(liftRight, new JoystickYAxis(new FTCJoystick(GAMEPAD.B, FTCJoystick.STICK.LEFT)).negate()); - - //Top Servos - new Button(new FTCButton(GAMEPAD.B, FTCButton.BUTTON.b)).setAction(new Action(){ - @Override - public void start() { - } - - @Override - public void enabled() { - intake.openTop(); - } - - @Override - public void stop() { - } - - @Override - public void disabled() { - intake.closeTop(); - } - }); - //Bottom Servos - new Button(new FTCButton(GAMEPAD.B, FTCButton.BUTTON.a)).setAction(new Action(){ - @Override - public void start() { - } - - @Override - public void enabled() { - intake.openBottom(); - } - - @Override - public void stop() { - } - - @Override - public void disabled() { - intake.closeBottom(); - } - }); - - //Half speed - new Button(new FTCButton(GAMEPAD.A, FTCButton.BUTTON.left_bumper)).setAction(new Action(){ - @Override - public void start() { - sensitivity.dirScale=0.5; - sensitivity.turnScale=0.3; - } - - @Override - public void enabled() { - - } - - @Override - public void stop() { - sensitivity.dirScale=1; - sensitivity.turnScale=0.6; - } - - @Override - public void disabled() { - - } - }); - - } - - @Override - public void enableMode(Sprocket.MODE mode) { - } - - @Override - public void teleopInit() { - intake.openAll(); - } - - @Override - public void autoInit() { - - } - - @Override - public void testInit() { - - } - - @Override - public void disable() { - - } - - @Override - public void update() { - Debug.msg("X-angle", gyro.x); - Debug.msg("Y-angle", gyro.y); - Debug.msg("Z-angle", gyro.z); - - //Debug.msg("Servo",intakeRightTop.getPosition()); - Debug.msg("Switch Value:", limitSwitch.getState()); - } - - @Override - public void disabledUpdate() { - - } - - @Override - public void debugs() { - - } -}