We are doing a field oriented drive system on our robot this year and have been coding the IMU and other commands for field oriented driving. One of these commands is a RotateToAngle command. When you press a button on the controller it will rotate back to zero (or whatever you pass into it). This works fine when in a separate command, but when you run the command, you can’t drive normally while it is rotating.
My question is: Is there a way you can run both commands in parallel but able to drive forward and rotate to zero at the same time. Would a command group work? Would I have to implement the rotate to angle command into the driveCommand file?
I’m not sure this is doable in two commands because both commands need to be controlling the same type of behavior. I think you’ll need to combine these behaviors into one command and run it when the button is pressed.
You might be able to store the last sets of drive values in the subsystem and control the separate parts with the two different commands, but I think you’re better off combining them.
For example, if the rotate to angle command said the front right wheel should have a speed of -1 it sets the speed. But, what if the drive command tells the same wheel to have a speed of 1 at the same time? Which one has priority?
Both will attempt to tell it to drive at the different speeds and you will get erratic behavior from the motor, that is why it is a bad idea to have multiple commands attempting the run the same motors.
I assume you have a mecanum drivetrain? (there is no “h” in “mecanum”)
If you want to drive in a straight line in any particular direction, and simultaneously rotate the bot to a desired angle, you can do that using mecanumDrive_Cartesian. Feed your gyro to the gyroAngle parameter; supply the x and y components of the desired straight-line field-centric motion to the x and y parameters; and feed the output of a closed-loop controller to the rotation parameter. The input to the controller will be your desired angle (setpoint) and your gyro reading (process variable).
and finally, and perhaps most importantly:
Mechanical workmanship is of paramount importance. Make sure your drivetrain is built and aligned correctly before attempting #2 above. If you’re not sure what “built and aligned correctly” means in the context of mecanum drive, I encourage you to ask before proceeding.
Yeah we have a mecanum drive train. The reason i said that with an ‘h’ is because in our code we have Robot::driveBaseSub->MechDrive(XAxis, YAxis, RotateAxis, IMU_Yaw); I should probably change that soon…
Yeah, OK. I was thinking about it in a more complicated way i guess. I just really tried to copy-paste from our original rotate to angle command into the drive command and it didn’t go so well. I will have to try that. Also, by closed loop, you mean PID? We have not implemented a PID loop into the robot yet, but plan on doing so once we have the basics down. Right now, if the robot overshoots the 0 degrees it rotates to, it just turns in the opposite direction with a slower speed. We have a tolerance of about 2 degrees on it right now which seems good for what we need.
Yeah, I get what you mean. We have field-centric drive working on a robot we used last year and then just stripped down to the bare minimum for what we need. The wheels are not perfectly aligned just from usage over the season and unseen measurement error by us. I will fix this issue soon.