Skip to content

Commit

Permalink
v4.28.1
Browse files Browse the repository at this point in the history 
- Ready for league meet #3
  • Loading branch information
@shuhul
shuhul committed Jan 21, 2023
1 parent 0c9a39c commit b687f23
Show file tree
Hide file tree
Showing 5 changed files with 78 additions and 42 deletions.
12 changes: 6 additions & 6 deletions TeamCode/src/main/java/automodules/AutoModuleUser.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -236,7 +236,7 @@ public void define() {

static AutoModule BackwardCycle2(Height height) {return new AutoModule(
outtake.stageClose(0.2),
outtake.stageReadyEnd(0.0),
outtake.stageReadyEnd(0.1),
lift.stageLift(1.0, heightMode.getValue(height)+4)
);}

Expand All @@ -248,7 +248,7 @@ static AutoModule BackwardCycle2(Height height) {return new AutoModule(
// lift.moveTime(-1.0, 0.1),
lift.resetCutoff(),
// outtake.stageStart(0.0),
lift.stageLift(1.0,0).attach(outtake.stageStartAfter(0.2))
lift.stageLift(1.0,0).attach(outtake.stageStartAfter(0.1))

// outtake.stageEnd(0.0),
// outtake.stageOpen(0.0),
Expand All @@ -265,7 +265,7 @@ static AutoModule BackwardCycle2(Height height) {return new AutoModule(
static Independent Cycle2(int i) { return new Independent() {
@Override
public void define() {
double x = (i*0.0)-0.2; double y = i*0.0;
double x = (i*0.0); double y = i*0.0;
if(i+1 == 1){
addAutoModule(leds.autoModuleColor(OLed.LEDColor.OFF));
addWaypoint(0.7, x, 18+y, 0);
Expand All @@ -275,12 +275,12 @@ public void define() {
if(i+1 == 10){ addAutoModule(leds.autoModuleColor(OLed.LEDColor.ORANGE)); }

addConcurrentAutoModuleWithCancel(BackwardCycle2(HIGH), 0.2);
addWaypoint(i == 0 ? 0.5 : 0.65, x, -15+y, 0);
addWaypoint(i == 0 ? 0.5 : 0.6, x, -15+y, 0);
addSegment(0.2, 0.5, mecanumNonstopSetPoint, x, -23+y, 0);
addConcurrentAutoModuleWithCancel(ForwardCycle2);
addWaypoint(0.65, x, y, 0);
addWaypoint(0.6, x, y, 0);
// addSegment(0.5, 1.0, mecanumNonstopSetPoint, x, 10+y, 0);
addWaypoint(0.4, x, 25+y, 0);
addWaypoint(0.35, x, 25+y, 0);


} else{
Expand Down
70 changes: 40 additions & 30 deletions TeamCode/src/main/java/auton/TerraAutoMega.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import elements.GameItems;
import geometry.position.Pose;
import robotparts.RobotPart;
import util.ExceptionCatcher;
import util.template.Mode;

import static global.General.bot;
Expand All @@ -33,54 +34,63 @@ public class TerraAutoMega extends AutoFramework {
public void initialize() {
setConfig(mecanumNonstopConfig);

outtake.arml.changePosition("start", 0.02);
outtake.armr.changePosition("start", 0.02);
outtake.arml.changePosition("start", 0.03);
outtake.armr.changePosition("start", 0.03);

lift.maintain();
outtake.readyStart(); outtake.closeClaw();
outtake.closeClaw();
ExceptionCatcher.catchInterrupted(() -> Thread.sleep(500));
outtake.readyStart();
scan(false);
x = 0; s = 0; y = 125;
}

AutoModule BackwardReadyFirst = new AutoModule(
outtake.stageMiddle(0.0),
lift.changeCutoff(1),
lift.stageLift(1.0, heightMode.getValue(MIDDLE))
);
// AutoModule BackwardReadyFirst = new AutoModule(
// outtake.stageMiddle(0.0),
// lift.changeCutoff(1),
// lift.stageLift(1.0, heightMode.getValue(MIDDLE))
// );

AutoModule BackwardFirst = new AutoModule(
outtake.stageReadyEnd(0.0),
lift.stageLift(1.0, heightMode.getValue(HIGH)+4.0)
outtake.stageMiddle(0.0),
RobotPart.pause(0.5),
lift.stageLift(1.0, heightMode.getValue(HIGH)+4.0).attach(outtake.stageReadyEndAfter(0.2))
);

AutoModule Backward = new AutoModule(
RobotPart.pause(0.15),
outtake.stageReadyEnd(0.0),
lift.stageLift(1.0, heightMode.getValue(HIGH)+4.0)
RobotPart.pause(0.1),
outtake.stageFlip(0.0),
RobotPart.pause(0.1),
lift.stageLift(1.0, heightMode.getValue(HIGH)+4.0).attach(outtake.stageReadyEndAfter(0.2))
);

AutoModule Forward(int i){return new AutoModule(
outtake.stageEnd(0.05),
outtake.stageOpen(0.0),
lift.moveTime(-0.6, 0.15),
lift.stageLift(1.0, i == 0 ? 13.0 : Math.max(13.5 - (i*13.5/5.0), 0)).attach(outtake.stageStartAfter(0.1))
lift.moveTime(-1, 0.2),
outtake.stageAfter(0.03*(5-i) + 0.03, 0.0),
lift.stageLift(1.0, 0)
// lift.stageLift(1.0, i == 0 ? 14.5 : Math.max(15.0 - (i*15.0/5.0), 0)).attach(outtake.stageStartAfter(0.15))
// outtake.stageEnd(0.05),
// outtake.stageOpen(0.0),
// lift.moveTime(-0.6, 0.15),
// lift.stageLift(1.0, i == 0 ? 13.0 : Math.max(13.5 - (i*13.5/5.0), 0)).attach(outtake.stageStartAfter(0.1))
);}

AutoModule Grab = new AutoModule(
outtake.stageClose(0.15),
lift.moveTime(1,0.2).attach(outtake.stageReadyStartAfter(0.1))
outtake.stageMiddleWithoutFlip(0.0),
lift.moveTime(1,0.3)
);

@Override
public void define() {

// Pre-loaded cone move
addWaypoint(1.0, 0, 124, 0);
addConcurrentAutoModule(BackwardReadyFirst);
addWaypoint(0.7, 5, 112, 30);
addConcurrentAutoModuleWithCancel(BackwardFirst);
addConcurrentAutoModule(BackwardFirst);
addWaypoint(1.0, 0, 112, 10);
// Pre-loaded cone place
addTimedSetpoint(1.0, 0.7, 1.0, -6.5, 138, 45);
addTimedSetpoint(1.0, 0.5, 0.7, -6.5, 138, 45);
addConcurrentAutoModule(Forward(0));
// Start 5 cycle
customNumber(5, i -> {
Expand All @@ -93,17 +103,17 @@ public void define() {
}
// Move to pick
addSegment(0.8, mecanumDefaultWayPoint, 18-x, 128 + s, 80);
addSegment(0.7, mecanumDefaultWayPoint, 57-x, 125 + s, 87);
addSegment(0.8, mecanumDefaultWayPoint, 52-x, 125 + s, 87);
addSegment(0.3, mecanumDefaultWayPoint, 66-x, 125 + s, 87);
// Pick
addTimedWaypoint( 0.3, 0.2, 66-x, 126 + s, 87);
addConcurrentAutoModuleWithCancel(Grab);
addTimedWaypoint( 0.2, 0.3, 62-x, 126 + s, 89);
addTimedWaypoint( 0.3, 0.3, 60-x, 126 + s, 89);
// Move to place
addConcurrentAutoModuleWithCancel(Backward);
addSegment(0.8, mecanumDefaultWayPoint, 30-x, 124 + s, 80);
addSegment(0.7, mecanumDefaultWayPoint, 11-x, 132 + s, 50);
addSegment(0.9, mecanumDefaultWayPoint, 30-x, 124 + s, 75);
addSegment(0.7, mecanumDefaultWayPoint, 15-x, 130 + s, 47);
// Place
addTimedSetpoint(1.0, 0.6, 0.6, -9 - x, 143 + s, 53);
addTimedSetpoint(1.0, 0.7, 0.5, -9 - x, 143 + s, 53);
addConcurrentAutoModuleWithCancel(Forward(i+1 == 5 ? 0 : i+1));
});
addPause(0.05);
Expand Down Expand Up @@ -159,7 +169,7 @@ public void define() {
addWaypoint(0.7, -120.0, y, -45);
addTimedSetpoint(1.0, 0.7, 1.0, -115.0, 120, 0);
});
addPause(0.1);
// addPause(0.1);
// End
}

Expand All @@ -171,7 +181,7 @@ public void postProcess() {
// preselectTeleOp = "TerraOp"
@Autonomous(name = "TerraAutoMegaRight", group = "auto")
public static class TerraAutoMegaRight extends TerraAutoMega {{ fieldSide = FieldSide.BLUE; fieldPlacement = FieldPlacement.LOWER; startPose = new Pose(20.5, Field.width/2.0 - Field.tileWidth - GameItems.Cone.height - 16,90); }}
@Autonomous(name = "TerraAutoMegaLeft", group = "auto")
public static class TerraAutoMegaLeft extends TerraAutoMega {{ fieldSide = FieldSide.BLUE; fieldPlacement = FieldPlacement.UPPER; startPose = new Pose(20.5, Field.width/2.0 + Field.tileWidth + GameItems.Cone.height + 16,90); }}
// @Autonomous(name = "TerraAutoMegaLeft", group = "auto")
// public static class TerraAutoMegaLeft extends TerraAutoMega {{ fieldSide = FieldSide.BLUE; fieldPlacement = FieldPlacement.UPPER; startPose = new Pose(20.5, Field.width/2.0 + Field.tileWidth + GameItems.Cone.height + 16,90); }}

}
15 changes: 9 additions & 6 deletions TeamCode/src/main/java/auton/TerraAutoNormal.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,6 +15,7 @@
import elements.GameItems;
import geometry.position.Pose;
import robotparts.RobotPart;
import util.ExceptionCatcher;
import util.template.Mode;

import static global.General.bot;
Expand All @@ -35,7 +36,9 @@ protected enum AutoMode implements Mode.ModeType {NORMAL, SIMPLE}
public void initialize() {
setConfig(mecanumNonstopConfig);
lift.maintain();
outtake.readyStart(); outtake.closeClaw();
outtake.closeClaw();
ExceptionCatcher.catchInterrupted(() -> Thread.sleep(500));
outtake.readyStart();
scan(false);
x = 0; s = 0;
}
Expand Down Expand Up @@ -120,10 +123,10 @@ public void define() {
// Place
customFlipped(() -> {
addTimedSetpoint(1.0, 0.6, 0.4, -1.3 - x, 134 + s, 53);
addTimedSetpoint(1.0, 0.6, 0.8, -9 - x, 143 + s, 53);
addTimedSetpoint(1.0, 0.6, 1.0, -9 - x, 143 + s, 53);
}, () -> {
addTimedSetpoint(1.0, 0.6, 0.4, -0.3 - x, 128 + s, 53);
addTimedSetpoint(1.0, 0.6, 0.8, -7.3 - x, 136 + s, 53);
addTimedSetpoint(1.0, 0.6, 1.0, -7.3 - x, 136 + s, 53);
});
addConcurrentAutoModuleWithCancel(Forward(i+1));
// addBreakpoint(() -> autoMode.equals(TerraAuto.AutoMode.SIMPLE) && i+1 == 3);
Expand All @@ -135,16 +138,16 @@ public void define() {
addSegment(0.7, mecanumDefaultWayPoint, -10, 128, 90);
addSegment(0.7, mecanumDefaultWayPoint, -45, 125, 60);
addSegment(0.7, mecanumDefaultWayPoint, -50, 126, 25);
addTimedSetpoint(1.0, 0.8, 2.0, -58, 78, 0);
addTimedSetpoint(1.0, 0.8, 1.2, -58, 78, 0);
}, () -> {
addTimedWaypoint(0.7, 0.5, 3.0, 122, 0);
addTimedSetpoint(1.0, 0.8, 2.0, 3.0, 78, 0);
addTimedSetpoint(1.0, 0.8, 1.2, 3.0, 78, 0);
}, () -> {
addSegment(0.7, mecanumDefaultWayPoint, 7, 130, 90);
addSegment(0.7, mecanumDefaultWayPoint, 39, 130, 58);
addSegment(0.7, mecanumDefaultWayPoint, 51, 114, 32);
addSegment(0.7, mecanumDefaultWayPoint, 56, 98, 0);
addTimedSetpoint(1.0, 0.8, 2.0, 65, 78, 0);
addTimedSetpoint(1.0, 0.8, 1.2, 65, 78, 0);
});
addPause(0.1);
// End
Expand Down
14 changes: 14 additions & 0 deletions TeamCode/src/main/java/robotparts/electronics/positional/PServo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -80,6 +80,20 @@ public void setPosition(String name){
servo.setPosition(positions.get(name));
}
}
// else{
// servo.setPosition(positions.get(name));
// }
}

public void setPosition(double pos){
if(access != null){
if (access.isAllowed()) {
servo.setPosition(pos);
}
}
// else{
// servo.setPosition(pos);
// }
}

/**
Expand Down
9 changes: 9 additions & 0 deletions TeamCode/src/main/java/robotparts/hardware/Outtake.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,8 @@ public void init() {
outtakeStatus.set(DRIVING);
}

public void arm(double pos){ armr.setPosition(pos); arml.setPosition(pos); }

public void moveStart(){ armr.setPosition("start"); arml.setPosition("start"); unFlip(); }
public void moveEnd(){ armr.setPosition("end"); arml.setPosition("end"); flip(); }
public void openClaw(){ claw.setPosition("open"); }
Expand Down Expand Up @@ -78,9 +80,16 @@ public void init() {
public Stage stageStartAfter(double t){ return super.customTimeAfter(this::moveStart, t); }
public Stage stageReadyStartAfter(double t){ return super.customTimeAfter(this::readyStart, t); }

public Stage stage(double pos, double t){ return super.customTime(() -> {unFlip(); arm(pos);}, t); }
public Stage stageAfter(double pos, double t){ return super.customTimeAfter(() -> {unFlip(); arm(pos);}, t); }

public Stage stageEndContinuous(double t){ return super.customContinuousTime(() -> armr, () -> arml, "end", t); }


public Stage stageMiddleWithoutFlip(double t){ return super.customTime(() -> {armr.setPosition("middle"); arml.setPosition("middle");}, t);}
public Stage stageReadyEndWithoutFlip(double t){ return super.customTime(() -> {armr.setPosition("endHalf"); arml.setPosition("endHalf");}, t); }




//
Expand Down

0 comments on commit b687f23

Please sign in to comment.