Servo Control with DMA

Status
Not open for further replies.
T

teebr

New member
Hello all,

I got a Teensy 3.5 a while ago, and was using it to control a couple of servos. The servo library is great, but I think I'm having a problem with interrupts messing with the timing of the PWM signal (I have I2C data incoming and maybe other bits and pieces in the future). I had the idea of controlling the servo completely using DMA, but haven't been able to get anything running successfully.

The basic idea is to have a digital pin set high every 20ms (this is a fixed interval), and then pulled back low after 1-2ms depending on the value set by the user. I played around with two PIT timers and the PTOR register to toggle the digital pin, using 4 DMA channels (not ideal, but I am past doing this because it's a good idea).
  • DMA2: triggered on rising edge: disable PIT1 timer
  • DMA3: triggered by DMA2: enable PIT1 (restart it)
  • DMA1: triggered by DMA3: set pin low (this will only happen when PIT1 hits zero after 1-2ms)
  • DMA0: triggered by DMA1: set pin high (this will only happen when PIT0 hits zero after 20ms)

Does this look like a logical way of doing things? It seems that there must be a better way of doing it, maybe using the FTM or PDB timers instead?

This doesn't work because PIT1 isn't being re-enabled for some reason? Code is below, at the moment I'm not using a servo, just making the LED on pin 13 flash, so the timers are set to 3s and 1s rather than 20ms and 1-2ms.

Code: 
#include "DMAChannel.h"

DMAChannel dma0;
DMAChannel dma1;
DMAChannel dma2;
DMAChannel dma3;

uint32_t PIN13_MASK = (1 << 5); // CORE_PIN13_BITMASK;
volatile uint32_t& PIN13_ADDR = GPIOC_PTOR;
volatile uint32_t& PIN13_CONFIG = PORTC_PCR5; //CORE_PIN13_CONFIG;
uint32_t PIN13_DMAMUX_SOURCE = 51; //DMAMUX_SOURCE_PORTC;
volatile uint32_t& PIT_VAL = PIT_LDVAL1;

uint32_t FRAME_PERIOD = 3;
uint32_t FRAME_TICKS = F_BUS * FRAME_PERIOD - 1;
uint32_t ON_PERIOD = 1;
uint32_t ON_TICKS = F_BUS * ON_PERIOD - 1;

volatile uint32_t PIT_TCTRL_EN = 1; //enable PIT timer
volatile uint32_t PIT_TCTRL_DIS = 0; //disable PIT timer

void setup() {
  Serial.begin(115200);
  pinMode(13, OUTPUT);
  
  initPIT();
  initDMA();

  // toggle pin high to kick things off
  PIN13_ADDR = PIN13_MASK;

  Serial.println("setup complete.");
}

void loop() {

  if (dma0.error ()) {
    Serial.println ("Error setting up DMA0");
  }
  else if (dma1.error ()) {
    Serial.println ("Error setting up DMA1");
  }
    else if (dma2.error ()) {
    Serial.println ("Error setting up DMA2");
  }
    else if (dma3.error ()) {
    Serial.println ("Error setting up DMA3");
  }

  //print remaining time on PIT timers:
  float num = (float)PIT_CVAL0;
  float den = (float)F_BUS;
  Serial.print(num / den);
  num = (float)PIT_CVAL1;
  Serial.print(" | ");
  Serial.println(num / den);

  delay(200);
}


void initDMA(void) {
/*
 * DMA2: triggered on rising edge: disable PIT1
 * DMA3: tiggered by DMA2: enable PIT1
 * DMA1: triggered by DMA3: set pin low (when PIT1 hits zero)
 * DMA0: triggered by DMA1: set pin high (when PIT0 hits zero)
 */
  DMAMUX0_CHCFG0 |= DMAMUX_TRIG; //enable pediodic interrupt from PIT on DMA (bit 6: 64)
  dma0.source(PIN13_MASK);
  dma0.destination(PIN13_ADDR);
  dma0.triggerAtCompletionOf(dma1);
  dma0.transferCount(1); //error if not set?
  dma0.enable();


  DMAMUX0_CHCFG1 |= DMAMUX_TRIG; //enable pediodic interrupt from PIT on DMA (bit 6: 64)
  dma1.source(PIN13_MASK);
  dma1.destination(PIN13_ADDR);
  dma1.triggerAtCompletionOf(dma3);
  dma1.transferCount(1);
  dma1.enable();

  PIN13_CONFIG |= PORT_PCR_IRQC(1);  // 1: rising edge, 2: falling edge 3: either edge
  dma2.source(PIT_TCTRL_DIS); //source must be global!
  dma2.destination(PIT_TCTRL1);
  dma2.triggerAtHardwareEvent(PIN13_DMAMUX_SOURCE);
  dma2.transferCount(1);
  dma2.enable();

  dma3.source(PIT_TCTRL_EN); //source must be global!
  dma3.destination(PIT_TCTRL1);
  dma3.triggerAtCompletionOf(dma2);
  dma3.transferCount(1);
  dma3.enable();
}

void initPIT(void) {
  // use PIT 0 and 1: only work with DMA channels 0 and 1 respectively.
  SIM_SCGC6 |= SIM_SCGC6_PIT; // turn on PIT clocks
  __asm__ volatile("nop"); // solves timing problem on Teensy 3.5
  PIT_MCR = 1; // enable the module
  PIT_TFLG0 = PIT_TFLG_TIF; // clear flag
  PIT_TCTRL0 |= PIT_TCTRL_TEN;  // enable timer 0
  PIT_LDVAL0 = FRAME_TICKS; //set to PWM frame length (generally 20ms)

  PIT_TFLG1 = PIT_TFLG_TIF; //clear flag
  PIT_TCTRL1 |= PIT_TCTRL_TEN;  // enable timer 1
  PIT_LDVAL1 = ON_TICKS;  //set to PWM width of 1-2ms
}
 
For anyone who's interested, I got it working using the PDB block and three DMA channels. I've tested it on two servos: at the moment both pins have to be on the same GPIO port, but I think that can be changed too. The number of DMA channels is independent of the number of servos.

Code: 
#include "DMAChannel.h"

DMAChannel dma0;
DMAChannel dma1;
DMAChannel dma2;

const int NUM_SERVOS = 2;
/*  The PDB has a 16 bit counter, which can trigger a DMA transfer when the counter
    reaches the value in the IDLY register. We want the modulus of the counter to be
    20ms (the servo frame length), and ideally close to the 65535 limit, as this will
    improve the resolution of the PWM signal. For FBus at 60MHz a prescale of 2 and
    multiplication factor of 10 gives 3 ticks per microsecond.
*/
uint32_t PDB_PRESCALE = 2; //TODO: feed this into PDB_CONFIG macro
uint32_t PDB_MULT_FACTOR = 10; //TODO: feed this into PDB_CONFIG macro

//it's easy to have integer maths errors if you you're not careful where you multiply/ divide
#define usToTicks(us) ((us*(F_BUS/1000000))/(PDB_PRESCALE*PDB_MULT_FACTOR))

/* Macro to configure the PDB0_SC register:
    pg 927 of the Teensy 3.5 manual
    PDB_SC_LDMOD(0): allow updates to timing registers immediately, so we can update the trigger times on the fly
    PDB_SC_TRGSEL: Trigger select: Note sure this matters, 15 is a software trigger, closest to DMA?
    PDB_SC_PDBEN: enable the PDB counter
    PDB_DMAEN: enable DMA for the interrupt (default is a software interrupt)
    PDB_SC_MULT(n): multiplication factor for counter: 1,10,20,40
    PDB_SC_PRESCALER(n): prescaler for counter: 2^(n-1)
    PDB_SC_CONT: Enable continuous operation of the counter

    These changes (and changes to some other PDB registers) don't apply until it's been ORd
    with the PDB_SC_LDOK value.
*/

#define PDB_CONFIG (PDB_SC_LDMOD(0) | PDB_SC_TRGSEL(15) | PDB_SC_PDBEN | PDB_SC_DMAEN \
                    | PDB_SC_CONT | PDB_SC_PRESCALER(1) | PDB_SC_MULT(1))

uint32_t GPIO_BUFFER[2 * NUM_SERVOS]; //values to write to GPIO_ADDR
volatile uint32_t& GPIO_ADDR = GPIOC_PTOR; //register you write to to toggle the digital pin states
volatile uint32_t PDB_DLY_TICKS[2 * NUM_SERVOS]; //
uint32_t PDB_SC_UPDATE; //value to sent to PDB0_SC when we want to update IDLY

const uint32_t SERVO_OFFSET_TICKS = usToTicks(3000); // value to stagger servo PWMs at (servo N starts at (N-1)*SERVO_OFFSET_TICKS us into the frame)
unsigned long oldTime22, newTime22, oldTime23, newTime23;


void setup() {
  Serial.begin(115200);
  pinMode(22, OUTPUT); //PORTC 1
  pinMode(23, OUTPUT); //PORTC 2

  initDMA();
  initPDB();

//  // check timing of PWM signals using interrupts:
//  oldTime22 = micros();
//  oldTime23 = micros();
//  attachInterrupt(digitalPinToInterrupt(22), pinTime22, CHANGE);
//  attachInterrupt(digitalPinToInterrupt(23), pinTime23, CHANGE);

  Serial.println("setup complete.");
  Serial.println("To update a servo value, enter the servo number, a space and the value in us");
  Serial.println("e.g. 1 2404");
}

void loop() {
  //servos all handled by DMA, just check for new servo values.

  if (dma0.error ()) {
    Serial.println ("Error on DMA0");
  }
  else if (dma1.error ()) {
    Serial.println ("Error on DMA1");
  }
  else if (dma2.error ()) {
    Serial.println ("Error on DMA2");
  }

  updatePWM(); //check for user input to change a 
  delay(50);
}


void initPDB(void) {
  SIM_SCGC6 |= SIM_SCGC6_PDB; //enable PDB clock

  PDB0_SC = PDB_CONFIG; //set most of the options
  /* The modulus  (point at which the counter loops back to zero) is the servo frame length
      interrupt delay will be udpated by DMAs, but for now it just needs to be set to a small
      value to trigger the first servo high.
      We then need to reset the software trigger for some reason? (maybe because DMA is not enabled when software triggering is?)
      Finally, write Load OK to update the modulus and delay values. This has be be done with, or after,
      the PDB enable bit has been written (this is done in PDB_CONFIG)
  */
  PDB0_MOD = usToTicks(20000);
  PDB0_IDLY = 0;//usToTicks(10);
  PDB0_SC |= PDB_SC_SWTRIG;
  PDB0_SC |= PDB_SC_LDOK;
}

void initDMA(void) {
  // The GPIO Buffer needs the same two values per cell: to toggle the pin high and low
  GPIO_BUFFER[0] = 1 << 1; //servo 1 high
  GPIO_BUFFER[1] = 1 << 1; //servo 1 low
  GPIO_BUFFER[2] = 1 << 2; //servo 2 high
  GPIO_BUFFER[3] = 1 << 2; //servo 2 low

  /* Buffer to update the PDB with new values.
      Note that they are in ascending order, but the last element is 0.
      This is because it's initialised at the point where it will next start at 0
  */
  PDB_DLY_TICKS[0] = usToTicks(1500) & 0xFFFF; //servo 1 low
  PDB_DLY_TICKS[1] = SERVO_OFFSET_TICKS & 0xFFFF; //servo 2 high
  PDB_DLY_TICKS[2] = (SERVO_OFFSET_TICKS + usToTicks(1500)) & 0xFFFF; //servo 2 low
  PDB_DLY_TICKS[3] = 0;                        //servo 1 high

  PDB_SC_UPDATE = PDB_CONFIG | PDB_SC_LDOK; //for DMA2


  /* DMA0 is triggered by the PDB interrupt, and toggles a servo's pin.
     DMA0 triggers DMA1, which moves to the next PDB delay
     DMA1 triggers DMA2, which writes to PDB_SC to update the PDB delay value.

     The servo pins start low. Servo 1 is set high when the initial PDB delay triggers DMA0
     This will then set the delay value to PDB_DLY_TICKS[0] later, which will then bring the GPIO
     back low. This then continutes to the next servo.
  */

  dma0.sourceBuffer(GPIO_BUFFER, sizeof(GPIO_BUFFER));
  dma0.destination(GPIO_ADDR);
  dma0.triggerAtHardwareEvent(DMAMUX_SOURCE_PDB);

  dma1.sourceBuffer(PDB_DLY_TICKS, sizeof(PDB_DLY_TICKS));
  dma1.destination(PDB0_IDLY);
  dma1.triggerAtTransfersOf(dma0);
  dma1.triggerAtCompletionOf(dma0); //need both

  dma2.source(PDB_SC_UPDATE);
  dma2.destination(PDB0_SC);
  dma2.triggerAtTransfersOf(dma1);
  dma2.triggerAtCompletionOf(dma1); //need both
  dma2.transferCount(1); //error if not set?

  dma0.enable();
  dma1.enable();
  dma2.enable();
}

void updatePWM() {
  /*format: looking for at least 5 bytes: a 1 or 2 to denote which servo,
     then a space and the PWM value in us. e.g. "1 1500"
  */
  
  int p = Serial.available();
  if ((p < 5) | (p > 6))
  {
    //incorrect number of bytes: clear the buffer and exit
    while (Serial.available()) {
      int dummy = Serial.read();
    }
    return;
  }

  int val = 0; //PWM value
  int servoNum = Serial.read() - 48;
  if((servoNum < 1) || (servoNum > NUM_SERVOS)) {
    Serial.print("Invalid Servo Number");
    return;
  }
  int dummy = Serial.read(); //the space (or whatever)
  
  //read in the remaining 3-4 bytes and convert to PWM values
  while ((p = Serial.available())) {
    int num =  Serial.read() - 48;
    if (p == 4) {
      val = val + num * 1000;
    }
    else if (p == 3) {
      val = val + num * 100;
    }
    else if ( p == 2) {
      val = val + num * 10;
    }
    else if (p == 1) {
      val = val + num;
    }
  }

  if (val < 500) {
    val = 500;
  }
  else if (val > 2500) {
    val = 2500;
  }

  /*make sure we update this only when CNT is past the current IDLY for this servo.
     Otherwise, there's a chance that if the new val is longer than the old one, we change it
     just after the old one has pulled the pin down. This will they trigger an event
     to pull it back high immediately and it's all out of sync.
  */
  uint32_t newOnTime = usToTicks(val) & 0xFFFF;
  while (PDB0_CNT <= (newOnTime + (servoNum - 1)*SERVO_OFFSET_TICKS)) {
    //wait to make sure the pin has already been pulled back low this cycle
  }
  PDB_DLY_TICKS[2 * (servoNum - 1)] = newOnTime + (servoNum - 1) * SERVO_OFFSET_TICKS;

  Serial.print("New PWM on servo ");
  Serial.print(servoNum);
  Serial.print(": "); 
  Serial.println(val);
}

void pinTime22(void) {
  newTime22 = micros();
  int val = digitalReadFast(22);
  if (val == 0) {
    //pin has just gone low, let's see how long it was high for
    Serial.print("22: ");
    Serial.println(newTime22 - oldTime22);
  }
  oldTime22 = newTime22;
}

void pinTime23(void) {
  newTime23 = micros();
  int val = digitalReadFast(23);
  if (val == 0) {
    //pin has just gone low, let's see how long it was high for
    Serial.print("23: ");
    Serial.println(newTime23 - oldTime23);
  }
  oldTime23 = newTime23;
}
 
Status
Not open for further replies.
Back
Top