"Flare" Command - Auto-Drive Distance to Target #35
Conversation
ZachOrr
force-pushed
the
zorr/flair
branch
11 times, most recently
from
0b9bd2f to
255aae8
Compare
| // Driver can strafe during this command. Forward should be forward to the robot. | ||
| // Note that this command assumes we're facing the target (use with Heading) | ||
| return new AutoDrive.State( | ||
| output, |
There was a problem hiding this comment.
The driver's forward should be able to control speed if it's less than the desired speed. This would allow the driver to slow the approach, if necessary.
The thing to think about is if the driver pulls "back" while we're moving towards the target how we want to negotiate that input.
There was a problem hiding this comment.
Maybe it's - 100% back is a full halt. They'll still be able to strafe around but they won't move forward. Anything less than 100% back is some percentage of a slow down. Anything forward up to our calculated output overrides the calculated speed. Anything above our calculated speed is ignored (this is so we don't over shoot or come in TOO hot)
src/main/java/frc/robot/commands/teleop/DistanceToTargetCommand.java
Outdated
Show resolved
Hide resolved
ZachOrr
force-pushed
the
zorr/flair
branch
4 times, most recently
from
768b2b3 to
5a8e16c
Compare
ZachOrr
changed the title
ZachOrr
force-pushed
the
zorr/flair
branch
2 times, most recently
from
c662c55 to
5916b15
Compare
| PolarCoordinate robotCoordinate = getRobotCoordinate(); | ||
| distanceController.reset( | ||
| robotCoordinate.getRadiusMeters(), | ||
| chassisSpeedsProvider.get().vxMetersPerSecond |
There was a problem hiding this comment.
I'm almost CERTAIN this LOC is wrong. It's under-thought. Our distance controller's velocity should be set based on the velocity of the robot towards the target. I think this can be done by constructing a triangle with the vx and vy speeds, looking at the robot's current angle from the target, and solving for the hypotenuse (which should be our velocity vector towards the point).
There was a problem hiding this comment.
Is the concern if we are moving at an angle to the target or away from the target the first move towards the target will be too violent? i.e. if we are moving away from the target at 50% speed and we hit the button while the profiled PID will ramp up velocity toward the target that will result in a severe direction changing event and potentially tip the robot.
Shouldn't the profiled PID take this into account though? Not sure, but something we can test since we never tested this command on the move.
There was a problem hiding this comment.
The use case is exactly right - if you're not moving in the direction of the target, we want to ramp down to zero and then back up to a velocity towards the target.
The profiled PID does not take this in to account by default. It'll assume your starting velocity is zero, unless you specify otherwise. https://github.com/wpilibsuite/allwpilib/blob/791770cf6e2b97901333fbd4d02215380e2a535c/wpimath/src/main/java/edu/wpi/first/math/controller/ProfiledPIDController.java#L363-L371
There was a problem hiding this comment.
I'm axing the velocity handoff stuff - since I'm still not confident on the math of this one. I need to do some more research before I feel confident landing this. If we have drivers driving and see not having a velocity handoff as really making the move in to flare jumpy we can prioritize getting it back in.
|
Okay got the last piece of this. For finding the trajectory of the robot towards/away from the point- The theta of our polar coordinate allows us to construct a triangle that represents our velocity towards/away from the point. If the robot is at a 45 degree angle - the hypotenuse of both the vx and vy vectors will be our velocity towards/away from the point. If the robot is not at a 45 - we're working with one side of a triangle and need to find the other in order to construct the velocity. In the case that the robot is 0-45, we're moving further on the X direction than the Y, so our vx is the leg we can use. In the case of 45-90, we're moving further on the Y direction than the X, so our vy is the leg we can use. In the case of our 0-45, our angle can be whatever the angle is. In the case of our 45-90, we need to look at the angle relative to the 90 as opposed to the zero (our Y axis as opposed to our X axis) because the vy leg would be "too short" to reach the angle starting from zero. Once we have a side and an angle, we can calculate our new side using - And then once we have our final side, we can calculate our hypotenuse - which will be our velocity towards/away from the point. Fairly straightforward - I think the "reference from 90" is the only arc of work I need to think about to make sure it's a clean, readable solution. |
ZachOrr
force-pushed
the
zorr/flair
branch
3 times, most recently
from
2952302 to
31b4ab0
Compare
| distanceController = new ProfiledPIDController(2, 0.0, 0.0, | ||
| // Use max speed above. Come up to max speed in one second | ||
| new TrapezoidProfile.Constraints( | ||
| Units.inchesToMeters(120), | ||
| Units.inchesToMeters(120) / 2 | ||
| ) | ||
| ); |
There was a problem hiding this comment.
These values were looking pretty good today. We're still jumpy towards the end. We probably need to look to decrease our P value.
ZachOrr
force-pushed
the
zorr/flair
branch
13 times, most recently
from
3e034a0 to
574dfe3
Compare
The "flare" command is used to drive the robot towards a target (ex: the Hub) until it reaches a certain distance. As the robot reaches the distance, it will slow down to a stop. The driver can strafe up-to the target to change the trajectory, as well as strafe around the target once the distance is reached.