Skip to content
This repository has been archived by the owner on Feb 13, 2018. It is now read-only.

Commit

Permalink
release: state relic recovery
Browse files Browse the repository at this point in the history
  • Loading branch information
@ariporad
ariporad committed Feb 12, 2018
1 parent 8f44193 commit df7ead7
Show file tree
Hide file tree
Showing 6 changed files with 61 additions and 46 deletions.
38 changes: 38 additions & 0 deletions 13330.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,38 @@
<?xml version='1.0' encoding='UTF-8' standalone='yes' ?>
<Robot type="FirstInspires-FTC">
<LynxUsbDevice name="Expansion Hub Portal 1" serialNumber="DQ189SQ9" parentModuleAddress="1">
<LynxModule name="REV 1" port="1">
<NeveRest40Gearmotor name="r1m0" port="0" />
<NeveRest40Gearmotor name="r1m1" port="1" />
<NeveRest40Gearmotor name="r1m2" port="2" />
<NeveRest40Gearmotor name="r1m3" port="3" />
<Servo name="r1s0" port="0" />
<Servo name="r1s1" port="1" />
<Servo name="r1s2" port="2" />
<Servo name="r1s3" port="3" />
<Servo name="r1s4" port="4" />
<Servo name="r1s5" port="5" />
<ColorSensor name="r1c1" port="0" bus="1" />
<ColorSensor name="r1c2" port="0" bus="2" />
<LynxEmbeddedIMU name="imu1" port="0" bus="0" />
<LynxColorSensor name="r1c0" port="1" bus="0" />
</LynxModule>
<LynxModule name="REV 2" port="2">
<RevRoboticsCoreHexMotor name="r2m0" port="0" />
<RevRoboticsCoreHexMotor name="r2m1" port="1" />
<RevRoboticsCoreHexMotor name="r2m2" port="2" />
<RevRoboticsCoreHexMotor name="r2m3" port="3" />
<Servo name="r2s0" port="0" />
<Servo name="r2s1" port="1" />
<Servo name="r2s2" port="2" />
<Servo name="r2s3" port="3" />
<Servo name="r2s4" port="4" />
<Servo name="r2s5" port="5" />
<LynxColorSensor name="r2c1" port="0" bus="1" />
<LynxColorSensor name="r2c2" port="0" bus="2" />
<LynxColorSensor name="r2c3" port="0" bus="3" />
<LynxEmbeddedIMU name="imu" port="0" bus="0" />
<LynxColorSensor name="r2c0" port="1" bus="0" />
</LynxModule>
</LynxUsbDevice>
</Robot>
16 changes: 10 additions & 6 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/competition/auto/PulsarAuto.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,6 @@ abstract public class PulsarAuto extends LinearOpMode {

abstract protected Alliance getAlliance();
abstract protected StartPosition getStartPosition();
protected boolean isSimpleAuto() { return false; }

public enum Alliance {
BLUE, RED;
Expand All @@ -41,6 +40,7 @@ protected enum TargetJewelPosition {FRONT, BACK, NONE}
protected PulsarRobotHardware hw;

private VuforiaController vuforiaController;
private static double JEWEL_TURN_SCALAR = 0.3;

/**
* IMPORTANT: turning a positive number of degrees is CCW
Expand All @@ -55,7 +55,8 @@ public void runOpMode() throws InterruptedException {
vuforiaController = new VuforiaController(hw);

hw.collectorUp();
hw.jewelsUp(true);
hw.servos.redJewelKicker.setPosition(hw.RED_JEWEL_INIT_POSITON);
hw.servos.blueJewelKicker.setPosition(hw.BLUE_JEWEL_INIT_POSITON);
hw.setFlipperPosition(0);

telemetry.addLine("Ready");
Expand All @@ -77,6 +78,8 @@ public void runOpMode() throws InterruptedException {

if (!opModeIsActive()) waitForStart();

hw.jewelsUp(false);

hw.storeCryptoboxTarget();
hw.conveyorOn();

Expand Down Expand Up @@ -221,14 +224,15 @@ private void knockOffJewel(TargetJewelPosition targetJewelPosition) {

double angle = targetJewelPosition == TargetJewelPosition.FRONT ? -10 : 10;

hw.turn(angle, 2000, 0.05);
hw.turn(angle, 2000, JEWEL_TURN_SCALAR);
scanCryptoKey();
hw.jewelsUp(true);
hw.turn(-angle, 2000, 0.05);
hw.turn(-angle, 2000, JEWEL_TURN_SCALAR);
}

private TargetJewelPosition getTargetJewel() {
int red = hw.colorSensors.jewel.red();
return TargetJewelPosition.FRONT;
/*int red = hw.colorSensors.jewel.red();
int blue = hw.colorSensors.jewel.blue();
Alliance backJewel;
Expand All @@ -238,6 +242,6 @@ private TargetJewelPosition getTargetJewel() {
else return TargetJewelPosition.NONE;
if (backJewel == getAlliance()) return TargetJewelPosition.FRONT;
else return TargetJewelPosition.BACK;
else return TargetJewelPosition.BACK; */
}
}
1 change: 1 addition & 0 deletions ...de/src/main/java/org/firstinspires/ftc/teamcode/competition/auto/full/PulsarAutoRedB.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

import org.firstinspires.ftc.teamcode.competition.auto.PulsarAuto;

@Disabled
@Autonomous(name = "Red B", group = "Final Auto")
public class PulsarAutoRedB extends PulsarAuto {
@Override
Expand Down
34 changes: 3 additions & 31 deletions ...de/src/main/java/org/firstinspires/ftc/teamcode/competition/teleop/PulsarStateTeleop.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,23 +3,17 @@
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.Range;

import org.firstinspires.ftc.teamcode.competition.auto.PulsarAuto;
import org.firstinspires.ftc.teamcode.lib.PulsarRobotHardware;
import org.redshiftrobotics.lib.blockplacer.Col;
import org.redshiftrobotics.lib.pid.imu.IMU;
import org.redshiftrobotics.lib.pid.imu.IMUWrapper;

@TeleOp(name="Pulsar State Teleop", group="Pulsar")
public class PulsarStateTeleop extends LinearOpMode {
private static final boolean USE_PID = false;
private static final boolean RELIC = true; // TODO: actually map relicPower to a control

private PulsarRobotHardware hw;

private IMU imu;

private boolean hasHadInput = false;

private static final double DRIVE_POWER_SCALAR = 1;
Expand Down Expand Up @@ -72,9 +66,6 @@ public void runOpMode() throws InterruptedException {
telemetry.update();

hw = new PulsarRobotHardware(this, PulsarAuto.Alliance.BLUE); // TODO: we actually need this now
imu = new IMUWrapper(hw.hwIMU);

hw.jewelsUp(false); // Don't sleep, we want to be able to start ASAP

telemetry.addLine("Ready for TeleOp (at State)!");
telemetry.addLine();
Expand All @@ -83,6 +74,8 @@ public void runOpMode() throws InterruptedException {

waitForStart();

hw.jewelsUp(false); // Don't sleep, we want to be able to start ASAP

while (opModeIsActive()) {
CheckForInputPresence();
ReadDriverControls(gamepad1);
Expand All @@ -99,6 +92,7 @@ public void runOpMode() throws InterruptedException {

private void CheckForInputPresence() {
hasHadInput = hasHadInput || !gamepad1.atRest() || !gamepad2.atRest();
hasHadInput = true; // FIXME
}

private void UpdateMotors(){
Expand Down Expand Up @@ -186,28 +180,6 @@ private void ReadOperatorControls(Gamepad operator){
}

private void ComputePLoop() {
if(USE_PID) {
lastAngle = currentAngle;
currentAngle = imu.getAngularRotationX();

isTurning = Math.abs(lastAngle - currentAngle) < 1 && anglePower != 0;

if (isTurning) {
targetAngle = currentAngle;
pPower = 0.0;
} else {
if (currentAngle + 360.0 - targetAngle <= 180.0) {
pPower = (currentAngle - targetAngle + 360.0) * pConstant;
} else if (targetAngle + 360.0 - currentAngle <= 180.0) {
pPower = (targetAngle - currentAngle + 360.0) * -1.0 * pConstant;
} else if (currentAngle - targetAngle <= 180.0) {
pPower = (currentAngle - targetAngle) * pConstant;
}
}

pPower = Range.clip(pPower / -180.0, -1.0, 1.0);
}

telemetry.addData("Movement", "(" + xDrivePower + ", " + yDrivePower + ")");

flPower = (yDrivePower - xDrivePower) * movementScalar + (pPower + anglePower) * rotationScalar;
Expand Down
16 changes: 8 additions & 8 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PulsarRobotHardware.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -182,10 +182,12 @@ protected DistanceSensors(HardwareMap hardwareMap) {
private static final double TURN_ANGLE_SCALAR = 1;


public final double RED_JEWEL_UP_POSITON = 0.6;
public final double RED_JEWEL_INIT_POSITON = 0.7;
public final double RED_JEWEL_UP_POSITION = 0.6;
public final double RED_JEWEL_DOWN_POSITON = 0;
public final double RED_JEWEL_ALT_DOWN_POSITON = 0.05;
public final double BLUE_JEWEL_UP_POSITON = 0.5;
public final double BLUE_JEWEL_INIT_POSITON = 0.45;
public final double BLUE_JEWEL_UP_POSITION = 0.5;
public final double BLUE_JEWEL_DOWN_POSITON = 0.8;
public final double BLUE_JEWEL_ALT_DOWN_POSITON = 0.75;
public final double RED_JEWEL_KICKER_FRONT_POSITION = 0;
Expand Down Expand Up @@ -265,10 +267,8 @@ public void setFlipperPosition(double position) {
}

public void jewelsUp(boolean sleep) {
servos.redJewelKicker.setPosition(RED_JEWEL_KICKER_CENTER_POSITION);
servos.blueJewelKicker.setPosition(BLUE_JEWEL_KICKER_CENTER_POSITION);
servos.blueJewelArm.setPosition(BLUE_JEWEL_UP_POSITON);
servos.redJewelArm.setPosition(RED_JEWEL_UP_POSITON);
servos.blueJewelArm.setPosition(BLUE_JEWEL_UP_POSITION);
servos.redJewelArm.setPosition(RED_JEWEL_UP_POSITION);
if (sleep) opMode.sleep(1000);
}
public void jewelDown(boolean sleep) {
Expand Down Expand Up @@ -412,8 +412,8 @@ public void turn(double angle) {
public void turn(double angle, long time) {
turningPIDController.turn(angle * TURN_ANGLE_SCALAR * alliance.getFlipFactor(), time);
}
public void turn(double angle, long time, double powerConstant) {
turningPIDController.turn(angle * TURN_ANGLE_SCALAR * alliance.getFlipFactor(), time, powerConstant);
public void turn(double angle, long time, double powerScalar) {
turningPIDController.turn(angle * TURN_ANGLE_SCALAR * alliance.getFlipFactor(), time, powerScalar);
}

/**
Expand Down
2 changes: 1 addition & 1 deletion lib
Submodule lib updated from 0f172f to a3b556

0 comments on commit df7ead7

Please sign in to comment.