Interact & Control a D2-125 Laser Servo

The Serial Commands example shows how to interact with Quarto through the serial port to read and set values and settings. But for graphically interacting with the Quarto, we want to use qControl instead. If you aren't familiar with it, qControl is a web-based application that can dynamically create interactive elements for controlling and viewing data on the Quarto -- for more details, please read the qControl documentation. In this example, we will use it to control a Vescent D2-125 Laser Servo. This will enable the high-bandwidth, low-noise analog performance of the D2-125 to be paired with the flexibility and digital control of the Quarto. This example will also use Protothreads -- if you aren't familiar, please take a look at the threading example -- and SmartData.

Hardware Setup

For the Quarto to control the D2-125, it needs to control the D2-125's "Laser Jump Amplitude" and "Absolute Jump TTL" BNC inputs. Additionally, for the Quarto to monitor the D2-125 servo output and error signal, it needs to read those signals. For this example, please make the following BNC connections:

Additionally, put the D2-125's switch labelled "Laser State" into the "Lock" (up) position. This turns on the D2-125's servo, but will let the Quarto disengage the lock by applying a logic signal to "Absolute Jump TTL".

Software

The software running on the Quarto will do a few things:

1. When in ramp mode, it will disengage the D2-125's servo and generate a ramp on the servo output (via Laser Jump Amp). Additionally, it will record and plot the error signal as the servo output is sweeping.

2. When in servo mode, it will engage the D2-125's servo and record and plot the error signal.

3. When in servo mode, it will calculate the RMS of the measured error signal.

4. When in servo mode, measure the average voltage output on the Servo Output of the D2-125.

The software implementation will use multiple threads to do this processing. The main code will have 3 threads:

1. ADC1: This thread runs whenever there is new ADC1 data (DC Error)
2. ADC2: This thread runs whenever there is new ADC2 data (Servo Out)
3. CalcRMS: This thread runs from the main loop and, as needed, calculates the RMS on a set of Error Signal measurements.

Control Diagram

For each of the main threads, below is a description of what they will do, followed by a flowchart showing that information visually.

AD1

When new ADC1 data comes in, first check if servoMode is true (servo mode) or false (ramp mode). If in servo mode, update servoMode based on user input (ie switch to ramp mode if requested) and then check if the Quarto is processing data (processData is true). If it is, then stop. If it is not processing data, then it needs more data, so add this data point to the data array and check if it now has a full set of data. If it does, set processData to true. Then stop.

In the case where the Quarto is in ramp mode, update the ramp output and then add the data point to the data array. Then check if the ramp has completed a full cycle (output is back of center of the output after going to its minimum and maximum value). If the ramp has completed a full cycle, update servoMode based on user input. Then stop. By only updating servoMode when the ramp has completed its cycle, it means that the Quarto will wait for the ramp cycle to complete before engaging the servo.

ADC2

When new ADC2 data comes in, first check if servoMode is true (servo mode) or false (ramp mode). If in servo mode, append data to the data array, and add the adc data to a summer and increment a counter. Then, if the counter is 10, update the servo output data with the average from the summer and then reset the summer and the counter. Then stop.

If the Quarto is in ramp mode, do nothing.

CalcRMS

Unlike the other two threads, which are started when there is new ADC data, this thread is run from the main loop and runs as often as possible while also running other threaded functions (such as core qControl functionality). First, this thread, waits until processData is true. Then, it runs an algorithm to calculate the RMS from the data array, sets errorRMS with that calculated value, and then sets processData to false. Finally, it restarts and again waits for processData to be true.

Load qCommand

Now that we have an outline of the program code, we'll start writing the code, piece by piece. First, we need to include the qCommand library and instantiate it and load the Protothreads library:

#include "protothreads.h"
#include "qCommand.h"
qCommand qC;

Similar to what was done in the Serial Commands example, we need the Quarto to process commands from qControl so we call the function readBinary that is part of the instantiated qCommand object qC in the main loop:

void loop() {
  qC.readBinary();
}

Control Variables

In this example, we want to be able to turn on and off the D2-125 servo, and have the D2-125 output a ramp when not in servo mode. For the ramp, we want to control the amplitude and the offset. So, we have three inputs, or controls, for the user to set. Will will create a global variable for each of these controls and assign them as qCommand variables:

SmartData<bool> servoEnable(false); 
SmartData<float> servoOffset(1.2);
SmartData<float> rampAmp(.15);

void setup() {
  qC.assignVariable("Servo", &servoEnable);
  qC.assignVariable("Offset", &servoOffset);
  qC.assignVariable("RampAmp", &rampAmp);
}

While we could assign these variables to direct types (boolean and float), by setting them to SmartData we can put limits on when and how they update and know that qCommand will update when their value changes. For a variable like servoEnable which only changes via user-intervention and has no limits on how it is set, this is unnecessary but using a SmartData type gives flexibility for adding future functionality.

Indicator Variables

Additionally, we want to display some information from the Quarto. In this example, we will show the ramp frequency along with the servo output voltage and the root-mean-square (RMS) of the error signal and the error signal itself.

Any read-only variable is automatically treated by qControl as an indicator, as is any data array. But assignVariable can also set a read_only property for variables that change in the Quarto program but should not be changed by the user, as we'll see the code below:

const uint16_t DataPointsHalf = 500; //Data points from middle to top of ramp -- half the total
const uint16_t updateRate = 2; //units us. Update every 2us
const float RampFreq = 1e6 / (float) (DataPointsHalf*2*updateRate);

SmartData<float> ServoOut(NAN);
SmartData<float> errorRMS(NAN);
float ErrorData[DataPointsHalf*2];
SmartData<float*> Data(ErrorData);

void setup() {
  /* previous code */
    
  qC.assignVariable("RampFreq", &RampFreq); //automatically treated as read_only by qControl
  qC.assignVariable("Servo Output", &ServoOut, true); //set read_only true
  qC.assignVariable("ErrorRMS", &errorRMS, true); //set read_only true
  qC.assignVariable("ErrorSignal", &Data); //automatically treated as read_only by qControl
}

Control Servo Output

Because the Quarto's Analog Output #1 is connected to the D2-125's "Laser Jump Amplitude", the Quarto can control the D2-125's servo output when the servo is disengaged. To control this value, we will write a helper function that handles the inverting and scaling between the Laser Jump Amplitude and the D2-125's servo output:

void setD2Output(float value) {
  writeDAC(1,-value/.91); // 1V on Laser Jump Amp put -0.91V on D2-125 output, so rescale
}

Setters

One powerful capability of SmartData objects is the ability to use setters to control how an variable can change. We will use this to prevent setting an offset outside of the -10V - 10V and to reduce the ramp amplitude such that offset plus the max ramp is in that range as well. Here's the code:

SmartData<float> servoOffset(1.2);
SmartData<float> rampAmp(.15);

float coerceRampAmp(float newV, float oldV) {
  if ( newV >= 20.0 - 2.0*abs(servoOffset.get() )) {
    // RampAmp can be 20V when offset is 0V, only 10V when offset if -5V, (sweeping from 0V to -10V)
    return 20.0 - 2.0*abs(servoOffset.get());
  } else {
    return newV;
  } 

float coerceOffset(float newV, float oldV) {
  if (newV > 10) newV = 10;
  if (newV < -10) newV = -10;
  
  if (rampAmp.get()/2.0 + newV >= 10.0) {
    rampAmp.set((10.0 - newV)*2.0); // reduce Ramp Amplitude
  }
  if (rampAmp.get()/2.0 - newV >= 10.0) {
    rampAmp.set((10.0 + newV)*2.0); // reduce Ramp Amplitude
  }

  if (servoEnable.get() == false) {
    //If Ramp is running (servo not), then adjust servo output
    setD2Output(newV);
  }
  return newV;
}
    
void setup() {
  /* previous code */

  servoOffset.setSetter(coerceOffset);
  rampAmp.setSetter(coerceRampAmp);
}
    

Read the Servo Output

When the servo is engaged, we want to read the Servo Output voltage, which is connected to the Quarto's Analog Input #2. We setup the Quarto to read Analog Input number 2 every 50ms and, if the servo is on, update the ServoOut variable with the average of the last 10 measurements. Here's the code:

SmartData<float> ServoOut(NAN);

void setup() {
  /* previous code */
  configureADC(2,50000,1,BIPOLAR_10V,getADC2); // Have ADC2 take measurement every 50ms, ±10V range
}

void getADC2(void) {
  static uint8_t counts = 0;
  static double sumTotal = 0;
  double newadc = readADC2_from_ISR(); //read ADC voltage

  if ( servoEnable.get() == false) {
    //If servo is off, set ServoOut to servoOffset control
    ServoOut.set(servoOffset.get());
    counts = 0;
    sumTotal = 0;
  } else {
    counts++;
    sumTotal += newadc;
    if (counts >= 10) {
      //after 10 measurements, set ServoOut to the average of them and restart the averaging
      ServoOut.set(sumTotal/10.0);
      counts = 0;
      sumTotal = 0;
    }
  }
}

Engage the Servo

The next step is to control the D2-125's servo. Normally, when the D2-125's front-panel switch labelled "laser state" is in the "lock" (up) position on the D2-125, the PIID loop is engaged and the servo is on. However, by applying a TTL signal to the "Absolute Jump TTL" on the front panel of the D2-125, the servo can be disengaged and the output will be set by the voltage on the "Laser Jump Amplitude". The Quarto will use this to turn the servo on and off by setting trigger 1 to high (servo off) or low (servo on). However, for code simplicity, we will integrate this control with the reading the Error Monitor and generating the Ramp, as described in the next section.

Generate Ramp / Read Error Monitor

In addition to controlling the D2-125's servo mode on and off, the Quarto needs to generate the Ramp Output and to read the Error Monitor. For simplicity, we will generate the ramp synchronously with reading ADC1. This means putting the ramp logic and the servo control logic inside getADC1, the function that is called when ADC1 has new data. Additionally, we will introduce a new global variable, processingData for setting if the Quarto is calculating the RMS and should pause data collection.

Here's the code:

const uint16_t DataPointsHalf = 500; //Data points from middle to top of ramp -- half the total
const uint16_t updateRate = 2; //units us. Update every 2us
float ErrorData[DataPointsHalf*2];
SmartData<float*> Data(ErrorData);

bool processingData = false;

SmartData<bool> servoEnable(false); 
SmartData<float> servoOffset(1.2);
SmartData<float> rampAmp(.15);
SmartData<float> errorRMS(NAN);

void setup() {
  /* previous code */
  configureADC(1,updateRate,0,BIPOLAR_10V,getADC1); // Have ADC take measurement every 2us, ±10V range
}

void getADC1(void) {
  static bool ramp_running = false; // local variable if ramp is running
  static bool ramp_up = true; // local variable for up vs down part of ramp
  static int16_t rampPos = 0;
  static bool sampleData = false;  
  static bool collectingData;

  double newadc = readADC1_from_ISR(); //read ADC voltage
  
  if ( Data.isFull()  ) {
    sampleData = false; // stop sampling data whenever Data is full
  }

  if (ramp_running) {
    //Ramp Mode
    if (sampleData) {
      Data.setNext(newadc); //Store data point in Data array
    }
    if (ramp_up) {
      rampPos++;
      if (rampPos >= DataPointsHalf) {
        //end of ramp, turn around
        ramp_up = ! ramp_up;
        if ( Data.isEmpty()) {         
          sampleData = true; //only start (re)sampling data when data empty and at start of ramp
        }
      }
    } else {
      rampPos--;
      if (-rampPos >= DataPointsHalf) {
        ramp_up = ! ramp_up;
      }
    }

    if ( (rampPos == 0) && ramp_up && (servoEnable.get() == true )) {
      //at end of ramp and servoEn has been set true, stop the ramp
      ramp_running = false;
      collectingData = true;
      triggerWrite(1,false); //Turn on Servo
    }

    //calculate ramp value
    float new_value = servoOffset.get() + rampAmp.get()/ (float) DataPointsHalf/2.0*rampPos;
    setD2Output(new_value);
  } else {    
    //Servo Engaged
    if (servoEnable.get() == false) {
      //Request made to turn servo off, 
      rampPos = 0;
      ramp_up = true;      
      triggerWrite(1,true); //Turn off Servo      

      if (processingData == false) {
        //once processData completed, start running ramp
        ramp_running = true;
        errorRMS.set(NAN);
      }
    } else {
      if (collectingData) {    
        Data.setNext(newadc);
        if (Data.isFull() == true) {
          //stop collecting data and start processing it
          collectingData = false;
          processingData = true;       
        }
      } else {
        if (processingData == false) {
          //processing done, can restart
          collectingData = true;
        }
      }
    }
  }
}

Calculate Error RMS

Finally, we need the thread to run that calculates the RMS of the error. getADC1 is setup to set processingData to true to signal that the data is ready for processing and waits for this new thread to set processingData to false when it is compete.

void loop() {
    PT_SCHEDULE(calcRMS());
}

PT_THREAD(calcRMS(void)) {
  PT_FUNC_START(pt);
  static uint16_t i;
  static float total = 0;
  static float average = 0;
  PT_WAIT_UNTIL(pt,processingData); //wait until processData is true
  for(i=0;i<DataPointsHalf*2; i++) { //calculate sum of Error
    total += ErrorData[i];
    PT_YIELD(pt); //yield during calculation for other threads
  }

  average = total / (DataPointsHalf*2);
  PT_YIELD(pt);
  total = 0;
  for(i=0;i<DataPointsHalf*2; i++) { //use average to calculate RMS
    total += (ErrorData[i] - average) * (ErrorData[i] - average);
    PT_YIELD(pt);
  }

  total = sqrt(total) / DataPointsHalf / 2;
  errorRMS.set(total);
  //PT_SLEEP(pt,50); // uncomment to slow down how often RMS is updated
  processingData = false; // calculation done, set processingData to false
  PT_RESTART(pt); //Restart Thread
  return PT_YIELDED; // Exit thread without stopping future runs of the thread
  PT_FUNC_END(pt);
}

Final Code

Click here for full code

#include "qCommand.h"
#include "protothreads.h"

qCommand qC;

SmartData<bool> servoEnable(false); 
SmartData<float> servoOffset(1.2);
SmartData<float> rampAmp(.15);

const uint16_t DataPointsHalf = 500; //Data points from middle to top of ramp -- half the total
const uint16_t updateRate = 2; //units us. Update every 2us
const float RampFreq = 1e6 / (float) (DataPointsHalf*2*updateRate);

SmartData<float> errorRMS(NAN);
SmartData<float> ServoOut(NAN);
float ErrorData[DataPointsHalf*2];
SmartData<float*> Data(ErrorData);

bool processingData = false;

void setD2Output(float value) {
  writeDAC(1,-value/.91); // 1V output yields -0.91V on D2-125 output
}

float coerceOffset(float newV, float oldV) {
  if (newV > 10) newV = 10;
  if (newV < -10) newV = -10;
  
  if (rampAmp.get()/2.0 + newV >= 10.0) {
    rampAmp.set((10.0 - newV)*2.0); // reduce amplitude
  }
  if (rampAmp.get()/2.0 - newV >= 10.0) {
    rampAmp.set((10.0 + newV)*2.0); // reduce amplitude
  }  

  if (servoEnable.get() == false) {
    //ramp not running, so adjust offset manually
    setD2Output(newV);
  }
  return newV;
}

float coerceRampAmp(float newV, float oldV) {
  if ( newV >= 20.0 - 2.0*abs(servoOffset.get() )) {
    //Serial2.printf("Coerce from %f to %f\n",newV, 20.0 - 2.0*abs(servoOffset.get()));
    return 20.0 - 2.0*abs(servoOffset.get());
  } else {
    return newV;
  } 
}

PT_THREAD(toggleLED(void)) {
  PT_FUNC_START(pt);
  while(true) {
    PT_SLEEP(pt, 500);
    toggleLEDBlue();
  }
  PT_FUNC_END(pt);
}


PT_THREAD(calcRMS(void)) {
  PT_FUNC_START(pt);
  static uint16_t i;
  static float total = 0;
  static float average = 0;
  PT_WAIT_UNTIL(pt,processingData);  
  for(i=0;i<DataPointsHalf*2; i++) {
    total += ErrorData[i];
    PT_YIELD(pt);
  }

  average = total / (DataPointsHalf*2);
  PT_YIELD(pt);
  total = 0;
  for(i=0;i<DataPointsHalf*2; i++) {
    total += (ErrorData[i] - average) * (ErrorData[i] - average);
    PT_YIELD(pt);
  }

  total = sqrt(total) / DataPointsHalf / 2;
  errorRMS.set(total);
  processingData = false;
  //PT_SLEEP(pt,100); //update is too fast visually
  PT_RESTART(pt); //Restart Thread
  return PT_YIELDED; // Exit thread without stopping future runs of the thread
  PT_FUNC_END(pt);
}

void setup() {
  triggerMode(1,OUTPUT);
  triggerWrite(1,HIGH);
  qC.assignVariable("Servo", &servoEnable);
  qC.assignVariable("Offset", &servoOffset);
  qC.assignVariable("RampAmp", &rampAmp);  
  qC.assignVariable("RampFreq", &RampFreq);
  qC.assignVariable("ErrorSignal", &Data);
  qC.assignVariable("ErrorRMS", &errorRMS, true);
  qC.assignVariable("ServoOutput", &ServoOut, true);

  servoOffset.setSetter(coerceOffset);
  rampAmp.setSetter(coerceRampAmp);

  configureADC(1,updateRate,0,BIPOLAR_10V,getADC1); // Have ADC take measurement every 2us, ±10V range
  configureADC(2,50000,1,BIPOLAR_10V,getADC2); // Have ADC take measurement every 50ms, ±10V range
}


void loop() {
  // put your main code here, to run repeatedly:
  qC.readSerial(Serial);
  qC.readSerial(Serial2);
  qC.readBinary();

  PT_SCHEDULE(calcRMS());
  PT_SCHEDULE(toggleLED());
}

void getADC2(void) {
    static uint8_t counts = 0;
    static double sumTotal = 0;
    double newadc = readADC2_from_ISR(); //read ADC voltage

  if ( servoEnable.get() == false) {
    //If servo is off, set ServoOut to servoOffset control
    ServoOut.set(servoOffset.get());
    counts = 0;
    sumTotal = 0;
  } else {
    counts++;
    sumTotal += newadc;
    if (counts >= 10) {
      //after 10 measurements, set ServoOut to the average of them and restart the averaging
      ServoOut.set(sumTotal/10.0);
      counts = 0;
      sumTotal = 0;
    }
  }
}
void getADC1(void) {
  static bool ramp_running = false; // local variable if ramp is running
  static bool ramp_up = true; // local variable for up vs down part of ramp
  static int16_t rampPos = 0;
  static bool sampleData = false;  
  static bool collectingData;

  double newadc = readADC1_from_ISR(); //read ADC voltage
  
  if ( Data.isFull()  ) {
    sampleData = false; // stop sampling data whenever Data is full
  }

  if (ramp_running) {
    //Ramp Mode
    if (sampleData) {
      Data.setNext(newadc); //Store data point in Data array
    }
    if (ramp_up) {
      rampPos++;
      if (rampPos >= DataPointsHalf) {
        //end of ramp, turn around
        ramp_up = ! ramp_up;
        if ( Data.isEmpty()) {
          sampleData = true; //only start (re)sampling data when data empty and at start of ramp
        }
      }
    } else {
      rampPos--;
      if (-rampPos >= DataPointsHalf) {
        ramp_up = ! ramp_up;
      }
    }

    if ( (rampPos == 0) && ramp_up && (servoEnable.get() == true )) {
      //at end of ramp and servoEn has been set true, stop the ramp
      ramp_running = false;
      collectingData = true;
      triggerWrite(1,false); //Turn on Servo
    }

    //calculate ramp value
    float new_value = servoOffset.get() + rampAmp.get()/ (float) DataPointsHalf/2.0*rampPos;
    setD2Output(new_value);
    
  } else {    
    //Servo Running
    if (servoEnable.get() == false) {
      //Request to turn servo off, 
      rampPos = 0;
      ramp_up = true;      
      triggerWrite(1,true); //Turn off Servo      

      if (processingData == false) {
        //once processData completed, start running ramp
        ramp_running = true;
        errorRMS.set(NAN);
      }
    } else {
    
      if (collectingData) {    
        Data.setNext(newadc);
        if (Data.isFull() == true) {
           //stop collecting data and start processing it
          collectingData = false;
          processingData = true;       
        }
      } else {
        if (processingData == false) {
          //processing done, can start collecting data again
          collectingData = true;
        }
      }
    }
  }
}

Controlling the D2-125

With the hardware and software all setup, we can now use qControl to control the D2-125 laser servo: