Add States to drive

[linglejack06]

• add lineup state to handle lineup logic, otf for otf, joystick for joystick
• add intakeLocation and goToIntake to help determine when we want coral station vs reef / processor for otf

[linglejack06]

@jkleiber @aidnem how do i make a state transition to another state upon a condition being met? ie if cancel otf i want joystick state, or if otf completes then i want lineup

also, what should even go into JoystickState, should i just make drive with joysticks send its inputs there and then joystick state can setGoalSpeeds?

[jkleiber]

@linglejack06 for the state transition question, I assume we could set a flag from a command that each state can either handle or not depending (for example, setting otf flag would trigger joystick state to attempt to transition to otf state. Likewise if otf flag goes false otf would fire the edge to go to joystick state)

As for the drive with joysticks, I would keep sending to the overall subsystem and pass those inputs into the joystick state I think

[linglejack06]

@jkleiber so just set goal speeds in drive for joysticks, but from state or command?

[jkleiber]

@linglejack06 if I understand correctly, the way it currently works is that drive with joysticks sends Drive some goal speeds and they get applied

So what we should do is pass those goal speeds to the relevant state(s) and have them apply it when they run their periodic. Otherwise they just get ignored

So short answer: command the states

[linglejack06]

@jkleiber @avidraccoon how do i run the onEntry functions?

also, how can i re run on entry? for example, if we change location in the middle of otf, i want to rerun its onEntry (which schedules the otf command) so it can go to the new location. Do i have to fire state to cancel and then refire the otf state?

[linglejack06]

also, can i cancel a state from within that state, ie if lineup is running but it cant find a tag then i want to go back to joysticks. this logic is in the lineup state though, not drive, so i cant fire a trigger

[jkleiber]

@linglejack06 take a look at scoring subsystem for how to fire triggers in states. @aidnem has been passing the subsystem to the state constructors 4o do this

[linglejack06]

yeah, i saw after looking further, do you mind checking over the states to see if thats how you want it structured?

…

On Wed, Feb 5, 2025 at 8:59 AM Justin Kleiber ***@***.***> wrote: @linglejack06 <https://github.com/linglejack06> take a look at scoring subsystem for how to fire triggers in states. @aidnem <https://github.com/aidnem> has been passing the subsystem to the state constructors 4o do this — Reply to this email directly, view it on GitHub <#70 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AZ7J5PR5NOVATAFTW7B774D2OIKNRAVCNFSM6AAAAABWPEJRQ6VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDMMZWHEZTANBSGM> . You are receiving this because you were mentioned.Message ID: ***@***.***>

[jkleiber]

Also on the on entry stuff I think that gets configured as part of the transition. You can also run the action again via the run on entry function in the state superclass I think? I'd need to take a look on a real code editor to be sure

I'll take a look at the state structure later today

[linglejack06]

Also on the on entry stuff I think that gets configured as part of the transition. You can also run the action again via the run on entry function in the state superclass I think? I'd need to take a look on a real code editor to be sure

I'll take a look at the state structure later today

ok, ive temporarily configured this function by just running on entry whenever the pose configured is different from drives desired location pose.

[linglejack06]

@jkleiber regarding the joystick command state thing. I dont think i can command the state as there is no way to grab the instance of the state and send it things through robot container or init bindings... unless im missing something

therefore, right now i just have setGoalSpeeds to only allow joystick speeds (field centric) if we are in joystick state.
ie

if(fieldCentric && stateMachine.inState(Joystick)) {
 // set field centric speeds
} else { // robot centric, joysticks dont touch this, only otf and lineup

[linglejack06]

also, on entry doesnt seem to be running, my otf state only began working once i added state machine periodic to drive periodic and i called otfState.onEntry in otf state periodic

[avidraccoon]

That is a known problem, and there is a branch on coppercore with an almost finished fix.

[avidraccoon]

The branch is for a few broken parts of the state machine I could also fix the not being able to pass variable in that.

[jkleiber]

super high level feedback, hopefully this helps with the problems you're seeing

[jkleiber]

recommend making these thresholds JSON configurable

[avidraccoon]

Because you defined the transition to lineup second is the only reason that this happened between these two states

[avidraccoon]

I have made a fix on the quickfix-statemachine branch, please let me know if it works or not

[avidraccoon]

It also fixes the onEntry problem

[linglejack06]

does on exit work too @avidraccoon

…

On Thu, Feb 6, 2025 at 12:22 PM avidraccoon ***@***.***> wrote: It also fixes the onEntry problem — Reply to this email directly, view it on GitHub <#70 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AZ7J5PQB2D3P7PIWIZSWGXL2OOK5JAVCNFSM6AAAAABWPEJRQ6VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDMNBQGUZDKNBSG4> . You are receiving this because you were mentioned.Message ID: ***@***.***>

[avidraccoon]

I believe because I have no evidence to say they don't and I fixed the problem onEntry had also for onExit.

[linglejack06]

@jkleiber state works now, but sometimes it doesn't like going straight to lineup due to thinking it's not close enough to otf pose. Ie if manually driving close and then clicking, it tries otf because the pose is not close enough to final pose (it's closer) can you think of any logic to change this behavior? If I increase distance pose can be from final pose it doesn't help

[jkleiber]

@linglejack06 I'm confused about the issue. If you're close enough to the OTF then lineup should happen automatically. Are you saying that the robot is closer to the reef than the OTF pose is so it goes backwards?

We could use the field finder utils in coppercore to see if you're already "on the runway" by seeing if you're in a certain rectangle. You'd need to define those rectangles but it could be a good add to skip OTF

[linglejack06]

@jkleiber update: found logic that works by just checking if driveToPose command is finished, if so we fire beginLineup trigger.

Drive now transitions through all states, and can even reroute otf if changing desired location mid state

I think this should be ready for review and merge once I push

[linglejack06]

trim.1EC55359-CBEF-4766-A11A-7215C0A4CAC6.MOV

[linglejack06]

Hold on review actually, I need to figure out why josh rotation isn't working anymore

[linglejack06]

@jkleiber ready for review

• rotation fixed
• states tested
• added goToIntake variable and intakeLocation variable to drive (this way we can have a scoring and intake location set) both interface with OTFState, currently set up so trigger goes to reef, top button goes to coral station.
• this quick cycle has been tested on robot and works great (station pose just a little off)

[linglejack06]

also, build failing due to need of new coppercore release

[jkleiber]

just some quick things to knock out before we merge this.

Also please update coppercore when able. I created a ticket for follow on improvements to the state machine

[linglejack06]

release published on coppercore, should fix build soon