Skip to content
This repository has been archived by the owner on Jun 24, 2021. It is now read-only.

Commit

Permalink
Aligning Beacons #2
Browse files Browse the repository at this point in the history
  • Loading branch information
@djfigs1
djfigs1 committed Jan 25, 2017
1 parent cbd00e7 commit 22bf43b
Show file tree
Hide file tree
Showing 5 changed files with 84 additions and 47 deletions.
6 changes: 3 additions & 3 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BeaconDoubleAutoExecutor_Blue.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
* Created by djfigs1 on 11/18/16.
*/

@Autonomous(name = "BLUE 2-Beacons")
@Autonomous(name = "BLUE Auto")
public class BeaconDoubleAutoExecutor_Blue extends RobotHardware {

private ActionSequence actionSequence;
Expand All @@ -17,7 +17,7 @@ public void init() {
super.init();
actionSequence = new RobotDoubleAutoFollowSequence(TEAM.BLUE);
}

//stop, hammer time
@Override
public void init_loop(){
beaconPosition(1);
Expand All @@ -38,4 +38,4 @@ public void loop() {
}
}
}

//good job
41 changes: 41 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BeaconDoubleAutoExecutor_Red.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,41 @@
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;

/**
* Created by djfigs1 on 11/18/16.
*/

@Autonomous(name = "RED Auto")
public class BeaconDoubleAutoExecutor_Red extends RobotHardware {

private ActionSequence actionSequence;
private Action action = null;

@Override
public void init() {
super.init();
actionSequence = new RobotDoubleAutoFollowSequence(TEAM.RED);
}

@Override
public void init_loop(){
beaconPosition(1);
}

@Override
public void loop() {
action = actionSequence.getCurrentAction();
if (action != null) {
telemetry.addData("Heading", gyroSensor.getHeading());
if (action.doAction(this)) {
actionSequence.currentActionComplete();
action = actionSequence.getCurrentAction();
}
}
else {
requestOpModeStop();
}
}
}

2 changes: 1 addition & 1 deletion TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DuoDistanceAlignAction.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@
*/
public class DuoDistanceAlignAction implements Action {

double alignRange = 0.05;
double alignRange;
double fixSpeed;


Expand Down
8 changes: 4 additions & 4 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotDoubleAutoFollowSequence.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@ public RobotDoubleAutoFollowSequence(RobotHardware.TEAM team) {
//1st Beacon
addAction(new FindLineAction(RobotHardware.VV_LINE_COLOR.BLUE, 0.10, true));
addAction(new FollowLineUntilDistance(RobotHardware.VV_LINE_COLOR.BLUE, 18, 0.06));
addAction(new DuoDistanceAlignAction(1, 0.09));
addAction(new DuoDistanceAlignAction(0, 0.09));
addAction(new PushBeaconAction(team, 0.08));
addAction(new TimeDriveAction(1000, -0.10, true));
//addAction(new FollowLineUntilDistance(RobotHardware.VV_LINE_COLOR.BLUE, 8, 0.06));
Expand All @@ -21,13 +21,13 @@ public RobotDoubleAutoFollowSequence(RobotHardware.TEAM team) {
addAction(new ResetBeaconAction());
addAction(new GyroTurnAction(RobotHardware.DIRECTION.LEFT, 100, 0.11));
addAction(new TimeDriveAction(500, 0.18, false));
addAction(new GyroTurnAction(RobotHardware.DIRECTION.RIGHT, 43, 0.11));
addAction(new GyroTurnAction(RobotHardware.DIRECTION.RIGHT, 46, 0.11));

//2nd Beacon
addAction(new FindLineAction(RobotHardware.VV_LINE_COLOR.BLUE, 0.10, true));
addAction(new FollowLineUntilDistance(RobotHardware.VV_LINE_COLOR.BLUE, 18, 0.06));
addAction(new DuoDistanceAlignAction(1, 0.09));
addAction(new DuoDistanceAlignAction(0, 0.09));
addAction(new PushBeaconAction(team, 0.08));
// You mother says hi

}
}
74 changes: 35 additions & 39 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RobotTeleOp.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,12 +5,14 @@
@TeleOp(name="TeleOp", group="Manual")
public class RobotTeleOp extends RobotTelemetry {

//region Variables
private boolean manual = true;
private boolean servoStartPosition = true;

enum AUTO_STATE {
FIND_LINE,
FOLLOW_LINE,
ALIGN_BEACONS,
PREP_BEACON,
WAIT_FOR_SERVO,
PUSH_BEACON,
Expand All @@ -31,11 +33,12 @@ enum AUTO_STATE {
double SLOW_SPEED = 0.07;
double QUICK_SPEED = 0.14;
int COLOR_THRESHOLD = 5;
int BEACON_DISTANCE = 9;
int BEACON_DISTANCE = 18;
double alignRange = 0.05;
VV_LINE_COLOR lineColor = VV_LINE_COLOR.BLUE;

float SPEED_SCALE = 3F;

float SPEED_SCALE = 4F;
//endregion

@Override
public void init() {
Expand All @@ -46,48 +49,15 @@ public void init() {

@Override
public void loop() {
//region Old
//
// GAMEPAD 1
// Manage the drive wheel motors.
//
//Manage the arm servos.
//GAMEPAD 2
//arm_1_position(get_arm_1_position() + gamepad2.left_stick_ y);
//arm_2_position(get_arm_2_position() + gamepad2.right_stick_y);


//if (gamepad2.x){ Nah.
// ToneGenerator toneG = new ToneGenerator(AudioManager.STREAM_ALARM, 100);
// toneG.startTone(ToneGenerator.TONE_CDMA_ALERT_CALL_GUARD, 200);
//
// arm_home_position();
//}

/*Raw Controls
double deadzone = .05;
if (Math.abs(gamepad2.left_stick_y - prev_left_y) > deadzone) {
arm_1_position(-gamepad2.left_stick_y);
prev_left_y = gamepad2.left_stick_y;
}
if (Math.abs(gamepad2.right_stick_y - prev_right_y) > deadzone) {
arm_2_position(-gamepad2.right_stick_y);
prev_right_y = gamepad2.right_stick_y;
}
*/
//endregion

//Telemetry Updates
//region Telemetry Updates
telemetry.addData("Manual", manual);
telemetry.addData("State", current_state.toString());
telemetry.addData("BServo", getBeaconPosition());

//endregion

//ABXY Buttons
//region ABXY Buttons
if (gamepad1.x) {
//Pushes the blue side of a beacon
if (!xPress) {

xPress = true;
team = TEAM.BLUE;
pressed = false;
Expand Down Expand Up @@ -143,7 +113,10 @@ public void loop() {
yPress = false;
}

//endregion

if (manual) {
//region Manual-Code
float l_left_drive_power = scale_motor_power(-gamepad1.left_stick_y) / SPEED_SCALE;
float l_right_drive_power = scale_motor_power(-gamepad1.right_stick_y) / SPEED_SCALE;

Expand All @@ -159,8 +132,10 @@ public void loop() {
if (gamepad2.right_bumper) {
beaconPosition(0);
}
//endregion

} else {
//region Auto-Code
switch (current_state) {
case FIND_LINE:
//Move until we find a white line
Expand Down Expand Up @@ -192,6 +167,24 @@ public void loop() {

//---------------------------

case ALIGN_BEACONS:
double leftCm = leftRangeSensor.cmUltrasonic();
double rightCm = getRightCm();

if (leftCm <= rightCm + alignRange && rightCm <= leftCm + alignRange){
current_state = AUTO_STATE.PREP_BEACON;
}
else {
if (leftCm >= rightCm + alignRange) {
set_drive_power(0.09, -0.09);
} else {
if (rightCm >= leftCm + alignRange) {
set_drive_power(-0.09, 0.09);
}
}
}
break;

case PREP_BEACON:
if ((getBeaconColor() == VV_BEACON_COLOR.BLUE && team == TEAM.RED) || (getBeaconColor() == VV_BEACON_COLOR.RED && team == TEAM.BLUE)) {
beaconPosition(0);
Expand Down Expand Up @@ -249,9 +242,11 @@ public void loop() {
manual = true;
break;
}
//endregion
}
}

//region Functions
void DPADPower() {
if (gamepad1.dpad_up) {
set_drive_power(0.1, 0.1);
Expand All @@ -266,4 +261,5 @@ void DPADPower() {
set_drive_power(-0.1, -0.1);
}
}
//endregion
}

0 comments on commit 22bf43b

Please sign in to comment.