QCC2 Gyro and PID Control Code

@jmmckinney ◉.◉ that’s so freaking cool. Thank you so much for all this!!! I’ll definitely use it for next season. :mindblown:

@jmmckinney - I took (another) look at Vamfun’s gyro blog post, and maybe the 87th time is a charm; I think I’m finally getting what he’s talking about.

HOWEVER, he uses some abbreviations that I don’t recognize. Can you (or, well, anyone) tell me what “A2D” and “A/D” stand for in his description?

@biglesliep I think he is referencing the Analog to Digital Converter and uses the variable A2D to represent the raw value there.

Both mean Analog to Digital Converter or Conversion, depending on context.

Edited to add: @mbrh is correct; the variable refers to the output (or result) of the Analog to Digital conversion.

@vamfun’s stuff is good, but it contains more than you need to understand how the gyro works. There are some extraneous details (like the scaling for different voltage references) which are more than you need to know for using the gyro, but very important if you’re the one working it out the first time.

I have two questions about this:

  1. How would you implement driver control with this? He stated that motor[port2] will no longer work, but it’s alternative, profileSetVelocity (port2, 1500), has a very high slew rate, which seems bad for driver control.

  2. The line: profileSetMaxVelocity (port2, 1728); //120rpm * 360 ticks per rev / 60 seconds = 1728 ticks per second

(120*360)/60 = 720, not 1728. Am I missing something obvious???

You picked up the wrong line of code. Should be:

//sets the raw output of the motor. Equivalent to motor[port2] = value;
//You need to use this function to set the motor value of a profiled motor, else it will be overridden
//This value is linearized using a TrueSpeed lookup table (63 is ~50% speed, 32 is ~25% etc...)
profileSetMotorOutput (port2, 127);

Beyond that, a “very high slew rate” would mean the speed ramped up quickly, and you are concerned that 1 second is too long. So your worry is a “low slew rate” actually.

Fix this by setting the acceleration time to whatever you want, including zero for instantaneous change. See here:

//time in milliseconds to ramp up to max speed for a move
//think of this like a "slew rate", or a 0-max speed time
//defaults to 1000, or 1 second from 0 to max speed
profileSetAccelerationTime (port2, 1000);

I haven’t looked inside the code, but I expect this is a typo.

Thanks for the reply, that’s super helpful.

So would profileSetMotorOutput (port2, vexRT[Ch2]); work for drivercontrol?

Yes, that would do it. Just be sure to call profileSetAccelerationTime on port2 with some time (in milliseconds) that works for your driving. Maybe, say, 50.


Alright thank you, that is very helpful.

Yes that is a typo, thanks. It should have been 120 * 2.4 * 360 … (for turbo gearing), which would have given 1728. I can’t go back and edit at this point which is unfortunate.

@1320D_Programmer you do have another option if you want to use motor[port] after you’re done with the lib. You’d just have to stop the motion profiler task. Just make sure you aren’t using any controls functions (like for an arm or something) from it if you’re going to stop the task.


So I had two more questions about this.

First, you said that 2.4 was the constant for turbo gearing. What would the constant be for High-Speed gearing?

Second, I’ve been getting an error when trying to initialize the gyro.
When I use the line:
gyro_init(Gyro, 1, 0);

I get the error:
Error:’&’ Invalid parameter for reference variable. Call to ‘gyro_init’. Parameter: ‘Gyro & gyro’ is ‘Gyro’ of type ‘Gyro’.

The Gyro Initialization void is this:

gyro_init (Gyro gyro, int port_number, char gyro_flipped) {
gyro.port_number = port_number;
gyro.config.gyro_flipped = gyro_flipped;
gyro_calibrate (gyro);

I’ve tried several things, nothing gets rid of the error. What am I doing wrong?

1.6 is what you would use for high speed gearing.

Think of it this way -
Torque gearing is 100 rpm - or 1.0 as the constant default value
High speed is 160 rpm - or 1.6 * the default
Turbo is 240 rpm - or 2.4 * the default

As for the errors, I’ll let Jason answer that…

Thank you, I didn’t realize it was that simple.

@1320D_Programmer just to add to this, torque 393s run at 100rpm at their nominal 7.2 volts. When you charge a 7.2v battery, the voltage of the battery will actually be something like 8.4v. A 7.2v battery at 7.2 volts would be dead. At 8.4 volts, the motors run at something more like 120rpm, so that’s what my baseline calculations were modeled after.

120rpm * gear_ratio * 360 ticks_per_revolution / 60 seconds = ticks_per_second

for torque: 120 rpm * 1.0 * 360 ticks_per_rev / 60 seconds = 720 ticks per second

for speed: 120 rpm * 1.6 * 360 ticks_per_rev / 60 seconds = 1152 ticks per second

for turbo: 120 rpm * 2.4 * 360 ticks_per_rev / 60 seconds = 1728 ticks per second

You need to create a gyro variable that you pass into those functions.

#include "NERD_Gyro.c"

Gyro gyro;

gyro_init(gyro, in1, false);

//then you can use this to get the rate of the gyro

Also, make sure you’re using the logical port number (in1, in2, … in8), not just an integer number when you initialize the gyro, otherwise you’ll set it to the wrong port.


Thank you that’s really helpful.

@jmmckinney Really cool! Pardon me, but can you make another example code with the new library and motionplanner? The example code before doesn’t work.