From 98caa8c55a422e6fcb82324c3a9c791c5d455663 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Tue, 28 Nov 2017 12:53:31 -0500 Subject: [PATCH 01/19] "Auto re-tuning as well as changes for modern robotics sensor from rev color prox, all file changes are auto." - Will :) --- .../ftc/teamcode/Auto/DefaultAuto.java | 40 ++++++---- .../ftc/teamcode/Auto/DefaultAutoMode.java | 24 +++--- .../ftc/teamcode/Auto/DefaultHardwareMap.java | 4 +- .../ftc/teamcode/Auto/StateMachine/State.java | 63 --------------- .../Auto/StateMachine/StateMachine.java | 77 ------------------- .../teamcode/Auto/StateMachine/TestState.java | 42 ---------- .../ftc/teamcode/Auto/TestColor.java | 17 ++-- .../ftc/teamcode/Auto/TestJewelID.java | 8 -- .../ftc/teamcode/Auto/TestVuforia.java | 1 + 9 files changed, 55 insertions(+), 221 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/State.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/StateMachine.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/TestState.java 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..d6ec690 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() { @@ -25,39 +25,53 @@ public void loop() { break; case 1: // raise glyph + hardware.lift.closeAll(); nextState(setGlyphLiftPos(5,1)); break; case 2: //get jewel + hardware.lift.closeAll(); nextState(getJewel()); break; + 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)); - case 3: // drive forward 24 + 2 (to get off balancing stone) - nextState(autoDrive(new XY(0,26),1)); + case BLUE: + nextState(autoDrive(new XY(0,-26),1)); + } 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 + case 7: //pictogram drive + hardware.lift.closeAll(); nextState(pictogramDrive(pictogram)); break; - case 8: // drive into crypto box slot slowly - nextState(autoDrive(new XY(0,12),0.25)); + case 8: //input glyph + hardware.lift.closeAll(); + nextState(autoDrive(new XY(0,6),1)); break; case 9: // lower glyph + hardware.lift.closeAll(); nextState(setGlyphLiftPos(-5,1)); break; @@ -66,11 +80,11 @@ public void loop() { nextState(pause(1)); break; - case 11: // back away from glyph - nextState(autoDrive(new XY(0,-2),1)); + case 11: //back away + nextState(autoDrive(new XY(0,2),1)); break; - case 12: // telemetry + case 12: //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..b05992a 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 @@ -33,7 +33,7 @@ public class DefaultAutoMode extends OpMode{ PictogramResults pictogram; //Color Prox Vars - ColorSensor sensorColor; + ColorSensor colorSensor; AllianceColor color; //auto mode objects @@ -62,7 +62,7 @@ public void autoInit(){ // 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(); @@ -125,10 +125,10 @@ public boolean pictogramDrive(PictogramResults image){ } //colorProx - private boolean getJewelColor(){ - if(sensorColor.red() > sensorColor.blue()){ + public boolean getJewelColor(){ + if(colorSensor.red() > colorSensor.blue()){ color = AllianceColor.RED; - }else if(sensorColor.blue()> sensorColor.red()){ + }else if(colorSensor.blue()> colorSensor.red()){ color = AllianceColor.BLUE; }else{ telemetry.addData("INFO","ID FAILED"); @@ -175,7 +175,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 +185,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,7 +206,6 @@ public boolean autoTurn(double degrees, double speed) { } return false; } - public boolean driveTurn(){ switch (startPosition){ case CLOSE: @@ -217,8 +214,16 @@ public boolean driveTurn(){ case FAR: return autoTurn(-90,0.5); } + 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),0.5); + } return false; } public boolean cryptoBoxTurn(){ @@ -243,7 +248,6 @@ public boolean cryptoBoxTurn(){ } break; } - telemetry.addData("INFO","Alliance Color or Start Position NOT defined"); return false; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java index 5b4d80f..d6b925b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/DefaultHardwareMap.java @@ -37,7 +37,7 @@ public class DefaultHardwareMap { public static int LEFT_BOTTOM=3; GlyphIntake2 lift; - ColorSensor sensorColor; + ColorSensor colorSensor; DigitalChannel limitSwitch; HardwareMap hwMap = null; @@ -75,7 +75,7 @@ public void init(HardwareMap ahwMap){ servos[3] = ahwMap.get(Servo.class, "intake_left_bottom"); lift=new GlyphIntake2(servos); - sensorColor = hwMap.get(ColorSensor.class, "colorSensor"); + colorSensor = hwMap.get(ColorSensor.class, "sensor_color"); limitSwitch = hwMap.get(DigitalChannel.class, "limit_switch_1"); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/State.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/State.java deleted file mode 100644 index 536ffb1..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/State.java +++ /dev/null @@ -1,63 +0,0 @@ -package org.firstinspires.ftc.teamcode.Auto.StateMachine; - -/** - * @author MHS Robotics - * - * The State class is designed to be passed in to a state machine. - * - * Every state has methods that determines what to be run when the state is running - * For each states you can define a start, stop, run, and isDone method to control how the state is run - * - */ - -public abstract class State { - - Integer nextState; - - - /** - * By default, states run in a linear fashion running one after another, if the user desires to - * have the state go somewhere other then the next linear state, they can define one in the - * Constructor - */ - public State(int nextState){ - this.nextState = nextState; - } - - public State(){} - - /** - * First thing called when the state is run, - * This is where any Motor powers, initialization or any other setup should be run - * This is called ONCE when the state starts - */ - public abstract void start(); - - /** - * This method is run once every loop while the state is running - * This is where the actual implementation of the state should be run - * Any logic, motor positions, motor powers and anything else to be run should be run here - * This is called EVERY LOOP until the auto mode is done - */ - public abstract void run(); - - /** - * Run one time after the state is done - * This is where any braking, encoder resets, motor resets and any cleanup should be done - * This is called ONCE at the end of the state - */ - public abstract void stop(); - - /** - * Boolean to check weather the state is done - */ - public abstract boolean isDone(); - - public int getNextState(int currentState){ - if(nextState != null){ - return nextState; - } - return currentState + 1; - } - -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/StateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/StateMachine.java deleted file mode 100644 index 90e2c1a..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/StateMachine.java +++ /dev/null @@ -1,77 +0,0 @@ -package org.firstinspires.ftc.teamcode.Auto.StateMachine; - -import com.qualcomm.robotcore.eventloop.opmode.OpMode; - -import org.firstinspires.ftc.robotcore.external.Telemetry; - -/** - * @author MHS Robotics - * - * State machines are designed to be run in a State Machine Auto - * @see org.firstinspires.ftc.teamcode.Auto.StateMachineAuto - * - * StateMachine is a state itsef so State Machines can be run inside of other State Machines, - * This allows the user to re-use code for actions that are run often in autonomous. - * - * @see #run() - */ - -public class StateMachine extends State { - - State[] states; - Telemetry telemetry; - String name; - int state; - boolean stateStarted = false; - int finalState; - boolean done = false; - - - public StateMachine(String name, int finalstate, Telemetry telemetry, State ... states){ - this.states = states; - this.telemetry = telemetry; - this.name = name; - this.finalState = finalstate; - } - - @Override - public void start() { - telemetry.addData("Running", name); - } - - @Override - public void run() { - - State currentState = states[state]; - - //Check If the state has started, if it hasn't run the 'start()' method - if (!stateStarted) { - currentState.start(); - stateStarted = true; // State has been started - } - - states[state].run(); - - if(currentState.isDone()){ - currentState.stop(); - if(currentState.getNextState(state) == finalState){ - done = true; - return; - } - if(currentState.getNextState(state) < states.length) { - state = currentState.getNextState(state); - } - stateStarted = false; - } - } - - @Override - public void stop() { - telemetry.addData("Status", name + ", Finished "); - } - - @Override - public boolean isDone() { - return done; - } -} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/TestState.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/TestState.java deleted file mode 100644 index 7ac3861..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/StateMachine/TestState.java +++ /dev/null @@ -1,42 +0,0 @@ -package org.firstinspires.ftc.teamcode.Auto.StateMachine; - -/** - * Created by thegb on 11/15/2017. - */ - -public class TestState extends State { - - /** - * By default, states run in a linear fashion running one after another, if the user desires to - * have the state go somewhere other then the next linear state, they can define one in the - * Constructor - * - * @param nextState indicates what state the state machine will go to after the current state is finished - */ - public TestState(int nextState) { - super(nextState); - } - - public TestState() { - } - - @Override - public void start() { - - } - - @Override - public void run() { - - } - - @Override - public void stop() { - - } - - @Override - public boolean isDone() { - return false; - } -} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java index d50e9f3..ca9f6f6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java @@ -1,6 +1,9 @@ package org.firstinspires.ftc.teamcode.Auto; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.ColorSensor; /** * Created by Montclair Robotics on 11/13/17. @@ -8,17 +11,19 @@ * */ @Autonomous(name = "Test: Color Sensor") public class TestColor extends DefaultAutoMode { + @Override public void init() { - autoInit(); + colorSensor.enableLed(false); + } + + @Override + public void start() { + colorSensor.enableLed(true); } @Override public void loop() { - telemetry.addData("Red", sensorColor.red()); - telemetry.addData("Blue", sensorColor.blue()); - telemetry.addData("Green", sensorColor.green()); - telemetry.addData("Alpha", sensorColor.alpha()); - telemetry.addData("aRGB", sensorColor.argb()); + getJewelColor(); } } 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..db35858 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 @@ -25,16 +25,8 @@ 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: - telemetry.addData("INFO",LSA); } } } 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() { From b69d2446f4c68ba6cda0f67731a2a98c15a9053e Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Wed, 29 Nov 2017 16:36:20 -0500 Subject: [PATCH 02/19] "Pleas commit will, we have a Stable, working thing going on" - jack --- .../ftc/teamcode/Auto/DefaultAuto.java | 4 +- .../ftc/teamcode/Auto/DefaultAutoMode.java | 14 +++- .../ftc/teamcode/Auto/Enums/JewelColor.java | 9 +++ .../ftc/teamcode/Auto/IHAteLIFE.java | 79 +++++++++++++++++++ .../ftc/teamcode/Auto/TestColor.java | 2 +- .../ftc/teamcode/Auto/TestColorSensor.java | 24 ++++++ .../ftc/teamcode/Auto/TestColorSensorAvg.java | 46 +++++++++++ .../ftc/teamcode/Auto/TestColorServoSlow.java | 8 ++ .../ftc/teamcode/Auto/TestVuforia.java | 1 - .../ftc/teamcode/CompTeleop.java | 5 +- 10 files changed, 183 insertions(+), 9 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/JewelColor.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/IHAteLIFE.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensor.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensorAvg.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorServoSlow.java 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 d6ec690..2a897f7 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 @@ -26,7 +26,7 @@ public void loop() { case 1: // raise glyph hardware.lift.closeAll(); - nextState(setGlyphLiftPos(5,1)); + nextState(raiseGlyph()); break; case 2: //get jewel @@ -72,7 +72,7 @@ public void loop() { case 9: // lower glyph hardware.lift.closeAll(); - nextState(setGlyphLiftPos(-5,1)); + nextState(lowerGlyph()); break; case 10: //release glyph 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 b05992a..5082c81 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 @@ -59,7 +59,6 @@ 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(); colorSensor = hardware.colorSensor; @@ -134,6 +133,9 @@ public boolean getJewelColor(){ telemetry.addData("INFO","ID FAILED"); return false; } + telemetry.addData("RED",colorSensor.red()); + telemetry.addData("BLUE",colorSensor.blue()); + telemetry.addData("BLUE MINUS RED",colorSensor.blue()-colorSensor.red()); telemetry.addData("Jewel Color", color); return true; } //pt1 @@ -257,7 +259,7 @@ public void glyphLiftPower(double 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() colorSensor.blue()){ + jewelColor = JewelColor.RED; + done = true; + }else if(colorSensor.blue()> colorSensor.red()){ + jewelColor = JewelColor.BLUE; + done = true; + }else{ + done=System.currentTimeMillis()-startTime>5000; + jewelColor = JewelColor.UNKNOWN; + } + + if(done){ + x++; + } + break; + + case 2: + switch (jewelColor){ + case UNKNOWN: + jewelArm.setPosition(autoMode.JEWEL_ARM_UP_POS); + break; + + case BLUE: + telemetry.addData("COLOR",jewelColor); + break; + + case RED: + telemetry.addData("COLOR",jewelColor); + break; + } + } + + + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java index ca9f6f6..7f30a0c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java @@ -14,7 +14,7 @@ public class TestColor extends DefaultAutoMode { @Override public void init() { - colorSensor.enableLed(false); + autoInit(); } @Override diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensor.java new file mode 100644 index 0000000..8bcc082 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensor.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.ColorSensor; + +/** + * Created by Montclair Robotics on 11/29/2017. + */ +@Autonomous(name="testColorSensor") +public class TestColorSensor extends OpMode { + ColorSensor colorSensor; + @Override + public void init() { + colorSensor = hardwareMap.get(ColorSensor.class, "sensor_color"); + } + + @Override + public void loop() { + telemetry.addData("RED",colorSensor.red()); + telemetry.addData("BLUE",colorSensor.blue()); + telemetry.addData("BLUE MINUS RED",colorSensor.blue()-colorSensor.red()); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensorAvg.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensorAvg.java new file mode 100644 index 0000000..cfb0154 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensorAvg.java @@ -0,0 +1,46 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.ColorSensor; + +/** + * Created by Montclair Robotics on 11/29/2017. + */ + +@TeleOp(name="testColorSensor Average") +public class TestColorSensorAvg extends OpMode +{ + int total; + int samples; + ColorSensor colorSensor; + @Override + public void init() { + colorSensor = hardwareMap.get(ColorSensor.class, "sensor_color"); + } + + @Override + public void loop() { + telemetry.addData("RED", colorSensor.red()); + telemetry.addData("BLUE", colorSensor.blue()); + telemetry.addData("BLUE MINUS RED", colorSensor.blue() - colorSensor.red()); + if(gamepad1.x) + { + total+=colorSensor.blue()-colorSensor.red(); + samples++; + } + if(gamepad1.y) + { + total=0; + samples=0; + } + if(samples>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/TestColorServoSlow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorServoSlow.java new file mode 100644 index 0000000..cd16fbc --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorServoSlow.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.teamcode.Auto; + +/** + * Created by Montclair Robotics on 11/29/2017. + */ + +public class TestColorServoSlow { +} 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 06569a8..df22a44 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,7 +8,6 @@ * @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..e327a39 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -28,7 +28,6 @@ public class CompTeleop extends OpMode { @Override public void init() { - gyro = new Gyro(hardwareMap); this.driveTrain = new DriveTrain(hardwareMap, gyro); liftA = hardwareMap.get(DcMotor.class,"lift_left"); @@ -41,7 +40,7 @@ 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"); } @@ -62,7 +61,7 @@ public void loop() { liftA.setPower(gamepad2.left_stick_y); liftB.setPower(-gamepad2.left_stick_y); - telemetry.addData("Orientation", gyro.x + "°"); +// telemetry.addData("Orientation", gyro.x + "°"); telemetry.addData("Limit Switch", limitSwitch); } } From d73347eee5f5bf7d763c931296c7129bf5fc45c1 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Wed, 29 Nov 2017 16:38:27 -0500 Subject: [PATCH 03/19] Actually this --- .../ftc/teamcode/CompTeleop.java | 25 ++++++++++++++++--- 1 file changed, 21 insertions(+), 4 deletions(-) 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 e327a39..4af37dc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -15,11 +15,11 @@ * */ @TeleOp(name="Teleop: PLEASE DON'T DELETE THIS WILL") public class CompTeleop extends OpMode { - public DriveTrain driveTrain; + //public DriveTrain driveTrain; DcMotor frontRight, backRight, frontLeft, backLeft; Servo[] servos; - Gyro gyro; +// Gyro gyro; GlyphIntake2 intake; @@ -28,7 +28,7 @@ public class CompTeleop extends OpMode { @Override public void init() { - this.driveTrain = new DriveTrain(hardwareMap, gyro); + //this.driveTrain = new DriveTrain(hardwareMap); liftA = hardwareMap.get(DcMotor.class,"lift_left"); liftB = hardwareMap.get(DcMotor.class,"lift_right"); @@ -46,7 +46,24 @@ public void init() { @Override public void loop() { - driveTrain.driveMechanum(gamepad1); + //driveTrain.driveMechanum(gamepad1); + + + double pow = 1.0; + + if (gamepad1.left_bumper) { + pow = 0.5; + } + + double x = gamepad1.left_stick_x * pow; + double y = -gamepad1.left_stick_y * pow; + double turn = gamepad1.right_stick_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) intake.openBottom(); From 368697aae7f7006b564b750de22caf8cb6be77ae Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Wed, 29 Nov 2017 16:57:37 -0500 Subject: [PATCH 04/19] End of today --- .../ftc/teamcode/Auto/DefaultAutoMode.java | 11 +++++------ .../ftc/teamcode/Auto/IHAteLIFE.java | 19 ++++--------------- .../ftc/teamcode/CompTeleop.java | 10 ++++++++++ 3 files changed, 19 insertions(+), 21 deletions(-) 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 5082c81..50e7d5b 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 @@ -10,6 +10,7 @@ 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.JewelColor; import org.firstinspires.ftc.teamcode.Auto.Enums.PictogramResults; import org.firstinspires.ftc.teamcode.Auto.Enums.StartPosition; import org.firstinspires.ftc.teamcode.Gyro; @@ -125,14 +126,12 @@ public boolean pictogramDrive(PictogramResults image){ //colorProx public boolean getJewelColor(){ - if(colorSensor.red() > colorSensor.blue()){ - color = AllianceColor.RED; + /*if(colorSensor.red() > colorSensor.blue()){ + jewelColor = JewelColor.RED; }else if(colorSensor.blue()> colorSensor.red()){ - color = AllianceColor.BLUE; + jewelColor = JewelColor.BLUE; }else{ - telemetry.addData("INFO","ID FAILED"); - return false; - } + jewelColor = JewelColor.UNKNOWN;*/ telemetry.addData("RED",colorSensor.red()); telemetry.addData("BLUE",colorSensor.blue()); telemetry.addData("BLUE MINUS RED",colorSensor.blue()-colorSensor.red()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/IHAteLIFE.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/IHAteLIFE.java index b4c51cc..55405f1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/IHAteLIFE.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/IHAteLIFE.java @@ -15,10 +15,10 @@ public class IHAteLIFE extends OpMode { ColorSensor colorSensor; Servo jewelArm; - DefaultAutoMode autoMode; JewelColor jewelColor; int x = 0; + int colorSum; int y = 0; boolean done = false; double startTime; @@ -33,8 +33,8 @@ public void init() { public void loop() { switch(x){ case 0: - jewelArm.setPosition(autoMode.JEWEL_ARM_DOWN_POS); - if(jewelArm.getPosition()==autoMode.JEWEL_ARM_DOWN_POS){ + jewelArm.setPosition(1); + if(jewelArm.getPosition()==1){ x++; startTime=System.currentTimeMillis(); } @@ -58,19 +58,8 @@ public void loop() { break; case 2: - switch (jewelColor){ - case UNKNOWN: - jewelArm.setPosition(autoMode.JEWEL_ARM_UP_POS); - break; + telemetry.addData("COLOR",jewelColor); - case BLUE: - telemetry.addData("COLOR",jewelColor); - break; - - case RED: - telemetry.addData("COLOR",jewelColor); - break; - } } 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 4af37dc..e539bd1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -30,6 +30,16 @@ public class CompTeleop extends OpMode { 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"); From 595fb344973bf62242c1fb0d245e1493a4438071 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Thu, 30 Nov 2017 07:41:02 -0500 Subject: [PATCH 05/19] Teleop Status: end of yesterday working, Auto Status: needs testing --- .../ftc/teamcode/Auto/DefaultAuto.java | 3 +- .../ftc/teamcode/Auto/DefaultAutoMode.java | 175 +++++++++++++----- .../ftc/teamcode/Auto/IHAteLIFE.java | 68 ------- .../ftc/teamcode/Auto/TestGlyphInfo.java | 87 +++++++++ .../ftc/teamcode/Auto/TestJewelID.java | 4 + 5 files changed, 221 insertions(+), 116 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/IHAteLIFE.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestGlyphInfo.java 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 2a897f7..9011991 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 @@ -18,9 +18,8 @@ public void init() { public void loop() { switch (state){ - case 0: //grab glyph, lower arm and get pictogram + case 0: //grab glyph and get pictogram hardware.lift.closeAll(); - hardware.jewelArm.setPosition(JEWEL_ARM_DOWN_POS); nextState(getPictogram()); 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 50e7d5b..d11c316 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 @@ -35,7 +35,7 @@ public class DefaultAutoMode extends OpMode{ //Color Prox Vars ColorSensor colorSensor; - AllianceColor color; + JewelColor jewelColor; //auto mode objects Gyro gyro; @@ -126,48 +126,140 @@ public boolean pictogramDrive(PictogramResults image){ //colorProx public boolean getJewelColor(){ - /*if(colorSensor.red() > colorSensor.blue()){ + if(colorSensor.red() > colorSensor.blue()){ jewelColor = JewelColor.RED; }else if(colorSensor.blue()> colorSensor.red()){ jewelColor = JewelColor.BLUE; - }else{ - jewelColor = JewelColor.UNKNOWN;*/ - telemetry.addData("RED",colorSensor.red()); - telemetry.addData("BLUE",colorSensor.blue()); - telemetry.addData("BLUE MINUS RED",colorSensor.blue()-colorSensor.red()); - telemetry.addData("Jewel Color", color); - return true; - } //pt1 - private boolean moveJewel(){ - if(getJewelColor()){ - if(color != allianceColor){ - return autoTurn(-30,0.5); - } else { - return autoTurn(30,0.5); - } - } else { - return false; + }else { + jewelColor = JewelColor.UNKNOWN; } - } //pt2 - private boolean jewelArmUp(){ - if(moveJewel()){ - hardware.jewelArm.setPosition(JEWEL_ARM_UP_POS); - return (pause(2)); - } else { - return false; - } - } //pt3 + telemetry.addData("Jewel Color", jewelColor); + return true; + } + + 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(hardware.jewelArm.getPosition()==JEWEL_ARM_DOWN_POS){ + jewelState++; + } + break; + + case 1: // get reading + if(getJewelColor()){ + jewelState++; + } + break; + + case 2: // raise arm + hardware.jewelArm.setPosition(JEWEL_ARM_UP_POS); + if(hardware.jewelArm.getPosition() == JEWEL_ARM_UP_POS){ + jewelState++; + } + break; + + case 3: //react accordingly + switch (allianceColor){ + case RED: + switch (jewelColor){ + case RED: + if(autoTurn(90,1)){ + jewelState++; + } + break; + + case BLUE: + if(autoTurn(-90,1)){ + jewelState++; + } + break; + + case UNKNOWN: + jewelState++; + break; + } + break; + + case BLUE: + switch (jewelColor){ + case RED: + if(autoTurn(-90,1)){ + + jewelState++; + } + break; + + case BLUE: + if(autoTurn(90,1)){ + jewelState++; + } + break; + + case UNKNOWN: + jewelState++; + break; + } + break; + } + break; + + case 4: //reset encoders + hardware.resetDriveEncoders(); + jewelState++; + break; + + case 5: //reset robot accordingly + switch (allianceColor){ + case RED: + switch (jewelColor){ + case RED: + if(autoTurn(-90,1)){ + jewelState++; + } + break; + + case BLUE: + if(autoTurn(90,1)){ + jewelState++; + } + break; + + case UNKNOWN: + jewelState++; + break; + } + break; + + case BLUE: + switch (jewelColor){ + case RED: + if(autoTurn(90,1)){ + jewelState++; + } + break; + + case BLUE: + if(autoTurn(-90,1)){ + jewelState++; + } + break; + + case UNKNOWN: + jewelState++; + break; + } + break; + } + break; + + case 6: // return true + return true; } - } //pt4 + return false; + } + //driving /* @@ -257,7 +349,6 @@ public void glyphLiftPower(double power){ hardware.glyphLeft.setPower(power); hardware.glyphRight.setPower(power); } - private boolean setGlyphLiftPos(double inch, double power){ if(inch>0){ if (hardware.getLiftTicks() colorSensor.blue()){ - jewelColor = JewelColor.RED; - done = true; - }else if(colorSensor.blue()> colorSensor.red()){ - jewelColor = JewelColor.BLUE; - done = true; - }else{ - done=System.currentTimeMillis()-startTime>5000; - jewelColor = JewelColor.UNKNOWN; - } - - if(done){ - x++; - } - break; - - case 2: - telemetry.addData("COLOR",jewelColor); - - } - - - - } -} 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 db35858..566268a 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 @@ -27,6 +27,10 @@ public void loop() { case 0: nextState(getJewel()); break; + + case 1: + telemetry.addData("INFO",LSA); + break; } } } From f5c69472c305e75489089ca21ea9d5d550e8138a Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Thu, 30 Nov 2017 07:50:47 -0500 Subject: [PATCH 06/19] new numbers --- .../ftc/teamcode/Components/GlyphIntake2.java | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) 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..89b67b5 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 @@ -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; - - servoClose[Robot.RIGHT_TOP] = 0.440; - servoClose[Robot.LEFT_TOP] = 0.560; - servoClose[Robot.RIGHT_BOTTOM] = 0.560; - servoClose[Robot.LEFT_BOTTOM] = 0.530; + servoOpen[Robot.RIGHT_TOP] = 0.650; //from 11/30 + servoOpen[Robot.LEFT_TOP] = 0.440; //from 11/30 + servoOpen[Robot.RIGHT_BOTTOM] = 0.400; //from 11/30 + servoOpen[Robot.LEFT_BOTTOM] = 0.460; //from 11/30 + + servoClose[Robot.RIGHT_TOP] = 0.470; //from 11/30 + servoClose[Robot.LEFT_TOP] = 0.520; //from 11/30 + servoClose[Robot.RIGHT_BOTTOM] = 0.500; //from 11/30 + servoClose[Robot.LEFT_BOTTOM] = 0.370; //from 11/30 } public void closeTop() { From 86ce431c023bf3f4b15223951d1685105efe34d0 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Thu, 30 Nov 2017 12:47:53 -0500 Subject: [PATCH 07/19] added safe zone auto just incase all our normal autos crap out --- .../ftc/teamcode/Auto/ProductionSafeZone.java | 44 +++++++++++++++++++ .../ftc/teamcode/Auto/TestColorServoSlow.java | 8 ---- 2 files changed, 44 insertions(+), 8 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionSafeZone.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorServoSlow.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionSafeZone.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionSafeZone.java new file mode 100644 index 0000000..6265900 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionSafeZone.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +/** + * Created by Montclair Robotics on 11/30/2017. + */ +@Autonomous(name = "Production: Safe Zone") +public class ProductionSafeZone extends DefaultAutoMode { + @Override + public void init() { + autoInit(); + } + + @Override + public void loop() { + switch (state){ + case 0: + hardware.lift.closeAll(); + nextState(raiseGlyph()); + break; + + case 1: + hardware.lift.closeAll(); + nextState(safeZoneDrive()); + break; + + case 2: + hardware.lift.closeAll(); + nextState(lowerGlyph()); + break; + + case 3: + hardware.lift.openAll(); + nextState(pause(2)); + break; + + case 4: + telemetry.addData("INFO",LSA); + break; + + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorServoSlow.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorServoSlow.java deleted file mode 100644 index cd16fbc..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorServoSlow.java +++ /dev/null @@ -1,8 +0,0 @@ -package org.firstinspires.ftc.teamcode.Auto; - -/** - * Created by Montclair Robotics on 11/29/2017. - */ - -public class TestColorServoSlow { -} From 724e4b1beea9c31bd1c4800ac6ca3dbf3ccf4c62 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Fri, 1 Dec 2017 11:32:53 -0500 Subject: [PATCH 08/19] "moved Auto enums out of auto, changed a couple caluse and what not" - will --- .../ftc/teamcode/Auto/DefaultAutoMode.java | 28 +++++++++---------- .../teamcode/Auto/ProductionBlueClose.java | 4 +-- .../ftc/teamcode/Auto/ProductionBlueFar.java | 4 +-- .../ftc/teamcode/Auto/ProductionRedClose.java | 4 +-- .../ftc/teamcode/Auto/ProductionRedFar.java | 4 +-- .../ftc/teamcode/Auto/TestJewelID.java | 5 +--- .../{Auto => }/Enums/AllianceColor.java | 2 +- .../teamcode/{Auto => }/Enums/JewelColor.java | 2 +- .../{Auto => }/Enums/PictogramResults.java | 2 +- .../{Auto => }/Enums/StartPosition.java | 2 +- 10 files changed, 27 insertions(+), 30 deletions(-) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => }/Enums/AllianceColor.java (64%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => }/Enums/JewelColor.java (67%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => }/Enums/PictogramResults.java (71%) rename TeamCode/src/main/java/org/firstinspires/ftc/teamcode/{Auto => }/Enums/StartPosition.java (62%) 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 d11c316..87ef5ad 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,10 +9,10 @@ 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.JewelColor; -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; @@ -142,7 +142,7 @@ public boolean getJewel(){ switch (jewelState){ case 0: //lower arm hardware.jewelArm.setPosition(JEWEL_ARM_DOWN_POS); - if(hardware.jewelArm.getPosition()==JEWEL_ARM_DOWN_POS){ + if(pause(2)){ jewelState++; } break; @@ -155,7 +155,7 @@ public boolean getJewel(){ case 2: // raise arm hardware.jewelArm.setPosition(JEWEL_ARM_UP_POS); - if(hardware.jewelArm.getPosition() == JEWEL_ARM_UP_POS){ + if(pause(2)){ jewelState++; } break; @@ -165,13 +165,13 @@ public boolean getJewel(){ case RED: switch (jewelColor){ case RED: - if(autoTurn(90,1)){ + if(autoTurn(30,1)){ jewelState++; } break; case BLUE: - if(autoTurn(-90,1)){ + if(autoTurn(-30,1)){ jewelState++; } break; @@ -185,14 +185,14 @@ public boolean getJewel(){ case BLUE: switch (jewelColor){ case RED: - if(autoTurn(-90,1)){ + if(autoTurn(-30,1)){ jewelState++; } break; case BLUE: - if(autoTurn(90,1)){ + if(autoTurn(30,1)){ jewelState++; } break; @@ -215,13 +215,13 @@ public boolean getJewel(){ case RED: switch (jewelColor){ case RED: - if(autoTurn(-90,1)){ + if(autoTurn(-30,1)){ jewelState++; } break; case BLUE: - if(autoTurn(90,1)){ + if(autoTurn(30,1)){ jewelState++; } break; @@ -235,13 +235,13 @@ public boolean getJewel(){ case BLUE: switch (jewelColor){ case RED: - if(autoTurn(90,1)){ + if(autoTurn(30,1)){ jewelState++; } break; case BLUE: - if(autoTurn(-90,1)){ + if(autoTurn(-30,1)){ jewelState++; } break; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlueClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlueClose.java index 233af3d..6279e4d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlueClose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlueClose.java @@ -2,8 +2,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.firstinspires.ftc.teamcode.Auto.Enums.AllianceColor; -import org.firstinspires.ftc.teamcode.Auto.Enums.StartPosition; +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; +import org.firstinspires.ftc.teamcode.Enums.StartPosition; /** * Created by Montclair Robotics on 11/13/17. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlueFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlueFar.java index b3afdea..b25d224 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlueFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlueFar.java @@ -2,8 +2,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.firstinspires.ftc.teamcode.Auto.Enums.AllianceColor; -import org.firstinspires.ftc.teamcode.Auto.Enums.StartPosition; +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; +import org.firstinspires.ftc.teamcode.Enums.StartPosition; /** * Created by Montclair Robotics on 11/13/17. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRedClose.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRedClose.java index a6af605..d3260b9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRedClose.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRedClose.java @@ -2,8 +2,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.firstinspires.ftc.teamcode.Auto.Enums.AllianceColor; -import org.firstinspires.ftc.teamcode.Auto.Enums.StartPosition; +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; +import org.firstinspires.ftc.teamcode.Enums.StartPosition; /** * Created by Montclair Robotics on 11/13/17. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRedFar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRedFar.java index aa7c8e0..c4424ee 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRedFar.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRedFar.java @@ -2,8 +2,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.firstinspires.ftc.teamcode.Auto.Enums.AllianceColor; -import org.firstinspires.ftc.teamcode.Auto.Enums.StartPosition; +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; +import org.firstinspires.ftc.teamcode.Enums.StartPosition; /** * Created by Montclair Robotics on 11/13/17. 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 566268a..5069a77 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 @@ -2,10 +2,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -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. 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/Auto/Enums/JewelColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/JewelColor.java similarity index 67% rename from TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/JewelColor.java rename to TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/JewelColor.java index 56f691a..dd8c06d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/Enums/JewelColor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Enums/JewelColor.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.teamcode.Auto.Enums; +package org.firstinspires.ftc.teamcode.Enums; /** * Created by Montclair Robotics on 11/29/2017. 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. From 7812f7e5bc96e78f45acc5af5f14a061ff98e2b8 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Fri, 1 Dec 2017 15:44:09 -0500 Subject: [PATCH 09/19] Committng some changes after reviewing the code -Jack --- .../ftc/teamcode/Auto/DefaultAutoMode.java | 22 +- .../ftc/teamcode/CompTeleop.java | 6 +- .../ftc/teamcode/CompTeleopWithGyro.java | 130 ++++++++ .../ftc/teamcode/Components/GlyphIntake2.java | 2 - .../org/firstinspires/ftc/teamcode/Gyro.java | 22 +- .../firstinspires/ftc/teamcode/GyroLock.java | 5 +- .../org/firstinspires/ftc/teamcode/PID.java | 23 +- .../org/firstinspires/ftc/teamcode/Robot.java | 305 ------------------ 8 files changed, 170 insertions(+), 345 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Robot.java 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 87ef5ad..c76d8f0 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 @@ -18,6 +18,8 @@ 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 @@ -129,7 +131,7 @@ public boolean getJewelColor(){ if(colorSensor.red() > colorSensor.blue()){ jewelColor = JewelColor.RED; }else if(colorSensor.blue()> colorSensor.red()){ - jewelColor = JewelColor.BLUE; + jewelColor = BLUE; }else { jewelColor = JewelColor.UNKNOWN; } @@ -153,15 +155,10 @@ public boolean getJewel(){ } break; - case 2: // raise arm - hardware.jewelArm.setPosition(JEWEL_ARM_UP_POS); - if(pause(2)){ - jewelState++; - } - break; + case 2: //react accordingly - case 3: //react accordingly switch (allianceColor){ + case RED: switch (jewelColor){ case RED: @@ -205,11 +202,18 @@ public boolean getJewel(){ } break; - case 4: //reset encoders + 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 switch (allianceColor){ case RED: 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 e539bd1..4be790b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -6,6 +6,7 @@ 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; @@ -55,9 +56,7 @@ public void init() { } @Override - public void loop() { - //driveTrain.driveMechanum(gamepad1); - + public void loop(){ double pow = 1.0; @@ -88,7 +87,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..588b10d --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -0,0 +1,130 @@ +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.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") +public class CompTeleopWithGyro extends OpMode { + //public DriveTrain driveTrain; + DcMotor frontRight, backRight, frontLeft, backLeft; + Servo[] servos; + + Gyro gyro; + PID gyroLock; + 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(.1,0,.1); + zeroAngle=gyro.get().getX(); + limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); + } + + @Override + public void loop(){ + + double pow = 1.0; + + if (gamepad1.left_bumper) { + pow = 0.5; + } + + double x = gamepad1.left_stick_x * pow; + double y = -gamepad1.left_stick_y * pow; + double turn = gamepad1.right_stick_x * pow; + + if(gamepad1.x) + { + zeroAngle=gyro.get().getX(); + } + + if(/*Math.abs(turn)<0.1 || */gamepad1.a) + { + turn=gyroLock.get(gyro.get().getX()); + } + + 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(); + } + + frontRight.setPower(x - y + turn); + backRight.setPower(-x - y + turn); + backLeft.setPower(-x + y + turn); + frontLeft.setPower(x + y + turn); + + + if (gamepad2.a) + intake.openBottom(); + else + intake.closeBottom(); + + if (gamepad2.b) + intake.openTop(); + else + intake.closeTop(); + + liftA.setPower(gamepad2.left_stick_y); + liftB.setPower(-gamepad2.left_stick_y); + + telemetry.addData("Limit Switch", limitSwitch); + } +} 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 89b67b5..f261193 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,8 +2,6 @@ import com.qualcomm.robotcore.hardware.Servo; -import org.firstinspires.ftc.teamcode.Robot; - /** * Created by Montclair Robotics on 11/13/17. * @Author:Rafi 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..c191dec 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 @@ -33,20 +28,9 @@ public Gyro(HardwareMap map) { imu.initialize(parameters); imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, 6); - update(); - Updater.add(this, Priority.INPUT); - } - - @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..cb4eedb 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GyroLock.java @@ -34,7 +34,7 @@ public void teleopLoop(Gamepad g) { // Reset the error record. pid.error.reset(); // Set the target to the current trajectory. - pid.setTarget(gyro.get()); + pid.setTarget(gyro.get().getX()); } // It sure is active now. @@ -52,8 +52,7 @@ public void autoLoop() { } public void loop() { - gyro.update(); - pid.setInput(gyro.get()); + pid.setInput(gyro.get().getX()); pid.update(); Debug.msg("PID Input", pid.getInput().intValue() + "°"); 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..0d89096 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode; +import org.montclairrobotics.sprocket.utils.Input; + /** * Created by Joshua Rapoport on 11/14/17. */ @@ -52,7 +54,7 @@ public void update() { protected Error error; /** The input from the robot, to be compared with the target. */ - private Double input; + private double input; /** The robot's target, to be compared with the input. */ private double target; private double output; @@ -77,6 +79,7 @@ public PID(double p, double i, double d) { this.input = this.target = this.output = 0.0; update(); + lastUpdateTime=System.currentTimeMillis(); } public double getOutput() { @@ -104,9 +107,16 @@ else if (outRange.compareTo(potentialI) < 0) error.total += error.current; } - double out = (P * error.current * dTime()) + (I * error.total) + (D * -dInput / dTime()); + double dTime=dTime(); + double dChg=0; + if(dTime>0) + { + dChg=D * -dInput / dTime; + } + + double out = (P * error.current * dTime()) + (I * error.total) + dChg; - setInput(newInput); + input=newInput; if (outRange.compareTo(out) > 0) out = outRange.max; @@ -174,4 +184,11 @@ public double dTime() { public void update() { lastUpdateTime = System.currentTimeMillis(); } + + public double get(double in) + { + setInput(in); + lastUpdateTime=System.currentTimeMillis(); + return output; + } } 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() { - - } -} From ed591369b257d5c4efb4676310ede1874d829536 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Fri, 1 Dec 2017 15:59:18 -0500 Subject: [PATCH 10/19] "mud-blood code" - will --- .../ftc/teamcode/Auto/DefaultAutoMode.java | 144 +++++++----------- 1 file changed, 51 insertions(+), 93 deletions(-) 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 c76d8f0..a6a0832 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 @@ -48,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; @@ -127,16 +132,38 @@ public boolean pictogramDrive(PictogramResults image){ } //colorProx - public boolean getJewelColor(){ - if(colorSensor.red() > colorSensor.blue()){ - jewelColor = JewelColor.RED; - }else if(colorSensor.blue()> colorSensor.red()){ - jewelColor = BLUE; - }else { - jewelColor = JewelColor.UNKNOWN; + 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--; } - telemetry.addData("Jewel Color", jewelColor); - return true; + if(Math.abs(redOverBlue)>10) { + if(redOverBlue>0) { + jewelColor=JewelColor.RED; + } else { + jewelColor=JewelColor.BLUE; + } + telemetry.addData("Jewel Color", jewelColor); + averaging=false; + return true; + } + if(System.currentTimeMillis()>avgTimeout) { + jewelColor=JewelColor.UNKNOWN; + telemetry.addData("Jewel Color", jewelColor); + averaging=false; + return true; + } + return false; } int jewelState = 0; @@ -150,55 +177,19 @@ public boolean getJewel(){ break; case 1: // get reading - if(getJewelColor()){ + if(getJewelColorAvg()){ jewelState++; } break; case 2: //react accordingly - - 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; + double reactDegrees=(allianceColor==AllianceColor.RED)?30:-30; + if(jewelColor==BLUE) + { + reactDegrees*=-1; + } + if(autoTurn(reactDegrees,1)){ + jewelState++; } break; @@ -215,46 +206,13 @@ public boolean getJewel(){ break; case 5: //reset robot accordingly - 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; + double resetDegrees=(allianceColor==AllianceColor.RED)?-30:30; + if(jewelColor==BLUE) + { + resetDegrees*=-1; + } + if(autoTurn(resetDegrees,1)){ + jewelState++; } break; From 13f72e0f6ff7f4747887e9d78c8e5a89af0f04c5 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Fri, 1 Dec 2017 19:18:43 -0500 Subject: [PATCH 11/19] :))))) --- .../ftc/teamcode/Auto/DefaultAuto.java | 34 ++--- .../ftc/teamcode/Auto/DefaultAutoMode.java | 139 +++++++++++++++--- .../ftc/teamcode/Auto/ProductionBlue.java | 29 ++++ .../ftc/teamcode/Auto/ProductionRed.java | 31 ++++ .../ftc/teamcode/Auto/ProductionSafeZone.java | 4 +- .../ftc/teamcode/Auto/TestColor.java | 15 +- .../ftc/teamcode/Auto/TestColorSensor.java | 24 --- .../ftc/teamcode/Auto/TestJewelID.java | 2 + .../ftc/teamcode/Auto/TestVuforia.java | 1 + .../ftc/teamcode/CompTeleop.java | 3 +- .../ftc/teamcode/CompTeleopWithGyro.java | 3 + .../ftc/teamcode/Components/GlyphIntake2.java | 20 +-- 12 files changed, 220 insertions(+), 85 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlue.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRed.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensor.java 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 9011991..0007b6e 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 @@ -18,72 +18,64 @@ public void init() { public void loop() { switch (state){ - case 0: //grab glyph and get pictogram - hardware.lift.closeAll(); - nextState(getPictogram()); - break; - - case 1: // raise glyph + case 0: // raise glyph hardware.lift.closeAll(); nextState(raiseGlyph()); break; - case 2: //get jewel + case 1: //get jewel hardware.lift.closeAll(); nextState(getJewel()); break; - case 3: //drive forward or backward 24 + 2(to get off balancing stone) + case 2: //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 color + case 3: // turn based on color hardware.lift.closeAll(); nextState(driveTurn()); break; - case 5: // drive to safezone + case 4: // drive to safezone hardware.lift.closeAll(); nextState(safeZoneDrive()); break; - case 6: // turn at cryptobox + case 5: // turn at cryptobox hardware.lift.closeAll(); nextState(cryptoBoxTurn()); break; - case 7: //pictogram drive - hardware.lift.closeAll(); - nextState(pictogramDrive(pictogram)); - break; - - case 8: //input glyph + case 6: //input glyph hardware.lift.closeAll(); nextState(autoDrive(new XY(0,6),1)); break; - case 9: // lower glyph + case 7: // lower glyph hardware.lift.closeAll(); nextState(lowerGlyph()); break; - case 10: //release glyph + case 8: //release glyph hardware.lift.openAll(); nextState(pause(1)); break; - case 11: //back away + case 9: //back away nextState(autoDrive(new XY(0,2),1)); break; - case 12: //Telemetry + case 10 : //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 a6a0832..e090c22 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 @@ -60,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; @@ -73,7 +74,7 @@ public void autoInit(){ setState(0); timer = new ElapsedTime(); startTime = timer.milliseconds(); - visionInit(); +// visionInit(); hardware.resetDriveEncoders(); hardware.resetLiftEncoders(); telemetry.addData("INFO", "INITIALIZED"); @@ -132,6 +133,19 @@ public boolean pictogramDrive(PictogramResults image){ } //colorProx + 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("Color",jewelColor); + return true; + } + public boolean getJewelColorAvg() { if(!averaging) { @@ -177,19 +191,61 @@ public boolean getJewel(){ break; case 1: // get reading - if(getJewelColorAvg()){ + if(getJewelColor()){ jewelState++; } break; case 2: //react accordingly - double reactDegrees=(allianceColor==AllianceColor.RED)?30:-30; - if(jewelColor==BLUE) - { - reactDegrees*=-1; - } - if(autoTurn(reactDegrees,1)){ - jewelState++; +// 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; @@ -206,13 +262,54 @@ public boolean getJewel(){ break; case 5: //reset robot accordingly - double resetDegrees=(allianceColor==AllianceColor.RED)?-30:30; - if(jewelColor==BLUE) - { - resetDegrees*=-1; - } - if(autoTurn(resetDegrees,1)){ - jewelState++; +// 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; @@ -264,10 +361,10 @@ public boolean autoTurn(double degrees, double speed) { 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; } @@ -277,7 +374,7 @@ public boolean safeZoneDrive(){ return true; case FAR: - return autoDrive(new XY(0,12),0.5); + return autoDrive(new XY(0,12),1); } return false; } @@ -289,7 +386,7 @@ public boolean cryptoBoxTurn(){ return true; case FAR: - return autoTurn(90,0.5); + return autoTurn(90,autoTurnSpeed); } break; @@ -299,7 +396,7 @@ public boolean cryptoBoxTurn(){ return true; case FAR: - return autoTurn(-90,0.5); + return autoTurn(-90,autoTurnSpeed); } break; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlue.java new file mode 100644 index 0000000..d27b031 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlue.java @@ -0,0 +1,29 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; + +/** + * Created by Montclair Robotics on 12/1/2017. + */ + +public class ProductionBlue extends DefaultAutoMode { + @Override + public void init() { + autoInit(); + allianceColor = AllianceColor.BLUE; + telemetry.addData("Alliance Color",allianceColor); + } + + @Override + public void loop() { + switch (state) { + case 0: + nextState(getJewel()); + break; + + case 1: + telemetry.addData("INFO",LSA); + break; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRed.java new file mode 100644 index 0000000..127a921 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRed.java @@ -0,0 +1,31 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; + +/** + * Created by Montclair Robotics on 12/1/2017. + */ +@Autonomous(name = "Production: Red") +public class ProductionRed extends DefaultAutoMode{ + @Override + public void init() { + autoInit(); + allianceColor = AllianceColor.RED; + telemetry.addData("Alliance Color",allianceColor); + } + + @Override + public void loop() { + switch (state) { + case 0: + nextState(getJewel()); + break; + + case 1: + telemetry.addData("INFO",LSA); + break; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionSafeZone.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionSafeZone.java index 6265900..862e281 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionSafeZone.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionSafeZone.java @@ -2,6 +2,8 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.montclairrobotics.sprocket.geometry.XY; + /** * Created by Montclair Robotics on 11/30/2017. */ @@ -22,7 +24,7 @@ public void loop() { case 1: hardware.lift.closeAll(); - nextState(safeZoneDrive()); + nextState(autoDrive(new XY(0,26),1)); break; case 2: diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java index 7f30a0c..5cb9d2b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColor.java @@ -3,13 +3,14 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; 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.ColorSensor; /** * Created by Montclair Robotics on 11/13/17. * @Author:Will * */ -@Autonomous(name = "Test: Color Sensor") +@TeleOp(name = "Test: Color Sensor") public class TestColor extends DefaultAutoMode { @Override @@ -17,13 +18,15 @@ public void init() { autoInit(); } - @Override - public void start() { - colorSensor.enableLed(true); - } - @Override public void loop() { + telemetry.addData("R",colorSensor.red()); + telemetry.addData("B",colorSensor.blue()); getJewelColor(); + + colorSensor.enableLed(true ); + if(gamepad1.a){ + colorSensor.enableLed(false); + } } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensor.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensor.java deleted file mode 100644 index 8bcc082..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/TestColorSensor.java +++ /dev/null @@ -1,24 +0,0 @@ -package org.firstinspires.ftc.teamcode.Auto; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.ColorSensor; - -/** - * Created by Montclair Robotics on 11/29/2017. - */ -@Autonomous(name="testColorSensor") -public class TestColorSensor extends OpMode { - ColorSensor colorSensor; - @Override - public void init() { - colorSensor = hardwareMap.get(ColorSensor.class, "sensor_color"); - } - - @Override - public void loop() { - telemetry.addData("RED",colorSensor.red()); - telemetry.addData("BLUE",colorSensor.blue()); - telemetry.addData("BLUE MINUS RED",colorSensor.blue()-colorSensor.red()); - } -} 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 5069a77..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,6 +1,7 @@ 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.Enums.AllianceColor; @@ -9,6 +10,7 @@ * @Author:Will * */ @Autonomous(name = "Test: Jewel ID") +@Disabled public class TestJewelID extends DefaultAutoMode { @Override 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 4be790b..859b057 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -14,7 +14,7 @@ * Created by Montclair Robotics on 11/13/17. * @Author:Garrett * */ -@TeleOp(name="Teleop: PLEASE DON'T DELETE THIS WILL") +@TeleOp(name="Teleop: Competition") public class CompTeleop extends OpMode { //public DriveTrain driveTrain; DcMotor frontRight, backRight, frontLeft, backLeft; @@ -73,7 +73,6 @@ public void loop(){ backLeft.setPower(-x + y + turn); frontLeft.setPower(x + y + turn); - if (gamepad2.a) intake.openBottom(); else diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java index 588b10d..30651d6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -1,5 +1,6 @@ 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; @@ -18,6 +19,7 @@ * @Author:Garrett * */ @TeleOp(name="Teleop: Gyro Enabled") +@Disabled public class CompTeleopWithGyro extends OpMode { //public DriveTrain driveTrain; DcMotor frontRight, backRight, frontLeft, backLeft; @@ -126,5 +128,6 @@ public void loop(){ liftB.setPower(-gamepad2.left_stick_y); telemetry.addData("Limit Switch", limitSwitch); + telemetry.addData("Control Mode",myCtrlMode); } } 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 f261193..334b698 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 @@ -12,8 +12,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; @@ -22,15 +22,15 @@ public GlyphIntake2(Servo... servos) { servoOpen=new double[4]; servoClose=new double[4]; - servoOpen[Robot.RIGHT_TOP] = 0.650; //from 11/30 - servoOpen[Robot.LEFT_TOP] = 0.440; //from 11/30 - servoOpen[Robot.RIGHT_BOTTOM] = 0.400; //from 11/30 - servoOpen[Robot.LEFT_BOTTOM] = 0.460; //from 11/30 + servoOpen[0] = 0.650; //from 11/30 + servoOpen[1] = 0.440; //from 11/30 + servoOpen[2] = 0.400; //from 11/30 + servoOpen[3] = 0.460; //from 11/30 - servoClose[Robot.RIGHT_TOP] = 0.470; //from 11/30 - servoClose[Robot.LEFT_TOP] = 0.520; //from 11/30 - servoClose[Robot.RIGHT_BOTTOM] = 0.500; //from 11/30 - servoClose[Robot.LEFT_BOTTOM] = 0.370; //from 11/30 + servoClose[0] = 0.470; //from 11/30 + servoClose[1] = 0.520; //from 11/30 + servoClose[2] = 0.500; //from 11/30 + servoClose[3] = 0.370; //from 11/30 } public void closeTop() { From c38d487b02f1b6077a26a63c332fc266b886d893 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 2 Dec 2017 09:30:05 -0500 Subject: [PATCH 12/19] New new servo pos --- .../firstinspires/ftc/teamcode/CompTeleop.java | 2 ++ .../ftc/teamcode/Components/GlyphIntake2.java | 18 ++++++++++-------- 2 files changed, 12 insertions(+), 8 deletions(-) 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 859b057..826fa33 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -22,6 +22,7 @@ public class CompTeleop extends OpMode { // Gyro gyro; + public static final int SERVORT=0,SERVOLT=1,SERVORB=2,SERVOLB=3; GlyphIntake2 intake; DcMotor liftA, liftB; @@ -44,6 +45,7 @@ public void init() { 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"); 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 334b698..4c70b15 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,6 +2,8 @@ import com.qualcomm.robotcore.hardware.Servo; +import org.firstinspires.ftc.teamcode.CompTeleop; + /** * Created by Montclair Robotics on 11/13/17. * @Author:Rafi @@ -22,15 +24,15 @@ public GlyphIntake2(Servo... servos) { servoOpen=new double[4]; servoClose=new double[4]; - servoOpen[0] = 0.650; //from 11/30 - servoOpen[1] = 0.440; //from 11/30 - servoOpen[2] = 0.400; //from 11/30 - servoOpen[3] = 0.460; //from 11/30 + 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.446; //from 11/30 - servoClose[0] = 0.470; //from 11/30 - servoClose[1] = 0.520; //from 11/30 - servoClose[2] = 0.500; //from 11/30 - servoClose[3] = 0.370; //from 11/30 + 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() { From 3d44c3346e20cf6af1ecebdcb9baba5517128c71 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 2 Dec 2017 09:38:08 -0500 Subject: [PATCH 13/19] Fixed back up to not back up forward --- .../ftc/teamcode/Auto/DefaultAuto.java | 2 +- .../ftc/teamcode/CompTeleop.java | 21 +++++++++++++++++-- .../ftc/teamcode/CompTeleopWithGyro.java | 3 ++- 3 files changed, 22 insertions(+), 4 deletions(-) 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 0007b6e..96a1b99 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 @@ -72,7 +72,7 @@ public void loop() { break; case 9: //back away - nextState(autoDrive(new XY(0,2),1)); + nextState(autoDrive(new XY(0,-4),1)); break; case 10 : //Telemetry 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 826fa33..6909da8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -70,17 +70,34 @@ public void loop(){ 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(); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java index 30651d6..784645d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -19,7 +19,7 @@ * @Author:Garrett * */ @TeleOp(name="Teleop: Gyro Enabled") -@Disabled +//@Disabled public class CompTeleopWithGyro extends OpMode { //public DriveTrain driveTrain; DcMotor frontRight, backRight, frontLeft, backLeft; @@ -129,5 +129,6 @@ public void loop(){ telemetry.addData("Limit Switch", limitSwitch); telemetry.addData("Control Mode",myCtrlMode); + telemetry.addData("Gyro Angle",gyro.get().getX()); } } From b32c55c726a643de43484acc9ada05b95d8a9a52 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 2 Dec 2017 09:42:37 -0500 Subject: [PATCH 14/19] Disabled Jewel Auto (sorry, just isn't lined up. I'm sure it works!) --- .../ftc/teamcode/Auto/DefaultAuto.java | 2 +- .../ftc/teamcode/CompTeleopWithGyro.java | 21 +++++++++++++++++-- 2 files changed, 20 insertions(+), 3 deletions(-) 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 96a1b99..11722d4 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 @@ -25,7 +25,7 @@ public void loop() { case 1: //get jewel hardware.lift.closeAll(); - nextState(getJewel()); + nextState(/*getJewel()*/true); break; case 2: //drive forward or backward 24 + 2(to get off balancing stone) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java index 784645d..6c43837 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -81,6 +81,23 @@ public void loop(){ 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; + } + if(gamepad1.x) { zeroAngle=gyro.get().getX(); @@ -114,12 +131,12 @@ public void loop(){ 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(); From 98f02d7087b5acab16512849438dc4ba5a6d93d7 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 2 Dec 2017 11:02:30 -0500 Subject: [PATCH 15/19] Anneekah --- .../main/java/org/firstinspires/ftc/teamcode/CompTeleop.java | 2 +- .../org/firstinspires/ftc/teamcode/Components/GlyphIntake2.java | 2 +- TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) 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 6909da8..dd8a6c4 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -14,7 +14,7 @@ * Created by Montclair Robotics on 11/13/17. * @Author:Garrett * */ -@TeleOp(name="Teleop: Competition") +@TeleOp(name="ANNEEKAH Teleop: Competition") public class CompTeleop extends OpMode { //public DriveTrain driveTrain; DcMotor frontRight, backRight, frontLeft, backLeft; 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 4c70b15..db2acf0 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 @@ -6,7 +6,7 @@ /** * Created by Montclair Robotics on 11/13/17. - * @Author:Rafi + * @author Rafi * */ public class GlyphIntake2 { 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 c191dec..8db3d41 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Gyro.java @@ -26,7 +26,7 @@ public Gyro(HardwareMap map) { parameters.loggingTag = "IMU"; // set the logging tag imu.initialize(parameters); - imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, 6); + //imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, 6); } From 9879fae459b2f8e5d4af1e8826c72ab542127bb9 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 2 Dec 2017 14:10:46 -0500 Subject: [PATCH 16/19] 2:10 --- .../ftc/teamcode/Auto/DefaultAuto.java | 32 +-- .../ftc/teamcode/Auto/DefaultAutoMode.java | 2 +- .../ftc/teamcode/Auto/ProductionBlue.java | 4 +- .../teamcode/Auto/ProductionDumbDrive.java | 59 +++++ .../ftc/teamcode/Auto/ProductionRed.java | 2 +- .../ftc/teamcode/CompTeleop.java | 6 +- .../ftc/teamcode/CompTeleopWithGyro.java | 65 +++--- .../ftc/teamcode/Components/DriveTrain.java | 102 ++++----- .../ftc/teamcode/Components/GlyphIntake2.java | 2 +- .../firstinspires/ftc/teamcode/GyroLock.java | 101 ++++----- .../org/firstinspires/ftc/teamcode/PID.java | 210 ++++-------------- 11 files changed, 260 insertions(+), 325 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionDumbDrive.java 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 11722d4..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 @@ -17,18 +17,21 @@ public void init() { @Override public void loop() { switch (state){ - - case 0: // raise glyph + case 0: hardware.lift.closeAll(); - nextState(raiseGlyph()); + nextState(pause(0.5)); break; + case 1: // raise glyph + hardware.lift.closeAll(); - case 1: //get jewel + nextState(raiseGlyph()); + break; + case 2: //get jewel hardware.lift.closeAll(); nextState(/*getJewel()*/true); break; - case 2: //drive forward or backward 24 + 2(to get off balancing stone) + case 3: //drive forward or backward 24 + 2(to get off balancing stone) hardware.lift.closeAll(); switch (allianceColor){ case RED: @@ -41,44 +44,43 @@ public void loop() { } break; - case 3: // turn based on color + case 4: // turn based on color hardware.lift.closeAll(); nextState(driveTurn()); break; - case 4: // drive to safezone + case 5: // drive to safezone hardware.lift.closeAll(); nextState(safeZoneDrive()); break; - case 5: // turn at cryptobox + case 6: // turn at cryptobox hardware.lift.closeAll(); nextState(cryptoBoxTurn()); break; - case 6: //input glyph + case 7: //input glyph hardware.lift.closeAll(); nextState(autoDrive(new XY(0,6),1)); break; - case 7: // lower glyph + case 8: // lower glyph hardware.lift.closeAll(); nextState(lowerGlyph()); break; - case 8: //release glyph + case 9: //release glyph hardware.lift.openAll(); nextState(pause(1)); break; - case 9: //back away - nextState(autoDrive(new XY(0,-4),1)); + case 10: //back away + nextState(autoDrive(new XY(0,-2),1)); break; - case 10 : //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 e090c22..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 @@ -26,7 +26,7 @@ * */ -public class DefaultAutoMode extends OpMode{ +public class DefaultAutoMode extends OpMode { //Vuforia Vars VuforiaLocalizer vuforia; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlue.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlue.java index d27b031..5ad0673 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlue.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionBlue.java @@ -1,11 +1,13 @@ package org.firstinspires.ftc.teamcode.Auto; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + import org.firstinspires.ftc.teamcode.Enums.AllianceColor; /** * Created by Montclair Robotics on 12/1/2017. */ - +@Autonomous(name="Production Red Jewel") public class ProductionBlue extends DefaultAutoMode { @Override public void init() { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionDumbDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionDumbDrive.java new file mode 100644 index 0000000..6ef4af8 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionDumbDrive.java @@ -0,0 +1,59 @@ +package org.firstinspires.ftc.teamcode.Auto; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; + +import org.firstinspires.ftc.teamcode.Enums.AllianceColor; +import org.firstinspires.ftc.teamcode.Enums.StartPosition; +import org.montclairrobotics.sprocket.geometry.XY; + +/** + * Created by Montclair Robotics on 11/13/17. + * @Author:Will + * */ + + +@Autonomous(name = "Production: DumbDrive") +public class ProductionDumbDrive extends DefaultAutoMode { + + public void init() { + autoInit(); + } + @Override + public void loop() { + switch (state){ + case 0: + hardware.lift.closeAll(); + nextState(pause(0.5)); + break; + case 1: // raise glyph + hardware.lift.closeAll(); + + nextState(raiseGlyph()); + break; + + case 2: //input glyph + hardware.lift.closeAll(); + autoDrive(new XY(0,100),0.5); + nextState(pause(5)); + break; + + case 3: // lower glyph + hardware.lift.closeAll(); + nextState(lowerGlyph()); + break; + + case 4: //release glyph + hardware.lift.openAll(); + nextState(pause(1)); + break; + + case 5: //back away + nextState(autoDrive(new XY(0,-2),1)); + break; + + case 6 : //Telemetry + telemetry.addData("INFO",LSA); + break; + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRed.java index 127a921..437d54f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRed.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auto/ProductionRed.java @@ -7,7 +7,7 @@ /** * Created by Montclair Robotics on 12/1/2017. */ -@Autonomous(name = "Production: Red") +@Autonomous(name = "Production: Red Jewel") public class ProductionRed 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 dd8a6c4..f76f87b 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleop.java @@ -7,14 +7,14 @@ 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.DriveTrain; import org.firstinspires.ftc.teamcode.Components.GlyphIntake2; /** * Created by Montclair Robotics on 11/13/17. * @Author:Garrett * */ -@TeleOp(name="ANNEEKAH Teleop: Competition") +@TeleOp(name="ANEEKAH Teleop: Competition") public class CompTeleop extends OpMode { //public DriveTrain driveTrain; DcMotor frontRight, backRight, frontLeft, backLeft; @@ -66,7 +66,7 @@ public void loop(){ pow = 0.5; } - double x = gamepad1.left_stick_x * pow; + double x = gamepad1.left_stick_x * pow * 2; double y = -gamepad1.left_stick_y * pow; double turn = gamepad1.right_stick_x * pow; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java index 6c43837..7dc5df8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -8,7 +8,7 @@ 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.DriveTrain; import org.firstinspires.ftc.teamcode.Components.GlyphIntake2; import org.montclairrobotics.sprocket.geometry.Degrees; import org.montclairrobotics.sprocket.geometry.Vector; @@ -27,6 +27,7 @@ public class CompTeleopWithGyro extends OpMode { Gyro gyro; PID gyroLock; + boolean lastEnabled; double zeroAngle; enum CTRL_MODE {ROBOT,FIELD}; @@ -63,7 +64,7 @@ public void init() { intake = new GlyphIntake2(servos); gyro = new Gyro(hardwareMap); myCtrlMode=CTRL_MODE.ROBOT; - gyroLock=new PID(.1,0,.1); + gyroLock=new PID(.04,0,0.4, -180, 180, -1.0, 1.0); zeroAngle=gyro.get().getX(); limitSwitch = hardwareMap.get(DigitalChannel.class, "limit_switch_1"); } @@ -71,65 +72,71 @@ public void init() { @Override public void loop(){ - double pow = 1.0; + double pow = 1; if (gamepad1.left_bumper) { - pow = 0.5; + pow = 0.5;//maybe lower } double x = gamepad1.left_stick_x * pow; double y = -gamepad1.left_stick_y * pow; double turn = gamepad1.right_stick_x * pow; - if(gamepad1.dpad_up) - { + if(gamepad1.dpad_up) { y=pow; } - if(gamepad1.dpad_left) - { + if(gamepad1.dpad_left) { x=-pow; } - if(gamepad1.dpad_down) - { + if(gamepad1.dpad_down) { y=-pow; } - if(gamepad1.dpad_right) - { + if(gamepad1.dpad_right) { x=pow; } - if(gamepad1.x) - { + if(gamepad1.x) { zeroAngle=gyro.get().getX(); } - if(/*Math.abs(turn)<0.1 || */gamepad1.a) - { - turn=gyroLock.get(gyro.get().getX()); + if(Math.abs(turn)<0.1 || gamepad1.a) { + if (!lastEnabled) { + gyroLock.setTarget(-gyro.get().getX()); + } + + turn = gyroLock.get(-gyro.get().getX()); + } else { + gyroLock.get(-gyro.get().getX()); } - if(gamepad1.left_trigger>0.5) - { + lastEnabled = gamepad1.a; + + if(gamepad1.left_trigger>0.5) { myCtrlMode=CTRL_MODE.ROBOT; } - if(gamepad1.right_trigger>0.5) - { + + if(gamepad1.right_trigger>0.5) { myCtrlMode=CTRL_MODE.FIELD; } - if(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(); } - frontRight.setPower(x - y + turn); - backRight.setPower(-x - y + turn); - backLeft.setPower(-x + y + turn); - frontLeft.setPower(x + y + turn); + 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(fr/max); if (gamepad2.a||gamepad2.x) intake.openBottom(); @@ -146,6 +153,8 @@ public void loop(){ telemetry.addData("Limit Switch", limitSwitch); telemetry.addData("Control Mode",myCtrlMode); - telemetry.addData("Gyro Angle",gyro.get().getX()); + telemetry.addData("Gyro Angle", gyro.get().getX() + " deg"); + telemetry.addData("Turn: ", turn); + 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 db2acf0..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 @@ -27,7 +27,7 @@ public GlyphIntake2(Servo... servos) { 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.446; //from 11/30 + servoOpen[CompTeleop.SERVOLB] = 0.5; //from 11/30 servoClose[CompTeleop.SERVORT] = 0.45; //from 11/30 servoClose[CompTeleop.SERVOLT] = 0.5; //from 11/30 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 cb4eedb..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,60 +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().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()) + "%"); - } -} +// +//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 0d89096..e688ce2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PID.java @@ -7,188 +7,54 @@ */ public class PID { - public class Range { - double min, max; - - public Range() { - this.min = this.max = 0; - } - public Range(double a, double b) { - this.min = a; - this.max = b; - } - - 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; } + double P,I,D; + double minIn,maxIn; + double minOut,maxOut; + + double target; + double lastError; + double currentError; + double rateError; + double totalError; + long lastUpdateTimeMillis; + + 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(); - lastUpdateTime=System.currentTimeMillis(); + 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 dTime=dTime(); - double dChg=0; - if(dTime>0) - { - dChg=D * -dInput / dTime; - } - - double out = (P * error.current * dTime()) + (I * error.total) + dChg; - - input=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(); - } - - public double get(double in) - { - setInput(in); - lastUpdateTime=System.currentTimeMillis(); - return output; + return (P * currentError) + (I * totalError) + (D * rateError); } } From 6a642edd9cabb396e8d6de5f2fbc6fb4a42e6800 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 2 Dec 2017 14:33:56 -0500 Subject: [PATCH 17/19] 2:33 --- .../ftc/teamcode/CompTeleopWithGyro.java | 79 +++++++++---------- 1 file changed, 37 insertions(+), 42 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java index 7dc5df8..9228d5a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -71,59 +71,54 @@ public void init() { @Override public void loop(){ - double pow = 1; if (gamepad1.left_bumper) { - pow = 0.5;//maybe lower + pow = 0.5; //maybe lower } - double x = gamepad1.left_stick_x * pow; - double y = -gamepad1.left_stick_y * pow; + double x = 0.0; + double y = 0.0; 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) { + 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(); - } - - if(Math.abs(turn)<0.1 || gamepad1.a) { - if (!lastEnabled) { - gyroLock.setTarget(-gyro.get().getX()); - } - - turn = gyroLock.get(-gyro.get().getX()); - } else { - gyroLock.get(-gyro.get().getX()); + zeroAngle = gyro.get().getX(); } - - lastEnabled = gamepad1.a; - - if(gamepad1.left_trigger>0.5) { - myCtrlMode=CTRL_MODE.ROBOT; +// +// if (gamepad1.a && !lastEnabled) { +// gyroLock.setTarget(-gyro.get().getX()); +// turn = 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 (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(); + 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; @@ -133,12 +128,12 @@ public void loop(){ 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); + frontRight.setPower(fr / max); + backRight.setPower(br / max); backLeft.setPower(bl/max); frontLeft.setPower(fr/max); - if (gamepad2.a||gamepad2.x) + if (gamepad2.a || gamepad2.x) intake.openBottom(); else intake.closeBottom(); @@ -153,7 +148,7 @@ public void loop(){ telemetry.addData("Limit Switch", limitSwitch); telemetry.addData("Control Mode",myCtrlMode); - telemetry.addData("Gyro Angle", gyro.get().getX() + " deg"); + telemetry.addData("Gyro Angle", (int) gyro.get().getX() + " deg"); telemetry.addData("Turn: ", turn); telemetry.update(); } From 047ec32d41ea1af5e1c3d09925547a38bd7f7aad Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Sat, 2 Dec 2017 14:37:43 -0500 Subject: [PATCH 18/19] 2:37 --- .../org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java index 9228d5a..f1f3be1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -126,12 +126,12 @@ else if (gamepad1.dpad_right) 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)))); + double max = (1/Math.sqrt(2) + 1); frontRight.setPower(fr / max); backRight.setPower(br / max); - backLeft.setPower(bl/max); - frontLeft.setPower(fr/max); + backLeft.setPower(bl / max); + frontLeft.setPower(fr / max); if (gamepad2.a || gamepad2.x) intake.openBottom(); From dd89e5b8a884afacf084d96d8a298a677007f8b5 Mon Sep 17 00:00:00 2001 From: "Asus Zen-Book\\Montclair Robotics" Date: Mon, 4 Dec 2017 14:47:28 -0500 Subject: [PATCH 19/19] Last Gyro --- .../ftc/teamcode/CompTeleopWithGyro.java | 56 +++++++++++++++---- 1 file changed, 45 insertions(+), 11 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java index f1f3be1..cf11c73 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/CompTeleopWithGyro.java @@ -16,7 +16,7 @@ /** * Created by Montclair Robotics on 11/13/17. - * @Author:Garrett + * @author Garrett * */ @TeleOp(name="Teleop: Gyro Enabled") //@Disabled @@ -28,6 +28,9 @@ public class CompTeleopWithGyro extends OpMode { Gyro gyro; PID gyroLock; boolean lastEnabled; + + double lastTime; + double zeroAngle; enum CTRL_MODE {ROBOT,FIELD}; @@ -64,9 +67,10 @@ public void init() { intake = new GlyphIntake2(servos); gyro = new Gyro(hardwareMap); myCtrlMode=CTRL_MODE.ROBOT; - gyroLock=new PID(.04,0,0.4, -180, 180, -1.0, 1.0); + 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 @@ -98,13 +102,39 @@ else if (gamepad1.dpad_right) if(gamepad1.x) { zeroAngle = gyro.get().getX(); } -// -// if (gamepad1.a && !lastEnabled) { -// gyroLock.setTarget(-gyro.get().getX()); -// turn = gyroLock.get(-gyro.get().getX()); -// } -// -// lastEnabled = gamepad1.a; + 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; @@ -126,12 +156,12 @@ else if (gamepad1.dpad_right) double bl = -x + y + turn; double fl = x + y + turn; - double max = (1/Math.sqrt(2) + 1); + 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(fr / max); + frontLeft.setPower(fl / max); if (gamepad2.a || gamepad2.x) intake.openBottom(); @@ -150,6 +180,10 @@ else if (gamepad1.dpad_right) 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(); } }