Vision states [Potentially mergable] #70
Conversation
| @@ -366,6 +370,10 @@ public void teleopPeriodic() | |||
| { | |||
| mGrabber.setWantedState(ArticulatedGrabber.WantedState.PREPARE_INTAKE); | |||
| } | |||
| if (mControlBoard.readButton(Buttons.VISION_CUBE_HARVEST)) | |||
| { | |||
| mSuperstructure.setWantedState(Superstructure.WantedState.VISION_AQUIRE_CUBE); | |||
There was a problem hiding this comment.
Nitpick: Aquire is correctly spelt as acquire.
| @@ -462,14 +472,14 @@ private void searchForCube(double timestamp) | |||
| if (!this.isInitialized()) | |||
| return; | |||
| double dx = mVisionTargetAngleEntry.getNumber(0).doubleValue(); | |||
| if (dx != 0.0) { | |||
| if (dx < 30 && dx > -30) { // If our target is within reasonable bounds | |||
There was a problem hiding this comment.
Can be changed to epsilonEquals(dx, 0, 30).
| private void updateTurnToRobotHeading(double timestamp) | ||
| { | ||
| if (!this.isInitialized()) | ||
| return; | ||
| final double dx = mVisionTargetAngleEntry.getNumber(0).doubleValue(); | ||
| final Rotation2d robotToTarget = Rotation2d.fromDegrees(-dx); | ||
| // angle reversed to correct for raspi coordsys | ||
| if (Util.epsilonEquals(dx, 0, 1)) //TODO: Double check with someone if this is how you call it (number, target, the range) |
There was a problem hiding this comment.
The arguments are correctly ordered.
| setWantAimToVisionTarget(); | ||
| } | ||
| if (dx == 0.0) { | ||
| dx = -5; | ||
| if (dx > 30) { // If we get a bogus target (The bogus target is about 198) |
There was a problem hiding this comment.
Should this be an else if, or do I misunderstand that this is aim if on target, otherwise spin and search for one?
|
|
||
| if (mDrive.getDriveState() != DriveControlState.FIND_CUBE || mDrive.getDriveState() != DriveControlState.TURN_TO_ROBOTANGLE ) |
There was a problem hiding this comment.
I don't think you will ever be able to get out of this... Neither of these control states return to another state after being activated. You can fix this bug (and better encapsulate Drive) by doing the following:
if (mControlBoard.readButton(Buttons.VISION_CUBE_HARVEST))
{
mSuperstructure.setWantedState(Superstructure.WantedState.VISION_ACQUIRE_CUBE);
}
else
{
mDrive.setOpenLoop(mCheesyDriveHelper.cheesyDrive(throttle, turn,
mControlBoard.readButton(Buttons.DRIVE_QUICK_TURN),
!mControlBoard.readButton(Buttons.DRIVE_SLOW)));
}That should stop the two subsystems from fighting--please let me know if I'm missing something here.
There was a problem hiding this comment.
I thought through that situation as well, and I chose not to do it because from what I understand, if that solution is implemented, then it will only set superstructure when the button is held down. I wanted to make sure that teleopPeriodic() was never pulling the drive out of vision cube mode, and your method would pull drive out when the driver released the button, which may be desired functionality, as a failsafe. I'll implement your solution for now.
| if (mDrive.getDriveState() != Drive.DriveControlState.FIND_CUBE) | ||
| { | ||
| mDrive.setWantSearchForCube(); //Begin Searching for cube | ||
| // IF THIS IS NOT CALLED REPEADLY, THEN THE FOLLOWING PART NEEDS TO BE CHANGED |
There was a problem hiding this comment.
onLoop gets called very fast (every 5 milliseconds). See com.spartronics4915.frc2018.loops.Looper (the instance of this class for subsystem loops is Robot.mEnabledLooper.)
| mDrive.setWantSearchForCube(); //Begin Searching for cube | ||
| // IF THIS IS NOT CALLED REPEADLY, THEN THE FOLLOWING PART NEEDS TO BE CHANGED | ||
| // The other solution is for wantSearchForCube to tell someone when he is done, ATM he just default back to OPEN_LOOP when done. | ||
| if (mDrive.getDriveState() == Drive.DriveControlState.OPEN_LOOP) // (Or whatever the state is for standard driver operation) |
There was a problem hiding this comment.
When drive is on target, it doesn't change states. You should add a Drive.atTarget method for this.
| // Drive forward very, very slowly | ||
| if (mHarvester.atTarget()) | ||
| { | ||
| newState = SystemState.IDLE; // Done |
There was a problem hiding this comment.
You should set drive to new DriveSignal(0, 0) here. Remember, IDLE stops controlling other subsystems, and most of the time you don't care what state they are in at the end, but in this case you do.
There was a problem hiding this comment.
Does setting a driveSignal of (0.0, 0.0) return the drive back to user control?
loqoman
changed the title
@pietroglyph @dbadb
I have attempted to make a run-through of a superstructure-drive-robot interaction for the teleop cube-grab. This is the code that I came up with. It is very comment-laden, and most comments will be removed once the final changes are made.
Two things that I could not figure out, and need outside assistance on:
Once we are aligned with the cube, we need to slowly drive forward until it is harvested by the harvester. I could not figure out how to simply drive forward. I did a little digging into '2d translation', and get the feeling that the answer may be hidden there.
In more simple terms, my logic tree is:
Turn to the cube -> Set the harvester into harvesting mode, and the drive to drive slowly forward -> Once the harvester returns a harvested cube stop everything, and return control to the driver.
I noticed the ControlBoardInterface, with an enum of all the buttons, but I could not find how those buttons are assigned to the physical buttons on any controller (aircraft throttle, or Sovic's strange gamepad). Moreover, i'm interested in the assigning of buttons, how is this done? What is the process?
Most of the code here i'm fairly confident on, and should be correct. Apologies for spelling, or any dumb mistakes I should have caught.
Thanks for checking this over!
Darwin C
Edit: Both of my questions have been answered, and implemented. What remains now is for the mode to be tested.