PID Servo
The Quarto can turn into a proportional-integral-differential (PID) Servo with just 20 lines of code! Let's see how.
Setup
The setup function is run once at start-up and is used to configure the ADC. For the servo, we want use ADC channel 1, so we use the function configureADC to configure it. We want the ADC to fire every 1µs and to have a voltage range of ±1.25V.
void setup(void) {
configureADC(1,1,0,BIPOLAR_1250mV,getADC1); // Have ADC take measurement every 1us, ±1.25V range
}
With the Arduino 2.0 IDE, if we hover over the function configureADC, a window pops up showing the function arguments:
First argument, fire_every_us tells the ADC how often to fire. In this example, we want to run the ADC at its maximum rate of 1MSPS (1 Mega Samples Per Second) so we fire every 1us, so we set this parameter to one. If we wanted to sample every 5us (or 200 kHz sample rate), this should be set to 5.
The second argument, fire_delay delays when the ADC fires. This has no real meaning when only using one ADC channel, but if you configure multiple ADC channels, to control the timing relationship between different channels.
The next argument, scale sets the ADC voltage range. Valid inputs are BIPOLAR_1250mV BIPOLAR_2500mV, BIPOLAR_5V and BIPOLAR_10V. In this case, we want the ADC range to be ±1.25V, so we set it to BIPOLAR_1250mV .
Finally, the last argument, function is the function to call when there is ADC data. We will call this function getADC1 although the name does not matter.
We will also define a setpoint which is the target value we want to see at the ADC input. We will use a macro to do this so if we want to change the setpoint, we only have to change the value in one place. For this example, we will have the SETPOINT equal to 0.25
#define SETPOINT 0.25
Function when ADC Data is Available
We now need to define the function that is called when the ADC gets data. On our case this function is called getADC. Here's the code for that function:
void getADC1(void) {
static double integral = 0;
static double prev_adc = 0;
double newadc = readADC1_from_ISR(); //read ADC voltage
double prop = (newadc-SETPOINT) * 1.975; //proportional
integral += (newadc - SETPOINT) * 0.01; // integral gain
double diff = ( newadc - prev_adc) * 0.00001; // turn diff down for accuracate BW measurement
double newdac = prop + integral + diff;
writeDAC(1,-newdac); //invert for negative feedback. Write to DAC channel 1.
prev_adc = newadc; //store new adc value for differential calculation
}
The first two lines of the function define two double-precision floating point numbers integral and prev_adc. These two definitions start with static which means that these variables do not disappear after the function runs, but they are kept for subsequent runs. So for the first run, integral and prev_adc start at 0. If at the end of the first run, integral is 1.2345, then integral will start with that value for the next run, and so on.
The next line reads the ADC1 value and stores it into the double newadc.
Any function that runs when new ADC data is available must execute the function readADCX_from_ISR()!
When the ADC has new data, it fires an interrupt. That interrupt executes the function configured in configureADCX and that function must clear the interrupt, which is done by running readADCX_from_ISR(). Otherwise the function will loop forever and the Quarto will crash.
The next set of commands calculate the proportional, integral and differential (PID) values. The proportional value is the difference between the ADC value (newadc) and the SETPOINT, multiplied by a scale constant, in this case 1.975. The integral calculation is similar, but with a different scale constant (0.01) and we use a += instead of an = to assign it a value so it sums the new calculation with the previous value of integral. Next, the differential looks at the difference between the current ADC value (newadc) and the previously measured ADC value (prev_adc) and multiplies that by the scale constant (0.00001). Finally, we sum these two values together in the new value newdac.
The second to last line of the function writes the newly calculated PID value to the channel 1 DAC using the writeDAC1 command. There is a minus sign in front of the argument newdac to invert the value, which we need to do to provide negative feedback.
The last line stores the most recent ADC measurement (newadc) in the variable prev_adc for use the next time this function is run.
Final Code
Putting this altogether, we have:
#define SETPOINT 0.25
void setup(void) {
configureADC(1,1,0,BIPOLAR_1250mV,getADC1); // Have ADC take measurement every 1us, ±1.25V range
}
void getADC1(void) {
static double integral = 0;
static double prev_adc = 0;
double newadc = readADC1_from_ISR(); //read ADC voltage
double prop = (newadc-SETPOINT) * 1.975; //proportional
integral += (newadc - SETPOINT) * 0.01; // integral gain
double diff = ( newadc - prev_adc) * 0.00001; // turn diff down for accuracate BW measurement
double newdac = prop + integral + diff;
writeDAC(1,-newdac); //invert for negative feedback
prev_adc = newadc; //store new adc value for differential calculation
}
Data
Using this code, if you connect the channel 1 DAC output to the ADC channel 1 input, the Quarto can lock to itself and it will oscillate at over 100 kHz, as shown below. (Just lower the proportional gain scalar from 1.975 to stop the oscillation)
Feature: Integrator Hold
What if you wanted your servo to support Integrator Hold (sometimes called Sample & Hold) where the servo output stays constant for a period of time, and then the servo re-engages lock later? We can implement this where a trigger line will control if the servo should be active or just holding its value. The first part is to define a variable servoActive that will store if the servo is active. Then we setup an interrupt to fire whenever trigger 1 changes its value and have it update the servoActive variable. We can do this with:
bool servoActive;
void setup(void) {
triggerMode(1, INPUT); // Set trigger1 as input
servoActive = triggerRead(1); //set servoActive variable initially based on trigger 1 level
enableInterruptTrigger(1,BOTH_EDGES,&servo_en); //Run servo_en function on any change to trigger 1
configureADC(1,1,0,BIPOLAR_1250mV,getADC1); // Have ADC take measurement every 1us, ±1.25V range
}
void servo_en(void) {
servoActive = triggerRead(1);
}
Now all we have to do is wrap the servo functionality around an if statement that looks at the servoActive variable. Below is the complete code:
bool servoActive;
void setup(void) {
triggerMode(1, INPUT); // Set trigger1 as input
servoActive = triggerRead(1); //set servoActive variable initially based on trigger 1 level
enableInterruptTrigger(1,BOTH_EDGES,&servo_en); //Run servo_en function on any change to trigger 1
configureADC(1,1,0,BIPOLAR_1250mV,getADC1); // Have ADC take measurement every 1us, ±1.25V range
}
void servo_en(void) {
servoActive = triggerRead(1);
}
void getADC1(void) {
static double integral = 0;
static double prev_adc = 0;
double newadc = readADC1_from_ISR(); //read ADC voltage
double setpoint = 0.25; // Target value for ADC to read
if (servoActive) {
double prop = (newadc - setpoint) * 1.975; //proportional
integral += (newadc - setpoint) * 0.01; // integral gain
double diff = ( newadc - prev_adc) * .00001; // turn diff down for accuracate BW measurement
double newdac = prop + integral + diff;
writeDAC(1,-newdac); //invert for negative feedback
}
prev_adc = newadc; //store new adc value for differential calculation
}
void loop(void) {
static unsigned long lastrun = 0;
if (millis() > lastrun) { //Run once every 1000ms
lastrun = millis() + 1000;
toggleLEDGreen();
//Serial.println("This runs every second");
}
}
This is one example of adding more functionality to the basic PID servo. The Quarto could be programmed to do double integration, or feed-forward, or compensate for temperature drift, or unlock-detection, or auto-locking, amout other features.