Arduino Code for SAMD21 Dev Breakout by Sparkfun

https://www.sparkfun.com/products/13672?_ga=2.219677657.1904334046.1564414689-1081061511.1562028237

Updated 2019-Aug-05

Fixed DRV SPI CS; Added Test Points T0 & T1; Disabled Serial1

Now on GIT https://github.com/gouldpa/FOC-Arduino-Brushless

Don't use the code below, it will always be out of date

/* External Encoder SPI
 * Motor Driver Control/Onboard Encoder SPI
 * 3 Phase timer with deadband TCC0
 * Timer IRQ 10KHz Timer 5
 * I2C Diaplay
 * RS485 with time-out Timer 4(start of packet)
 * ADC sequential, free-running (6) and DMA and IRQ
 * SerialUSB is available
 * Serial1 is available but is currently used as test points T0 and T1
 * 
 * Joint Control -> PID -> Motor Control (FOC) -> Three Phase Output
 */

#include <SPI.h>
#include "wiring_private.h" // pinPeripheral() function

//  I2c OLED  ///////////////////////////////////////////////////////////
#include "ssd1306.h" // library by Alexey Dynda
char display_str[] = "1234567890";

//  On-Board Encoder + Motor Driver and External Encoder /////////////////
#define ENC_SPI_MISO 6 //29 D6 PA20 ser 5:2 SER
#define ENC_SPI_SCK 7 // 30 D7 PA21 ser5:3 SER
#define ENC_SPI_MOSI A5 //47 A5 PB02 ser5:0 ALT
#define ENC_CS A0 //PA02
#define ENC_CS_PORT_PIN PORT_PA02 // Fast Pin Switching REG_PORT_OUTSET0
SPIClass ENC_SPI_5 (&sercom5, ENC_SPI_MISO,  ENC_SPI_SCK,  ENC_SPI_MOSI, SPI_PAD_0_SCK_3, SERCOM_RX_PAD_2); 

#define DRV_SPI_MISO MISO // 21 PA12 SER4:0 ALT
#define DRV_SPI_SCK SCK // 20 PB11 SER4:3 ALT
#define DRV_SPI_MOSI MOSI //19 PB10 SER4:2 ALT
#define DRV_CS 30 // PB22
#define DRV_CS_PORT_PIN PORT_PB22 // Fast Pin Switching USE REG_PORT_OUTSET1
SPIClass DRV_SPI_4 (&sercom4, DRV_SPI_MISO,  DRV_SPI_SCK,  DRV_SPI_MOSI, SPI_PAD_2_SCK_3, SERCOM_RX_PAD_0);

uint16_t enc_data, drv_data; 

//  Three Phase Center Aligned PWM with Dead-band //////////////////
#define UH 3   //W1 14 PA09
#define UL 5   //W5 24 PA15
#define VH 10  //W2 27 PA18
#define VL 11  //W6 25 PA16
#define WH 12  //W3 28 PA19 
#define WL 13  //W7 26 PA17

//  RS485 Serial with Direction CTRL /////////////////////////////
#define RS485_TX_SIZE 16
#define RS485_RX_SIZE 32
char RS485_txbuf[RS485_TX_SIZE];
char RS485_rxbuf[RS485_RX_SIZE];
#define RS485_RX_MAX 4
#define RS485_TX_PACKET_SIZE 4
#define UART_ERROR 0x80
#define TX_COMPLETE 0x02
#define RX_CHAR 0x04
char RS485_tx_to_send=0;
char RS485_tx_left=0;
char RS485_rx_cnt=0;

uint8_t i=0;

#define RS485_RX 38 //PA13 ser2:1 ALT
#define RS485_TX 4 //PA08 ser2:0 ALT
#define RS485_DIR 2

Uart Serial2 (&sercom2, RS485_RX, RS485_TX, SERCOM_RX_PAD_1, UART_TX_PAD_0);

//  TC4 serial time-out //////////////////////////////////////////
#define TC4_INTERRUPT 0 //Disable Interrupt 

//  ADC DMA sequential free running (6) with Interrupts /////////////////
#define ADCPIN1 A1
#define ADC_Number 6

#define HWORDS 7
uint16_t adcbuf[HWORDS];     

typedef struct {
    uint16_t btctrl;
    uint16_t btcnt;
    uint32_t srcaddr;
    uint32_t dstaddr;
    uint32_t descaddr;
} dmacdescriptor ;
volatile dmacdescriptor wrb[12] __attribute__ ((aligned (16)));
dmacdescriptor descriptor_section[12] __attribute__ ((aligned (16)));
dmacdescriptor descriptor __attribute__ ((aligned (16)));
DmacDescriptor *desc; // DMA descriptor address (so we can change contents)

static uint32_t ADC_DMA_chnl = 3;  // DMA channel



/////          BLDC       //////////////////////////////////////////////////////////////////////////////////////

int count;
#define SIN_ARRAY_SIZE_BITS 12
#define SIN_ARRAY_SIZE (1<<SIN_ARRAY_SIZE_BITS)
#define MOTOR_POLE_PAIRS    7
#define FULL_ANGLE 360.0
#define DEG_RAD (3.1415/180.0)
#define FULL_PWM_BITS 10
#define FULL_PWM (1<<FULL_PWM_BITS)
#define POS_ONE 0
#define MAGNET_OFFSET -100 //0
#define PHASE_OFFSET 120
#define MAX_PWM 250

/* Store in EEPROM
#define CONFIG_ARRAY_SIZE 32
int32 config_array[CONFIG_ARRAY_SIZE];

#define ID_OS           0
#define MAG_OFFSET_OS   1
#define PHASE_OFFSET_OS 2
#define TEMP_MAX_OS     3
#define NOMINAL_OS      4
#define NINETY_OS       5
int current_config = 0;

*/
int32_t Analog[3];
int32_t servo_val=0;

float sin_scale;
int phase_offset;
int16_t SinArray[SIN_ARRAY_SIZE];
int Temp_Max = 90;
int32_t PWMUU;
int32_t PWMVV;
int32_t PWMWW;

int32_t Motor_Position=0;
int32_t Motor_Position_Old=0;
int32_t Motor_Abs=0; 
int Motor_Count_Start=0;
int32_t Joint_Position;
int32_t Tar_Position = 0;
int32_t Tar_Position_Speed = 0;
int32_t Tar_Speed = 0;
int32_t Tar_Move =0;
int32_t Motor_Pos_Change=0;
int32_t off_set=0;
int Kp = 1;
int Target_Phase;
int8_t Servo_ID=1;
unsigned int magnetoffset = 215;
unsigned int phase_direction = 0;
int phase_set = -115;
int32_t Motor_Joint;
unsigned int Mag_Sensor_Data;
int Motor_on = 0;
int incomplete = 1;
int time = 0;
int forward=0;

// common
uint32_t counter=0;
int start=0;
int tpwm=0;
#define T0 0
#define T0_PORT PORT_PA10
#define T1 1
#define T1_PORT PORT_PA11
/**
 * @brief  Standard Arduino Setup
 * @retval void
 */
void setup() {
  SerialUSB.begin(115200);
  //Serial1.begin(115200);
  Serial2.begin(1000000);
  // Configure interrupt request
  NVIC_DisableIRQ(SERCOM2_IRQn);
  NVIC_ClearPendingIRQ(SERCOM2_IRQn);
  NVIC_SetPriority(SERCOM2_IRQn, 0);
  NVIC_EnableIRQ(SERCOM2_IRQn);
  delay(1000);

  //Serial1.print("Setup_Sin_array");
  ///////  Create Sin table based on number of Motor poles
  Setup_Sin_array();

  ENC_SPI_5.begin();
  DRV_SPI_4.begin();
  pinMode (ENC_CS, OUTPUT);
  digitalWrite(ENC_CS,HIGH);
  pinMode (DRV_CS, OUTPUT);
  digitalWrite(DRV_CS,HIGH);

  pinPeripheral(ENC_SPI_MISO, PIO_SERCOM);
  pinPeripheral(ENC_SPI_SCK, PIO_SERCOM);
  pinPeripheral(ENC_SPI_MOSI, PIO_SERCOM_ALT);

  pinPeripheral(DRV_SPI_MISO, PIO_SERCOM_ALT);
  pinPeripheral(DRV_SPI_SCK, PIO_SERCOM_ALT);
  pinPeripheral(DRV_SPI_MOSI, PIO_SERCOM_ALT);

  ENC_SPI_5.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE1));
  DRV_SPI_4.beginTransaction(SPISettings(1000000, MSBFIRST, SPI_MODE1));

  // ADC and DMA
  adc_init();
  dma_init();

  // 3 phase 
  TCC0_Setup();
  
  // I2C OLED
  Display_Init();
  
  // RS_485
  pinMode (RS485_DIR, OUTPUT);
  digitalWrite(RS485_DIR,LOW); 
  pinPeripheral(RS485_RX, PIO_SERCOM);
  pinPeripheral(RS485_TX, PIO_SERCOM_ALT);

  // Time-out (Start of Packet) for RS485 
  TC_Clock_Enable(); 
  TC4_Configure(); //configure the timer
  TC4_StartCounter(); //starts the timer
  
  // 10KHz Timer
  TC5_Configure(); //configure the timer to run at <sampleRate>Hertz
  TC5_StartCounter(); //starts the timer

  pinMode (T0, OUTPUT);
  digitalWrite(T0,LOW);
  pinMode (T1, OUTPUT);
  digitalWrite(T1,LOW);
}
 
/**
 * @brief  Standard Arduino Loop
 * @retval void
 */
void loop() {
  Motor_on=1;
  
  textDemo();

  SerialUSB.print(Target_Phase);
  SerialUSB.print(' ');
  SerialUSB.print(Motor_Joint);
  /*
  SerialUSB.print(' ');
  SerialUSB.print(PWMUU);
  SerialUSB.print(' ');
  SerialUSB.print(PWMVV);
  SerialUSB.print(' ');
  SerialUSB.print(PWMWW);
  */
  SerialUSB.print(' ');
  SerialUSB.println(Tar_Position);

  if (SerialUSB.available())
  {
    SerialUSB.read();
    if (forward)
    {
      Tar_Position=0;
      forward=0;
    }
    else
    {
      Tar_Position=8000;
      forward=1;
    }
  }
  
  if (0) // Print ADC information
  {
    int i;
    for (i=1; i<7; i++){
      SerialUSB.print(adcbuf[i]);
      SerialUSB.print(" ");
    }
    SerialUSB.println(" ");
  }
  counter++;
  if (counter>999){
    counter=0;}
  delay(1);
}

/**
 * @brief  Puts one variable on the SSD1306 OLED display
 * @retval void
 */
static void textDemo()
{
  sprintf(display_str, "%5d  ",(drv_data & 0x3FFF) );
    ssd1306_setFixedFont(ssd1306xled_font6x8);

    ssd1306_printFixed2x(0,  16, display_str, STYLE_NORMAL);

}
/**
 * @brief  this function gets called by the interrupt at 100us or 10kHz
 * @retval void
 */
void Task_100us(void)
{
  REG_PORT_OUTSET0 = T1_PORT;
  REG_PORT_OUTSET0 = T0_PORT;
  //digitalWrite(1,HIGH);
  //digitalWrite(0,HIGH);
  Read_2_Bytes_Dual_SPI();
  REG_PORT_OUTCLR0 = T0_PORT;
  Motor_CTRL();
  REG_PORT_OUTSET0 = T0_PORT;
  Motor_Vector_Phases(Target_Phase, Motor_Joint);
  REG_PORT_OUTCLR0 = T0_PORT;
  Three_Phases();
  REG_PORT_OUTSET0 = T0_PORT;
  
  adc_dma(adcbuf,HWORDS);
  REG_PORT_OUTCLR0 = T0_PORT;
  adc_start_with_DMA();
  REG_PORT_OUTCLR0 = T1_PORT;

}

////////////////////////////           BLDC Sub-routines ///////////////////////////////////

/*
 * 
 */
void Three_Phases(void)
{  
  if (Motor_on==0)
  {
      PWMUU = 0;
      PWMVV = 0;
      PWMWW = 0;
  }
  REG_TCC0_CCB1 = PWMUU;       // TCC0 CCB1 - center the servo on D5
  while(TCC0->SYNCBUSY.bit.CCB1);
  REG_TCC0_CCB2 = PWMVV;       // TCC0 CCB2 - center the servo on D6
  while(TCC0->SYNCBUSY.bit.CCB2);
  REG_TCC0_CCB3 = PWMWW;       // TCC0 CCB3 - center the servo on D7
  while(TCC0->SYNCBUSY.bit.CCB3);
}
/*
 * 
 */
void Motor_CTRL(void)
{
int drive;
 
    Motor_Position_Old = Motor_Position;
    
    Mag_Sensor_Data  = drv_data & 0x3FFF;
    Motor_Position = (Mag_Sensor_Data>>2); //& 0x2FFF;
    
   if (start==0)
   {  
    start=1; 
    Motor_Abs = 0;
   }
    Motor_Pos_Change = (Motor_Position-Motor_Position_Old);
    
    if (Motor_Pos_Change>2048)
        off_set =  Motor_Pos_Change-4096;
    else if (Motor_Pos_Change<-2048)
        off_set = Motor_Pos_Change + 4096;
    else 
        off_set = Motor_Pos_Change; 
    
    Motor_Abs += off_set;
    
    Joint_Position = Motor_Abs;

   // PID Control - Just P for now
    Motor_Joint = Kp * (Joint_Position - Tar_Position);
/*
    if (forward)
      Motor_Joint = 250;
    else
      Motor_Joint = -250;
   */ 

    if (Motor_Joint > 0) // Forward or Reverse
    {
        drive = PHASE_OFFSET; //phase_set;
    }
    else
    {
        drive = -PHASE_OFFSET; //phase_set;
        Motor_Joint = -Motor_Joint;
    }
    // Limit Voltage/PWM to motor
    if (Motor_Joint > MAX_PWM)
        Motor_Joint = MAX_PWM;
    
    // Drive Motor at ~90deg electrically ahead of magnets 
    Target_Phase = Motor_Position + drive + MAGNET_OFFSET; // MAGNET_OFFSET;

    //Handle wrap-around
    if (Target_Phase > SIN_ARRAY_SIZE)
        Target_Phase = Target_Phase - SIN_ARRAY_SIZE;
    else if (Target_Phase < 0)
        Target_Phase = Target_Phase + SIN_ARRAY_SIZE;
    else
    {} 
}

/*
 * 
 */
void Motor_Vector_Phases(int Mot_Phase, int Motor_PWM)
{
    int U_Ang = Mot_Phase;
    int V_Ang = Mot_Phase + phase_offset;
    int W_Ang = Mot_Phase - phase_offset;
    
    if (V_Ang > SIN_ARRAY_SIZE)
        V_Ang-=SIN_ARRAY_SIZE;
    if (W_Ang < 0)
        W_Ang+=SIN_ARRAY_SIZE;
    
    PWMUU = (SinArray[U_Ang] * Motor_PWM) >>FULL_PWM_BITS;
    PWMVV = (SinArray[V_Ang] * Motor_PWM) >>FULL_PWM_BITS;
    PWMWW = (SinArray[W_Ang] * Motor_PWM) >>FULL_PWM_BITS;
    
}

/*
 * 
 */
void Setup_Sin_array(void)
{
    int step1;
    float tempa;
    float tempb;
    int phase;
    sin_scale = 1.0/((sin(60*DEG_RAD))+(sin(60*DEG_RAD)));
    phase_offset = SIN_ARRAY_SIZE/(MOTOR_POLE_PAIRS*3);
    for (step1=0; step1<SIN_ARRAY_SIZE; step1++)
    {
        tempa = (float)step1 / ((float)SIN_ARRAY_SIZE/(float)MOTOR_POLE_PAIRS);
        phase = floor(tempa);
        tempb = (tempa-(float)phase) * FULL_ANGLE;
        
        if (tempb<(FULL_ANGLE/3.0))
        {
            SinArray[step1] = (int)(( sin((tempb-30.0)* DEG_RAD) - sin((tempb -( 30.0 + 120.0 ))* DEG_RAD) ) * sin_scale * (float)FULL_PWM);
        }
        else if (tempb<((FULL_ANGLE*2.0)/3.0))
        {    
            SinArray[step1] = (int)(( sin(((240.0-tempb)-30.0)* DEG_RAD) - sin(((240.0-tempb) -( 30.0 + 120.0 ))* DEG_RAD) ) * sin_scale * (float)FULL_PWM);
        }
        else
        {
            SinArray[step1] = 0;
        }
        //Serial1.print(step1);
        //Serial1.print(',');
        //Serial1.print(SinArray[step1]);
        //Serial1.println();
    }
}   

/**
 * @brief  Read 2 bytes of SPI on two SPI at the same time
 * @retval void
 */
void Read_2_Bytes_Dual_SPI(void)
{ 
  // It can get stuck in this loop if something goes wrong. 
  // Add some count downs to the while()
  //digitalWrite(DRV_CS,LOW); // This shouldn't be needed but something is wrong
    REG_PORT_OUTCLR0 = ENC_CS_PORT_PIN;
    REG_PORT_OUTCLR1 = DRV_CS_PORT_PIN;
    
    //first byte write
    SERCOM4->SPI.DATA.reg=0xFF;
    SERCOM5->SPI.DATA.reg=0xFF;
    while(SERCOM4->SPI.INTFLAG.bit.DRE == 0 ){}
    //second byte write 4
    SERCOM4->SPI.DATA.reg=0xFF; 
    
    while(SERCOM5->SPI.INTFLAG.bit.DRE == 0 ){}
    //second byte write 5
    SERCOM5->SPI.DATA.reg=0xFF; 
    
    while(SERCOM4->SPI.INTFLAG.bit.RXC == 0 ){}
    //first byte read 4
    drv_data=(SERCOM4->SPI.DATA.reg)<<8;
    
    while(SERCOM5->SPI.INTFLAG.bit.RXC == 0 ){}
    //first byte read 5
    enc_data=(SERCOM5->SPI.DATA.reg)<<8;

    while(SERCOM4->SPI.INTFLAG.bit.RXC == 0 ){}
    //second byte read4
    drv_data+=SERCOM4->SPI.DATA.reg;
    
    while(SERCOM5->SPI.INTFLAG.bit.RXC == 0 ){}
    //second byte read 5
    enc_data+=SERCOM5->SPI.DATA.reg;    

    REG_PORT_OUTSET0 = ENC_CS_PORT_PIN;
    REG_PORT_OUTSET1 = DRV_CS_PORT_PIN;
    //digitalWrite(DRV_CS,HIGH);
}

/**
 * @brief  this function gets called by the interrupt at 100us or 10kHz
 * @retval void
 */
void TC5_Handler (void) {
  Task_100us();
  TC5->COUNT16.INTFLAG.bit.MC0 = 1; //don't change this, it's part of the timer code
}

/*
 * @brief  Initializes TC_Clock_Enable
 * @retval void
 */
void TC_Clock_Enable (void)
{
   // Enable GCLK for TCC4 and TC4 (timer counter input clock)
 GCLK->CLKCTRL.reg = (uint16_t) (GCLK_CLKCTRL_CLKEN | GCLK_CLKCTRL_GEN_GCLK0 | GCLK_CLKCTRL_ID(GCM_TC4_TC5)) ;
 while (GCLK->STATUS.bit.SYNCBUSY);
}

/**
 * @brief  Initializes TC5_Configure
 * @param  int sampleRate
 * @retval void
 */
void TC5_Configure()
{
 //reset timer
 TC5->COUNT16.CTRLA.reg = TC_CTRLA_SWRST;
 while (TC5->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY);
 while (TC5->COUNT16.CTRLA.bit.SWRST);

 // Set Timer counter Mode to 16 bits
 TC5->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
 // Set TC5 mode as match frequency
 TC5->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ;
 //set prescaler and enable TC5
 TC5->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV1 | TC_CTRLA_ENABLE;
 //set TC5 timer counter based off of the system clock and the user defined sample rate or waveform
 TC5->COUNT16.CC[0].reg = (uint16_t) 4800;
 while (TC5->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY);
 
 // Configure interrupt request
 NVIC_DisableIRQ(TC5_IRQn);
 NVIC_ClearPendingIRQ(TC5_IRQn);
 NVIC_SetPriority(TC5_IRQn, 7); // 0 is the highest Priority Interrupt; RS485 should be the highest 
 NVIC_EnableIRQ(TC5_IRQn);

 // Enable the TC5 interrupt request
 TC5->COUNT16.INTENSET.bit.MC0 = 1;
 while (TC5->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY); //wait until TC5 is done syncing 
} 

/**
 * @brief  Starts/Enables TC5_StartCounter and waits for it to be ready
 * @retval void
 */
void TC5_StartCounter()
{
  TC5->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE; //set the CTRLA register
  while (TC5->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY); //wait until snyc'd
}

/**
 * @brief  disable TC5
 * @retval void
 */
void TC5_Disable()
{
  TC5->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE;
  while (TC5->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY);
}

/**
 * @brief  Initialation Timer TCC0 
 * @retval void
 */
void TCC0_Setup()
{
  REG_GCLK_GENDIV = GCLK_GENDIV_DIV(2) |          // Divide the 48MHz clock source by divisor 3: 48MHz/3=16MHz
                    GCLK_GENDIV_ID(4);            // Select Generic Clock (GCLK) 4
  while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization

  REG_GCLK_GENCTRL = GCLK_GENCTRL_IDC |           // Set the duty cycle to 50/50 HIGH/LOW
                     GCLK_GENCTRL_GENEN |         // Enable GCLK4
                     GCLK_GENCTRL_SRC_DFLL48M |   // Set the 48MHz clock source
                     GCLK_GENCTRL_ID(4);          // Select GCLK4
  while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization

  // Enable the port multiplexer for the 3 PWM channels: timer TCC0 outputs
  const uint8_t CHANNELS = 6;
  const uint8_t pwmPins[] = { UH, UL, VH, VL, WH, WL };
  for (uint8_t i = 0; i < CHANNELS; i++)
  {
     PORT->Group[g_APinDescription[pwmPins[i]].ulPort].PINCFG[g_APinDescription[pwmPins[i]].ulPin].bit.PMUXEN = 1;
  }
  // Connect the TCC0 timer to the port outputs - port pins are paired odd PMUO and even PMUXE
  // F & E specify the timers: TCC0, TCC1 and TCC2
  PORT->Group[g_APinDescription[UH].ulPort].PMUX[g_APinDescription[UH].ulPin >> 1].reg = PORT_PMUX_PMUXO_E | PORT_PMUX_PMUXE_F; //W1 14 PA09 
  PORT->Group[g_APinDescription[UL].ulPort].PMUX[g_APinDescription[UL].ulPin >> 1].reg = PORT_PMUX_PMUXO_F | PORT_PMUX_PMUXE_F; //W5 24 PA15
  PORT->Group[g_APinDescription[VH].ulPort].PMUX[g_APinDescription[VH].ulPin >> 1].reg = PORT_PMUX_PMUXO_F | PORT_PMUX_PMUXE_F; //W2 27 PA18
  PORT->Group[g_APinDescription[VL].ulPort].PMUX[g_APinDescription[VL].ulPin >> 1].reg = PORT_PMUX_PMUXO_F | PORT_PMUX_PMUXE_F; //W6 25 PA16 
  PORT->Group[g_APinDescription[WH].ulPort].PMUX[g_APinDescription[WH].ulPin >> 1].reg = PORT_PMUX_PMUXO_F | PORT_PMUX_PMUXE_F; //W3 28 PA19 
  PORT->Group[g_APinDescription[WL].ulPort].PMUX[g_APinDescription[WL].ulPin >> 1].reg = PORT_PMUX_PMUXO_F | PORT_PMUX_PMUXE_F; //W7 26 PA17

  // Feed GCLK4 to TCC0 and TCC1
  REG_GCLK_CLKCTRL = GCLK_CLKCTRL_CLKEN |         // Enable GCLK4 to TCC0 and TCC1
                     GCLK_CLKCTRL_GEN_GCLK4 |     // Select GCLK4
                     GCLK_CLKCTRL_ID_TCC0_TCC1;   // Feed GCLK4 to TCC0 and TCC1
  while (GCLK->STATUS.bit.SYNCBUSY);              // Wait for synchronization

  // Dual slope PWM operation: timers countinuously count up to PER register value then down 0
  //REG_TCC0_WAVE |= TCC_WAVE_POL(0xF) | TCC_WAVE_WAVEGEN_DSBOTTOM;         // Reverse the output polarity on all TCC0 outputs // Setup dual slope PWM on TCC0
  REG_TCC0_WAVE |= TCC_WAVE_WAVEGEN_DSBOTTOM;         // Reverse the output polarity on all TCC0 outputs // Setup dual slope PWM on TCC0
                     
  while (TCC0->SYNCBUSY.bit.WAVE);               // Wait for synchronization

  // Each timer counts up to a maximum or TOP value set by the PER register,
  // this determines the frequency of the PWM operation:
  // 20000 = 50Hz, 10000 = 100Hz, 2500  = 400Hz
  REG_TCC0_PER = 1000;      // Set the frequency of the PWM on TCC0 to 50Hz
  while(TCC0->SYNCBUSY.bit.PER);

  // The CCBx register value corresponds to the pulsewidth in microseconds (us)

  REG_TCC0_CCB1 = 500;       // TCC0 CCB1 - center the servo on D5
  while(TCC0->SYNCBUSY.bit.CCB1);
  REG_TCC0_CCB2 = 300;       // TCC0 CCB2 - center the servo on D6
  while(TCC0->SYNCBUSY.bit.CCB2);
  REG_TCC0_CCB3 = 100;       // TCC0 CCB3 - center the servo on D7
  while(TCC0->SYNCBUSY.bit.CCB3);
  
  // Set the output matrix so that D3 and D7 are set to output CC0 and enable low and high dead time insertion
  REG_TCC0_WEXCTRL |=  TCC_WEXCTRL_DTHS(5) | TCC_WEXCTRL_DTLS(5) | 
                        TCC_WEXCTRL_DTIEN3 | TCC_WEXCTRL_DTIEN2 | TCC_WEXCTRL_DTIEN1; // | TCC_WEXCTRL_OTMX(0x2);
  while(TCC0->SYNCBUSY.bit.CCB2);

  REG_TCC0_DRVCTRL |=  TCC_DRVCTRL_INVEN3 | TCC_DRVCTRL_INVEN2 | TCC_DRVCTRL_INVEN1;
  while(TCC0->SYNCBUSY.bit.CCB2);
  
  // Divide the 16MHz signal by 8 giving 2MHz (0.5us) TCC0 timer tick and enable the outputs
  //REG_TCC0_CTRLA |= TCC_CTRLA_PRESCALER_DIV1 | TCC_CTRLA_ENABLE;  // Divide GCLK4 by 8, Enable the TCC0 output

  REG_TCC0_CTRLA |= TCC_CTRLA_PRESCALER_DIV1 | TCC_CTRLA_ENABLE;  // Divide GCLK4 by 8, Enable the TCC0 output


  while (TCC0->SYNCBUSY.bit.ENABLE);              // Wait for synchronization
}

/**
 * @brief  Initialization for the SSD1306 OLED display via I2C
 * @retval void
 */
void Display_Init()
{
    ssd1306_setFixedFont(ssd1306xled_font6x8);
    ssd1306_128x64_i2c_init();
    ssd1306_clearScreen();
}

/*
 * @brief  TX and RX Interrupt
 * @param 
 * @retval void
 */
void SERCOM2_Handler()
{
  char flag = SERCOM2->USART.INTFLAG.reg;
  int uart_status;
  
  if (flag & TX_COMPLETE){ // TX Complete
    if (RS485_tx_left){
      SERCOM2->USART.DATA.reg = RS485_txbuf[RS485_tx_to_send-RS485_tx_left];
      SERCOM2->USART.INTENCLR.reg = 0x01;
      SERCOM2->USART.INTENSET.reg = 0x86;
      SERCOM2->USART.INTFLAG.reg = TX_COMPLETE;
      RS485_tx_left--;
    }
    else{
      SERCOM2->USART.INTENCLR.reg = 0x00;
      SERCOM2->USART.INTENSET.reg = 0x84;
      SERCOM2->USART.INTFLAG.reg = TX_COMPLETE;
      digitalWrite(RS485_DIR,LOW);
    }
  }
  if (flag & RX_CHAR){ // RX in
    
    if (TC4->COUNT16.COUNT.reg>100) //Check if this is a new packet (using timeout)
    {
      RS485_rx_cnt=0;
    }
    
    TC4->COUNT16.COUNT.reg=0; //Reset the timeout
    RS485_rxbuf[RS485_rx_cnt] = SERCOM2->USART.DATA.reg; // Put new byte into packet
    SERCOM2->USART.INTENSET.reg = 0x84;
    if (RS485_rx_cnt ==(RS485_RX_MAX-1)) // if the packet is complete reply [other checks required]
    {
      //parse the rx packet; Full Packet Received 
       Serial2_Transmit(RS485_TX_PACKET_SIZE); // Send a reply
    }
    RS485_rx_cnt++;
  }
  if (flag & UART_ERROR){
    uart_status = SERCOM2->USART.STATUS.reg;
    SERCOM2->USART.STATUS.reg = uart_status;
    SERCOM2->USART.INTENSET.reg = 0x86;
    SERCOM2->USART.INTENCLR.reg = 0x80;
    SERCOM2->USART.INTFLAG.reg = UART_ERROR;
  }
}

/*
 * @brief  Sends RS485 packet
 * @param  int RS485_chars_to_send
 * @retval void
 */
void Serial2_Transmit(int RS485_chars_to_send){
  digitalWrite(RS485_DIR,HIGH); //Set RS485 Transceiver to Transmit
  //delayMicroseconds(1);
  // Create Packet to send
  int p;
  for (p=0; p<RS485_TX_SIZE; p++)
  {
    RS485_txbuf[p]=p;
  }
  RS485_tx_left = RS485_chars_to_send;
  RS485_tx_to_send = RS485_chars_to_send;
  SERCOM2->USART.DATA.reg = RS485_txbuf[RS485_tx_to_send -RS485_tx_left];
  SERCOM2->USART.INTENSET.reg = 0x86;
  RS485_tx_left--;
}

/*
 * @brief  Initializes TC4_Handler
 * @retval void
 */
void TC4_Handler (void) {
  TC4->COUNT16.INTFLAG.bit.MC0 = 1; //don't change this, it's part of the timer code
}

/*
 * @brief  Initializes TC4_Configure
 * @retval void
 */
void TC4_Configure(void)
{
  //reset timer
  TC4->COUNT16.CTRLA.reg = TC_CTRLA_SWRST;
  while (TC4->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY);
  while (TC4->COUNT16.CTRLA.bit.SWRST);
  
  // Set Timer counter Mode to 16 bits
  TC4->COUNT16.CTRLA.reg |= TC_CTRLA_MODE_COUNT16;
  // Set TC4 mode as match frequency
  TC4->COUNT16.CTRLA.reg |= TC_CTRLA_WAVEGEN_MFRQ;
  //set prescaler and enable TC4
  TC4->COUNT16.CTRLA.reg |= TC_CTRLA_PRESCALER_DIV256 | TC_CTRLA_ENABLE;
  TC4->COUNT16.CTRLBCLR.reg |= TC_CTRLBCLR_DIR;
  //set TC4 timer counter based off of the system clock and the user defined sample rate or waveform
  TC4->COUNT16.CC[0].reg = 10000;
  while (TC4->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY);
  
  if (TC4_INTERRUPT)
  {
     // Configure interrupt request
    NVIC_DisableIRQ(TC4_IRQn);
    NVIC_ClearPendingIRQ(TC4_IRQn);
    NVIC_SetPriority(TC4_IRQn, 7);
    NVIC_EnableIRQ(TC4_IRQn);
    
    // Enable the TC4 interrupt request
    TC4->COUNT16.INTENSET.bit.MC0 = 1;
    while (TC4->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY); //wait until TC5 is done syncing 
  }
} 

/**
 * @brief  Starts/Enables TC4_StartCounter and waits for it to be ready
 * @retval void
 */
void TC4_StartCounter()
{
  TC4->COUNT16.CTRLA.reg |= TC_CTRLA_ENABLE; //set the CTRLA register
  while (TC4->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY); //wait until snyc'd
}

/**
 * @brief  disable TC4
 * @retval void
 */
void TC4_Disable()
{
  TC4->COUNT16.CTRLA.reg &= ~TC_CTRLA_ENABLE;
  while (TC4->COUNT16.STATUS.reg & TC_STATUS_SYNCBUSY);
}

/**
 * @brief  dma_init
 * @retval void
 */
void dma_init() {
    // probably on by default
    PM->AHBMASK.reg |= PM_AHBMASK_DMAC ;
    PM->APBBMASK.reg |= PM_APBBMASK_DMAC ;
    NVIC_EnableIRQ( DMAC_IRQn ) ;
    DMAC->BASEADDR.reg = (uint32_t)descriptor_section;
    DMAC->WRBADDR.reg = (uint32_t)wrb;
    DMAC->CTRL.reg = DMAC_CTRL_DMAENABLE | DMAC_CTRL_LVLEN(0xf);
}

/**
 * @brief  adc_dma 
 * @retval void
 */
void adc_dma(void *rxdata,  size_t hwords) {
    uint32_t temp_CHCTRLB_reg;

    DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl);
    DMAC->CHCTRLA.reg &= ~DMAC_CHCTRLA_ENABLE;
    DMAC->CHCTRLA.reg = DMAC_CHCTRLA_SWRST;
    DMAC->SWTRIGCTRL.reg &= (uint32_t)(~(1 << ADC_DMA_chnl));
    temp_CHCTRLB_reg = DMAC_CHCTRLB_LVL(0) |
      DMAC_CHCTRLB_TRIGSRC(ADC_DMAC_ID_RESRDY) | DMAC_CHCTRLB_TRIGACT_BEAT;
    DMAC->CHCTRLB.reg = temp_CHCTRLB_reg;
    DMAC->CHINTENSET.reg = DMAC_CHINTENSET_MASK ; // enable all 3 interrupts
    descriptor.descaddr = 0;
    descriptor.srcaddr = (uint32_t) &ADC->RESULT.reg;
    descriptor.btcnt =  hwords;
    descriptor.dstaddr = (uint32_t)rxdata + hwords*2;   // end address
    descriptor.btctrl =  DMAC_BTCTRL_BEATSIZE_HWORD | DMAC_BTCTRL_DSTINC | DMAC_BTCTRL_VALID;
    memcpy(&descriptor_section[ADC_DMA_chnl],&descriptor, sizeof(dmacdescriptor));

    // start channel
    DMAC->CHID.reg = DMAC_CHID_ID(ADC_DMA_chnl);
    DMAC->CHCTRLA.reg |= DMAC_CHCTRLA_ENABLE;
}

/**
 * @brief  ADC sync wait 
 * @retval void
 */
static __inline__ void ADCsync() __attribute__((always_inline, unused));
static void   ADCsync() {
  while (ADC->STATUS.bit.SYNCBUSY == 1); //Just wait till the ADC is free
}

/**
 * @brief  Initialize ADC
 * @retval void
 */
void adc_init(){
  analogRead(ADCPIN1);  // do some pin init  pinPeripheral() 
  ADC->CTRLA.bit.ENABLE = 0x00;             // Disable ADC
  ADCsync();
  //ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC0_Val; //  2.2297 V Supply VDDANA
  //ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_1X_Val;      // Gain select as 1X
  ADC->INPUTCTRL.bit.GAIN = ADC_INPUTCTRL_GAIN_DIV2_Val;  // default
  ADC->REFCTRL.bit.REFSEL = ADC_REFCTRL_REFSEL_INTVCC1_Val;
  ADCsync();    //  ref 31.6.16
  ADC->INPUTCTRL.bit.MUXPOS = 1;
  ADCsync();
  ADC->INPUTCTRL.bit.INPUTSCAN = ADC_Number+1;
  ADCsync();
  ADC->INPUTCTRL.bit.INPUTOFFSET = 0;
  ADCsync();
  ADC->AVGCTRL.reg = 0x00 ;       //no averaging
  ADC->SAMPCTRL.reg = 0x05;  ; //sample length in 1/2 CLK_ADC cycles
  ADCsync();
  ADC->CTRLB.reg = ADC_CTRLB_PRESCALER_DIV16 | ADC_CTRLB_FREERUN | ADC_CTRLB_RESSEL_12BIT;
  ADCsync();
}

/**
 * @brief  adc_stop_with_DMA
 * @retval void
 */
void adc_stop_with_DMA(void)
{
    ADC->CTRLA.bit.ENABLE = 0x00;
}

/**
 * @brief  adc_start_with_DMA
 * @retval void
 */
void adc_start_with_DMA(void)
{
  ADC->INPUTCTRL.bit.MUXPOS = 1;
  ADC->INPUTCTRL.bit.INPUTSCAN = ADC_Number+1;
  ADC->INPUTCTRL.bit.INPUTOFFSET = 0;
  ADC->SWTRIG.bit.FLUSH = 1;
  ADC->CTRLA.bit.ENABLE = 0x01; 
}


/**
 * @brief  DMAC_Handler
 * @retval void
 */
void DMAC_Handler() {
    uint8_t active_channel;
    active_channel =  DMAC->INTPEND.reg & DMAC_INTPEND_ID_Msk; // get channel number
    DMAC->CHID.reg = DMAC_CHID_ID(active_channel);
    adc_stop_with_DMA();
    DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TCMPL; // clear
    DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_TERR;
    DMAC->CHINTFLAG.reg = DMAC_CHINTENCLR_SUSP;
}

/*
    MIT License

    Copyright (c) 2017-2019, Alexey Dynda

    Permission is hereby granted, free of charge, to any person obtaining a copy
    of this software and associated documentation files (the "Software"), to deal
    in the Software without restriction, including without limitation the rights
    to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    copies of the Software, and to permit persons to whom the Software is
    furnished to do so, subject to the following conditions:

    The above copyright notice and this permission notice shall be included in all
    copies or substantial portions of the Software.

    THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    SOFTWARE.
*/