ChargeCommand and cube_0p5_middle_charge auto

[ErikCald]

What is ChargeCommand

ChargeCommand is a command that has 3 states which it cycles through one by one until it reaches the final state.

• It starts in state 0. The condition to move to state 1 is when the gyro pitch/roll value goes above a certain value. This condition means the robot has changed from driving along the floor to driving up the ramp of the charge station.
• It changes from state 1 to state 2 when the gyro pitch/roll value goes back below a certain value. This means the robot has gone past the tipping point of the charging station and the charge station is almost balanced.
• State 2 only runs for a short period of time (currnently 0.1 seconds) and it drives backwards slightly to get rid of all the robot's speed.
• After the command ends, it should be in a SequentialCommandGroup to then schedule a ChargeStationLock command in order to lock the wheels in a pattern that prevents it from rolling off the charging station if it's not balanced.

When we check the roll or pitch values, they're first run through Math.abs() so that we don't have to bother managing positive vs. negative pitch/roll values.

ChargeCommandRoll vs ChargeCommand

ChargeCommandRoll was quickly added in order to balance on the charging station with cube_0p5_middle_charge but we ran out of time to get it to work and just used ChargeCommand. Instead of the roll being managed in a seperate file, there should be a boolean in the parameters of the constructor of ChargeCommand to decide whether to use roll or pitch. If using pitch, the robot must go up the charging station the narrow way. If using roll, the robot must go up the wider way.

• [x] Add a boolean to the constructor of ChargeCommand to decide whether to use pitch or use roll.
• [x] Currently we call SwerveSubsystem.getInstance().getPitch();. Instead, have an if statement that will either use SwerveSubsystem.getInstance().getPitch(); or SwerveSubsystem.getInstance().getRoll(); based on the boolean. Make sure to make this change to all the locations we have getPitch() in this command.
• [x] Ideally, this means ChargeCommandRoll can be removed and replaced entirely by ChargeCommand.

Tasks to improve ChargeCommand

The current solution involves a ProfiledPidController and it's not working. The ProfiledPidControllershould be removed and replaced with constant speed values instead. Note: The ProfiledPidControllerfor theta should be kept, it's working great. It just the X direction one, using the variable name pidControlXthat should be removed.

• [x] Remove pidControlX variable.
• [x] Since pidControlX is being removed, the deltaX parameter of the constructor can also be removed. It should be replaced with a second boolean that will decide whether the robot is going in the positive X direction or the negative X direction. The speeds below should be flipped if the negative X direction is desired. One way to achieve this is to create an integer that is 1 or -1 based on the boolean and to multiple all the speeds by this integer. (int m_flipXDirection)
• [x] Instead of using pidControlX.calculate(currentX, desiredX); to decide how much speed in the X direction is needed, have constant values that are decided by the states. See below tasks:
• [x] Each state should now uniquely call SwerveSubsystem.getInstance().drive(YOUR_X_SPEED_HERE * m_flipXDirection, 0, theta, true, false);
• [x] When state == 0 the X speed should be pretty fast, somewhere between 1.5 m/s and 2 m/s
• [x] When state == 1 The X speed should be pretty slow, somewhere between 0.5 m/s and 1 m/s.
• [x] When state == 2 it does a small jump back because normally it goes slightly too far. This could probably be kept the same. But if it doesn't go far enough you can either increase the speed in state 1 or decrease the jump back in state 2. To reduce the jump back in state 2, either reduce the time of TIME_FOR_REVERSING or decrease the -0.5 m/s in state 2.

cube_0p5_middle_charge auto

This auto is basically already done. It was even tested at humber on the practice field. However the ChargeCommand at the time was inadequate and there wasn't time to get it to work.

This auto starts in the middle, lifts the arm, then drives foward, scores a top cube and final schedules the ChargeCommand to go onto the charging station.

All you need to do is:

• [x] Add a new event marker or replace old event markers for your newly modifed ChargeCommand.
• [x] This marker with your ChargeCommand must drive in the positive X direction and it must use Roll in order to drive onto the charging station with the wide way going up the charging station. (the intake facing towards our drive team)
• [ ] At the moment I can tell there will be an issue with scoring the cube. Right now it opens the gripper and then immediantly runs the ChargeCommand. The intake pistons do not work instantly and take about 0.15 seconds to open. So there should be a 0.3s wait before the ChargeCommand runs. The easiest way to do this is probably just created a "wait" marker, which is actually already in the code.
• [ ] For this auto, consistency is more important then speed. This auto is designed to give you around 5 seconds, maybe even 7 seconds to get onto the charge station and lock the wheels. So when you are tuning this command at North Bay, it should probably take between 4 and 5 seconds to get onto the charge station, or longer if that helps and you have the time.

[weizhang-2706]

Three states with different speeds are reasonable. But I still don't understand how to end at the right balanced spot with the new chargeCommand.

[weizhang-2706]

Since we don't use pid to control X, then we basically is using speed + time duration to control where the robot finally ends on the charge station. I feel this is harder to tune.

[ErikCald]

It's not speed + time duration. It's using the gyro pitch or roll as the core sensor to move accurately. We drive up the ramp, then the moment we cross over the tipping point of the charging station, the gyro senses that, changes to state 2 and then does a reverse voltage to cut all speed and land directly on the center of the charging station.

Position control is not accurate enough for this task, the gyro readings are amazing for this task. This setup of 3 states has proven to work through the humber event, but the position control was giving inaccurate speeds and making it hard for the gyro to take precedence.

It should actually be very easy to tune based on the experiences I had tuning this command on the practice field at humber.

[ErikCald]

Closed by #100