Ideas for 1.5 piece engage command

[edf42001]

Just of collection of things my brain has thought about:

• [x] Add a slew rate limiter to TurnToNAngleCmd just like drive for distance. The more buttery smooth it is, the more consistent it will be
• [x] The line currentAngle = modAngle(currentAngle); in TurnToNAngleCmd is unnecessary because of the lines

  double error = targetAngle - currentAngle;
  error = modAngle(error);

  which btw you could combine into double error = modAngle(targetAngle - currentAngle);

  • [x] While you're at it, you could delete any extra comments / spaces / general clean up the command
• [x] You should call driveTrain.resetOdometry at the start so it lines up with the sim field and we can see if it is working
  • [x] The distance it drives might be off I don't know if we calibrated the encoders, but that is fine for now
  • [x] where should the robot start? Discuss with drive team exactly which cone node the robot will start at (there's two in front of the charging station i think)
• [ ] What angle should the robot turn to? What if it is on the other alliance << tricky tricky
• [ ] I predict we may want to lower the top speed from 0.5 for TurnToNAngleCmd. Grok turns very fast
• [ ] Once done, how many seconds is the whole routine? I have ideas for speeding it up if need be

[edf42001]

If you think about it, yesterday was the first time we really got to test autos for real. What a learning experience! My take aways were: Don't be afraid to make big changes, and don't be afraid to go slow. Once it works it can be dialed in. (You might have took away different lessons :+1:)

Here's a few random thoughts.

• To flip the angle when the alliance flips for TurnToNAngle, you could do it in the initialize method (not the constructor, as the robot doesn't know what alliance it is on yet when the constructor is run). But something like this. Then you don't have to do it manually for every time you turn.

if (on red alliance) {
targetAngle = -targetAngle;
}

• I suggest use all drive straight for distances for the two piece. Maybe even for the one piece. More consistent.
  • To squeeze even more consistency out of it, add a parameter to the constructor for the "angle to drive straight at" (currently this is called startAngle) and is just where the robot is when the command starts. But imagine you turn to -5 degrees, but it ends at -6. Then it would drive straight at -6. You can force it to always drive to the exact same angle with this method. If you do you'll need to flip the angle in initialize if on red alliance like above.

At the end, if you want to save time you could:

• Add a parameter to the constructor of DriveStraightForDistance that sets the speed limit. Then you could go faster for two piece (since the ground is flat), but leave the 1.5 the same speed.
• Lower the elevator/arm while the drive backwards happens after scoring the first cone. This could save up to 1 second based on the videos. (This is what elle does in teleop all the time so it is safe)
• Raise the arm/elevator halfway through driving forward when scoring the second cube. (Could do this with parallel and sequential groups and wait commands to trigger it after a certain time)

Other things

• If the cube intake works better then cone, you could switch 1.5 to cube as well.

• I think there's a small problem with your deadline command group

[edf42001]

I think you've successfully completed this issue.Only thing would be to crank the dial up on the slowness at which the robot approaches picking up a 1.5th cone, to avoid knocking it over. Or try a tipped cone. If 1.5 cone is an extra autonomous you wanted.