Commit e3797c11 authored by Sam Calisch's avatar Sam Calisch

added firmware

parent 34c3252a
#include <avr/io.h>
#include <util/delay.h>
#include <avr/pgmspace.h>
#define output(directions,pin) (directions |= pin) // set port direction for output
#define set(port,pin) (port |= pin) // set port pin
#define clear(port,pin) (port &= (~pin)) // clear port pin
#define pin_test(pins,pin) (pins & pin) // test for port pin
#define bit_test(byte,bit) (byte & (1 << bit)) // test for bit set
/* USART buffer defines. */
/* \brief Receive buffer size: 2,4,8,16,32,64,128 or 256 bytes. */
#define USART_RX_BUFFER_SIZE 128
/* \brief Transmit buffer size: 2,4,8,16,32,64,128 or 256 bytes */
#define USART_TX_BUFFER_SIZE 128
/* \brief Receive buffer mask. */
#define USART_RX_BUFFER_MASK ( USART_RX_BUFFER_SIZE - 1 )
/* \brief Transmit buffer mask. */
#define USART_TX_BUFFER_MASK ( USART_TX_BUFFER_SIZE - 1 )
#if ( USART_RX_BUFFER_SIZE & USART_RX_BUFFER_MASK )
#error RX buffer size is not a power of 2
#endif
#if ( USART_TX_BUFFER_SIZE & USART_TX_BUFFER_MASK )
#error TX buffer size is not a power of 2
#endif
/* \brief USART transmit and receive ring buffer. */
typedef struct USART_Buffer
{
/* \brief Receive buffer. */
volatile uint8_t RX[USART_RX_BUFFER_SIZE];
/* \brief Transmit buffer. */
volatile uint8_t TX[USART_TX_BUFFER_SIZE];
/* \brief Receive buffer head. */
volatile uint8_t RX_Head;
/* \brief Receive buffer tail. */
volatile uint8_t RX_Tail;
/* \brief Transmit buffer head. */
volatile uint8_t TX_Head;
/* \brief Transmit buffer tail. */
volatile uint8_t TX_Tail;
} USART_Buffer_t;
/*! \brief Struct used when interrupt driven driver is used.
*
* Struct containing pointer to a usart, a buffer and a location to store Data
* register interrupt level temporary.
*/
typedef struct Usart_and_buffer
{
/* \brief Pointer to USART module to use. */
USART_t * usart;
/* \brief Data register empty interrupt level. */
USART_DREINTLVL_t dreIntLevel;
/* \brief Data buffer. */
USART_Buffer_t buffer;
} USART_data_t;
#define USART_RxdInterruptLevel_Set(_usart, _rxdIntLevel) \
((_usart)->CTRLA = ((_usart)->CTRLA & ~USART_RXCINTLVL_gm) | _rxdIntLevel)
#define USART_Format_Set(_usart, _charSize, _parityMode, _twoStopBits) \
(_usart)->CTRLC = (uint8_t) _charSize | _parityMode | \
(_twoStopBits ? USART_SBMODE_bm : 0)
#define USART_Baudrate_Set(_usart, _bselValue, _bScaleFactor) \
(_usart)->BAUDCTRLA =(uint8_t)_bselValue; \
(_usart)->BAUDCTRLB =(_bScaleFactor << USART_BSCALE0_bp)|(_bselValue >> 8)
#define USART_Rx_Enable(_usart) ((_usart)->CTRLB |= USART_RXEN_bm)
#define USART_Rx_Disable(_usart) ((_usart)->CTRLB &= ~USART_RXEN_bm)
#define USART_Tx_Enable(_usart) ((_usart)->CTRLB |= USART_TXEN_bm)
#define USART_Tx_Disable(_usart) ((_usart)->CTRLB &= ~USART_TXEN_bm)
_Bool USART_TXBuffer_FreeSpace(USART_data_t * usart_data)
{
/* Make copies to make sure that volatile access is specified. */
uint8_t tempHead = (usart_data->buffer.TX_Head + 1) & USART_TX_BUFFER_MASK;
uint8_t tempTail = usart_data->buffer.TX_Tail;
/* There are data left in the buffer unless Head and Tail are equal. */
return (tempHead != tempTail);
}
_Bool USART_TXBuffer_PutByte(USART_data_t * usart_data, uint8_t data)
{
uint8_t tempCTRLA;
uint8_t tempTX_Head;
_Bool TXBuffer_FreeSpace;
USART_Buffer_t * TXbufPtr;
TXbufPtr = &usart_data->buffer;
TXBuffer_FreeSpace = USART_TXBuffer_FreeSpace(usart_data);
if(TXBuffer_FreeSpace)
{
tempTX_Head = TXbufPtr->TX_Head;
TXbufPtr->TX[tempTX_Head]= data;
/* Advance buffer head. */
TXbufPtr->TX_Head = (tempTX_Head + 1) & USART_TX_BUFFER_MASK;
/* Enable DRE interrupt. */
tempCTRLA = usart_data->usart->CTRLA;
tempCTRLA = (tempCTRLA & ~USART_DREINTLVL_gm) | usart_data->dreIntLevel;
usart_data->usart->CTRLA = tempCTRLA;
}
return TXBuffer_FreeSpace;
}
_Bool USART_RXBufferData_Available(USART_data_t * usart_data)
{
/* Make copies to make sure that volatile access is specified. */
uint8_t tempHead = usart_data->buffer.RX_Head;
uint8_t tempTail = usart_data->buffer.RX_Tail;
/* There are data left in the buffer unless Head and Tail are equal. */
return (tempHead != tempTail);
}
uint8_t USART_RXBuffer_GetByte(USART_data_t * usart_data)
{
USART_Buffer_t * bufPtr;
uint8_t ans;
bufPtr = &usart_data->buffer;
ans = (bufPtr->RX[bufPtr->RX_Tail]);
/* Advance buffer tail. */
bufPtr->RX_Tail = (bufPtr->RX_Tail + 1) & USART_RX_BUFFER_MASK;
return ans;
}
_Bool USART_RXComplete(USART_data_t * usart_data)
{
USART_Buffer_t * bufPtr;
_Bool ans;
bufPtr = &usart_data->buffer;
/* Advance buffer head. */
uint8_t tempRX_Head = (bufPtr->RX_Head + 1) & USART_RX_BUFFER_MASK;
/* Check for overflow. */
uint8_t tempRX_Tail = bufPtr->RX_Tail;
uint8_t data = usart_data->usart->DATA;
if (tempRX_Head == tempRX_Tail) {
ans = 0;
}else{
ans = 1;
usart_data->buffer.RX[usart_data->buffer.RX_Head] = data;
usart_data->buffer.RX_Head = tempRX_Head;
}
return ans;
}
void USART_DataRegEmpty(USART_data_t * usart_data)
{
USART_Buffer_t * bufPtr;
bufPtr = &usart_data->buffer;
/* Check if all data is transmitted. */
uint8_t tempTX_Tail = usart_data->buffer.TX_Tail;
if (bufPtr->TX_Head == tempTX_Tail){
/* Disable DRE interrupts. */
uint8_t tempCTRLA = usart_data->usart->CTRLA;
tempCTRLA = (tempCTRLA & ~USART_DREINTLVL_gm) | USART_DREINTLVL_OFF_gc;
usart_data->usart->CTRLA = tempCTRLA;
}else{
/* Start transmitting. */
uint8_t data = bufPtr->TX[usart_data->buffer.TX_Tail];
usart_data->usart->DATA = data;
/* Advance buffer tail. */
bufPtr->TX_Tail = (bufPtr->TX_Tail + 1) & USART_TX_BUFFER_MASK;
}
}
void USART_InterruptDriver_Initialize(USART_data_t * usart_data,
USART_t * usart,
USART_DREINTLVL_t dreIntLevel)
{
usart_data->usart = usart;
usart_data->dreIntLevel = dreIntLevel;
usart_data->buffer.RX_Tail = 0;
usart_data->buffer.RX_Head = 0;
usart_data->buffer.TX_Tail = 0;
usart_data->buffer.TX_Head = 0;
}
void USART_InterruptDriver_DreInterruptLevel_Set(USART_data_t * usart_data,
USART_DREINTLVL_t dreIntLevel)
{
usart_data->dreIntLevel = dreIntLevel;
}
void usart_send_byte(USART_data_t * usart_data, char msg){
while(!USART_TXBuffer_FreeSpace(usart_data)){}
USART_TXBuffer_PutByte(usart_data,msg);
}
void usart_send_uint32(USART_data_t * usart_data, uint32_t n){
unsigned char * nn = (unsigned char *)&n;
usart_send_byte(usart_data, nn[0] );
usart_send_byte(usart_data, nn[1] );
usart_send_byte(usart_data, nn[2] );
usart_send_byte(usart_data, nn[3] );
}
void usart_send_int32(USART_data_t * usart_data, int32_t n){
unsigned char * nn = ( unsigned char *)&n;
usart_send_byte(usart_data, nn[0] );
usart_send_byte(usart_data, nn[1] );
usart_send_byte(usart_data, nn[2] );
usart_send_byte(usart_data, nn[3] );
}
void usart_send_uint16(USART_data_t * usart_data, uint16_t n){
unsigned char * nn = (unsigned char *)&n;
usart_send_byte(usart_data, nn[0] );
usart_send_byte(usart_data, nn[1] );
}
void usart_send_int16(USART_data_t * usart_data, int16_t n){
unsigned char * nn = ( unsigned char *)&n;
usart_send_byte(usart_data, nn[0] );
usart_send_byte(usart_data, nn[1] );
}
uint32_t parse_uint32(char* b){return *(uint32_t *) b;}
int32_t parse_int32(char* b){return *(int32_t *) b;}
uint16_t parse_uint16(char* b){return *(uint16_t *) b;}
/*
void usart_write_buffer(char* buffer){
//write \n terminated buffer over usart
int j=0;
do{
usart_send_byte( buffer[j] );
j++;
} while( j < PACKET_SIZE);
//} while( buffer[j-1] != 10);
}
*/
//
// sewing machine driver
//
// sec 2016
//
#include <stdio.h>
#include <avr/sleep.h>
#include <avr/interrupt.h>
#include "serial.h"
//calibration offsets for ADC
#define ADCACAL0_offset 0x20
#define ADCACAL1_offset 0x21
#define ADC_SAMPLES 10
static char input_buffer[100] = {0};
char checksum = 0;
USART_data_t USART_data;
int position_in_buffer = 0;
//keep these variables only to report status
//motor pwms
uint16_t pwm_1 = 0;
uint16_t pwm_2 = 0;
uint16_t pwm_3 = 0;
//motor directions
char dir_1 = 0;
char dir_2 = 0;
char dir_3 = 0;
//currents through motors
uint16_t cur_1[ADC_SAMPLES];
uint16_t cur_2[ADC_SAMPLES];
uint16_t cur_3[ADC_SAMPLES];
uint16_t cur_1_avg = 0;
uint16_t cur_2_avg = 0;
uint16_t cur_3_avg = 0;
uint16_t cur_pointer = 0;
//motor encoder overflow counters
int32_t mot_1_ovf = 0;
int32_t mot_2_ovf = 0;
int32_t mot_3_ovf = 0;
//motor positions: signed encoder ticks with overflow.
int32_t pos_1 = 0;
int32_t pos_2 = 0;
int32_t pos_3 = 0;
//desired motor positions
int32_t posd_1 = 0;
int32_t posd_2 = 0;
int32_t posd_3 = 0;
//running PID mode
char pid_mode=1;
//pid error
int32_t e_1[2];
int32_t e_2[2];
int32_t e_3[2];
uint16_t error_pointer = 0;
//pid derivative of error
uint16_t dt = 0; //time step
uint16_t last_t = 0; //last loop time
int32_t de_1 = 0;
int32_t de_2 = 0;
int32_t de_3 = 0;
//pid integral of error
int32_t ie_1 = 0;
int32_t ie_2 = 0;
int32_t ie_3 = 0;
int32_t ie_max = 10000;
//signed PID outputs
int32_t out_1 = 0;
int32_t out_2 = 0;
int32_t out_3 = 0;
//antiwindup
char pwm1_not_maxed=1;
char pwm2_not_maxed=1;
char pwm3_not_maxed=1;
//gains
int16_t Kp1 = 0;
int16_t Kp2 = -10000;
int16_t Kp3 = 0;
int16_t Ki1 = 0;
int16_t Ki2 = -1;//-1;
int16_t Ki3 = 0;
int16_t Kd1 = 0;
int16_t Kd2 = 10000;//40000;
int16_t Kd3 = 0;
//functions to set pwm and direction on motors
void set_pwm_1(uint16_t pwm, char dir){
if (dir==0){
//TCC0.CCABUF = TCC0.PER-pwm; //set compare value
//TCC0.CCBBUF = TCC0.PER;
TCC0.CCABUF = pwm; //set compare value
TCC0.CCBBUF = 0;
} else {
//TCC0.CCABUF = TCC0.PER; //set compare value
//TCC0.CCBBUF = TCC0.PER-pwm;
TCC0.CCABUF = 0; //set compare value
TCC0.CCBBUF = pwm;
}
do {} while(TCC0.INTFLAGS && TC0_OVFIF_bm == 0 ); //wait
TCC0.INTFLAGS = TC0_OVFIF_bm;
}
void set_pwm_2(uint16_t pwm, char dir){
if (dir==0){
//TCD1.CCABUF = TCD1.PER-pwm; //set compare value
//TCD1.CCBBUF = TCD1.PER;
TCD1.CCABUF = pwm; //set compare value
TCD1.CCBBUF = 0;
} else {
//TCD1.CCABUF = TCD1.PER; //set compare value
//TCD1.CCBBUF = TCD1.PER-pwm;
TCD1.CCABUF = 0; //set compare value
TCD1.CCBBUF = pwm;
}
do {} while(TCD1.INTFLAGS && TC1_OVFIF_bm == 0 ); //wait
TCD1.INTFLAGS = TC1_OVFIF_bm;
}
void set_pwm_3(uint16_t pwm, char dir){
if (dir==0){
//TCC0.CCCBUF = TCC0.PER-pwm; //set compare value
//TCC0.CCDBUF = TCC0.PER;
TCC0.CCCBUF = pwm; //set compare value
TCC0.CCDBUF = 0;
} else {
//TCC0.CCCBUF = TCC0.PER; //set compare value
//TCC0.CCDBUF = TCC0.PER-pwm;
TCC0.CCCBUF = 0; //set compare value
TCC0.CCDBUF = pwm;
}
do {} while(TCC0.INTFLAGS && TC0_OVFIF_bm == 0 ); //wait
TCC0.INTFLAGS = TC0_OVFIF_bm;
}
void send_packet(){
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,200);
//usart_send_uint16(&USART_data,TCC1.CNT);
//usart_send_uint16(&USART_data,TCE0.CNT);
//usart_send_uint16(&USART_data,TCD0.CNT);
//usart_send_int32(&USART_data,mot_1_ovf);
//usart_send_int32(&USART_data,mot_2_ovf);
//usart_send_int32(&USART_data,mot_3_ovf);
usart_send_int32(&USART_data,pos_1);
usart_send_int32(&USART_data,pos_2);
usart_send_int32(&USART_data,pos_3);
usart_send_int32(&USART_data,posd_1);
usart_send_int32(&USART_data,posd_2);
usart_send_int32(&USART_data,posd_3);
usart_send_int32(&USART_data,de_1);
usart_send_int32(&USART_data,de_2);
usart_send_int32(&USART_data,de_3);
usart_send_int32(&USART_data,ie_1);
usart_send_int32(&USART_data,ie_2);
usart_send_int32(&USART_data,ie_3);
usart_send_int32(&USART_data,out_1);
usart_send_int32(&USART_data,out_2);
usart_send_int32(&USART_data,out_3);
usart_send_uint16(&USART_data,pwm_1);
usart_send_uint16(&USART_data,pwm_2);
usart_send_uint16(&USART_data,pwm_3);
usart_send_uint16(&USART_data,cur_1_avg);
usart_send_uint16(&USART_data,cur_2_avg);
usart_send_uint16(&USART_data,cur_3_avg);
usart_send_byte(&USART_data,201);
usart_send_byte(&USART_data,201);
usart_send_byte(&USART_data,201);
usart_send_byte(&USART_data,201);
usart_send_byte(&USART_data,201);
}
void send_ack(){
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,200);
usart_send_byte(&USART_data,6);
usart_send_byte(&USART_data,201);
usart_send_byte(&USART_data,201);
usart_send_byte(&USART_data,201);
usart_send_byte(&USART_data,201);
usart_send_byte(&USART_data,201);
}
void parse_packet(char* buffer){
checksum = 0;
for( int i=5; i<position_in_buffer-5; ++i) {checksum += buffer[i];}
if (checksum == buffer[position_in_buffer-5] && buffer[0]==(char)200 && buffer[1]==(char)200 && buffer[2]==(char)200 && buffer[3]==(char)200 && buffer[4]==(char)200){
send_ack();
char id = buffer[5];
if (id==110){
//_delay_us(100);
//can we do this only if we've accumulated an appropriate number of samples?
send_packet();
} else if (id==100){
//set pwms directly
pwm_1 = parse_uint16(&buffer[6]);//((uint16_t)buffer[7]<<8) | (uint8_t)buffer[6];
dir_1 = buffer[8];
pwm_2 = parse_uint16(&buffer[9]);//((uint16_t)buffer[10]<<8) | (uint8_t)buffer[9];
dir_2 = buffer[11];
pwm_3 = parse_uint16(&buffer[12]);//((uint16_t)buffer[13]<<8) | (uint8_t)buffer[12];
dir_3 = buffer[14];
set_pwm_1(pwm_1,dir_1);
set_pwm_2(pwm_2,dir_2);
set_pwm_3(pwm_3,dir_3);
} else if (id==101){
//set desired positions
posd_1 = parse_uint32(&buffer[6]);//((uint16_t)buffer[7]<<8) | (uint8_t)buffer[6];
posd_2 = parse_uint32(&buffer[10]);//((uint16_t)buffer[9]<<8) | (uint8_t)buffer[8];
posd_3 = parse_uint32(&buffer[14]);//((uint16_t)buffer[11]<<8) | (uint8_t)buffer[10];
} else if (id==102){
//stop immediately
set_pwm_1(0,dir_1);
set_pwm_2(0,dir_2);
set_pwm_3(0,dir_3);
pid_mode = 0;
}
}
}
void read_usart(){
if (USART_RXBufferData_Available(&USART_data)) {
input_buffer[position_in_buffer] = USART_RXBuffer_GetByte(&USART_data);
if (input_buffer[position_in_buffer] == (char)201 &&
input_buffer[position_in_buffer-1] == (char)201 &&
input_buffer[position_in_buffer-2] == (char)201 &&
input_buffer[position_in_buffer-3] == (char)201 &&
input_buffer[position_in_buffer-4] == (char)201 &&
((position_in_buffer == 20) || (position_in_buffer == 11) || (position_in_buffer == 23)) //check against expected lengths
){
parse_packet(input_buffer);
position_in_buffer = 0;
}
else{
position_in_buffer++;
}
}
}
int main(void) {
// set up clock
OSC.CTRL = OSC_RC32MEN_bm; // enable 32MHz clock
while (!(OSC.STATUS & OSC_RC32MRDY_bm)); // wait for clock to be ready
CCP = CCP_IOREG_gc; // enable protected register change
CLK.CTRL = CLK_SCLKSEL_RC32M_gc; // switch to 32MHz clock
//set up usart
PORTE.DIRSET = PIN3_bm; //TXE0
PORTE.DIRCLR = PIN2_bm; //RXD0
USART_InterruptDriver_Initialize(&USART_data, &USARTE0, USART_DREINTLVL_LO_gc);
USART_Format_Set(USART_data.usart, USART_CHSIZE_8BIT_gc,
USART_PMODE_DISABLED_gc, 0);
//USARTE0.CTRLC = (USARTE0.CTRLC & ~USART_CMODE_gm) | USART_CMODE_ASYNCHRONOUS_gc;
USART_RxdInterruptLevel_Set(USART_data.usart, USART_RXCINTLVL_LO_gc);
//take f_sysclk/(BSEL+1) ~= f_baud*16 with zero scale. See manual or spreadsheet for scale defs
USART_Baudrate_Set(&USARTE0, 123 , -4); //230400 baud with .08% error
USART_Rx_Enable(USART_data.usart);
USART_Tx_Enable(USART_data.usart);
//set up quadrature decoding for three motors
//TODO: scale these to one revolution. For 20.4:1 gear ratio motors, this is 980. For 99:1, it is 4741.
//Or we could just track everything in encoder ticks.
//MOTOR1: TCC1
PORTC.DIRCLR = PIN4_bm | PIN5_bm; //set up port c pin 4 and pin 5 as quad input
PORTC.PIN4CTRL = (PORTC.PIN4CTRL & ~PORT_ISC_gm) | PORT_ISC_LEVEL_gc;
PORTC.PIN5CTRL = (PORTC.PIN5CTRL & ~PORT_ISC_gm) | PORT_ISC_LEVEL_gc;
EVSYS.CH0MUX = EVSYS_CHMUX_PORTC_PIN4_gc; /* Configure event channel 0 for quadrature decoding of pins. */
EVSYS.CH0CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_4SAMPLES_gc;
TCC1.CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH0_gc; //event channel zero
TCC1.CTRLB = (TCC1.CTRLB & ~TC1_WGMODE_gm) | TC_WGMODE_NORMAL_gc; //set overflow on top and bottom so we can increment rotation counter
TCC1.INTCTRLA = (TCC1.INTCTRLA & ~TC1_OVFINTLVL_gm) | TC_OVFINTLVL_LO_gc;
TCC1.INTFLAGS = TC1_OVFIF_bm; //clear interrupt flag
TCC1.PER = 0xFFFF;
TCC1.CTRLA = TC_CLKSEL_DIV1_gc; //start timer
//MOTOR2: TCE0
PORTE.DIRCLR = PIN0_bm | PIN1_bm; //set up port e pin 0 and pin 1 as quad input
PORTE.PIN0CTRL = (PORTE.PIN0CTRL & ~PORT_ISC_gm) | PORT_ISC_LEVEL_gc;
PORTE.PIN1CTRL = (PORTE.PIN1CTRL & ~PORT_ISC_gm) | PORT_ISC_LEVEL_gc;
EVSYS.CH4MUX = EVSYS_CHMUX_PORTE_PIN0_gc; /* Configure event channel 1 for quadrature decoding of pins. */
EVSYS.CH4CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_4SAMPLES_gc;
TCE0.CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH4_gc; //event channel four (I think channels 1 and 2 are used by USART?)
TCE0.CTRLB = (TCE0.CTRLB & ~TC0_WGMODE_gm) | TC_WGMODE_NORMAL_gc; //set overflow on top and bottom so we can increment rotation counter
TCE0.INTCTRLA = (TCE0.INTCTRLA & ~TC0_OVFINTLVL_gm) | TC_OVFINTLVL_LO_gc;
TCE0.INTFLAGS = TC0_OVFIF_bm; //clear interrupt flag
TCE0.PER = 0xFFFF;
TCE0.CTRLA = TC_CLKSEL_DIV1_gc; //start timer
//MOTOR3: TCD0
PORTD.DIRCLR = PIN0_bm | PIN1_bm; //set up port d pin 0 and pin 1 as quad input
PORTD.PIN0CTRL = (PORTD.PIN0CTRL & ~PORT_ISC_gm) | PORT_ISC_LEVEL_gc;
PORTD.PIN1CTRL = (PORTD.PIN1CTRL & ~PORT_ISC_gm) | PORT_ISC_LEVEL_gc;
EVSYS.CH2MUX = EVSYS_CHMUX_PORTD_PIN0_gc; /* Configure event channel 0 for quadrature decoding of pins. */
EVSYS.CH2CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_4SAMPLES_gc;
TCD0.CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH2_gc; //event channel two
TCD0.CTRLB = (TCD0.CTRLB & ~TC0_WGMODE_gm) | TC_WGMODE_NORMAL_gc; //set overflow on top and bottom so we can increment rotation counter
TCD0.INTCTRLA = (TCD0.INTCTRLA & ~TC0_OVFINTLVL_gm) | TC_OVFINTLVL_LO_gc;
TCD0.INTFLAGS = TC0_OVFIF_bm; //clear interrupt flag
TCD0.PER = 0xFFFF;
TCD0.CTRLA = TC_CLKSEL_DIV1_gc; //start timer
//set up ADC for current measurement
//calibrate ADC to reduce nonlinearity
NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc;
ADCA.CALL = pgm_read_byte(PROD_SIGNATURES_START + ADCACAL0_offset);
NVM_CMD = NVM_CMD_READ_CALIB_ROW_gc;
ADCA.CALL = pgm_read_byte(PROD_SIGNATURES_START + ADCACAL1_offset);
NVM_CMD = NVM_CMD_NO_OPERATION_gc;
ADCA.CTRLB = (ADCA.CTRLB & ~ADC_RESOLUTION_gm) | ADC_RESOLUTION_8BIT_gc; //set resolution
ADCA.REFCTRL = (ADCA.REFCTRL & ~ADC_REFSEL_gm) | ADC_REFSEL_INTVCC_gc; //set reference
ADCA.PRESCALER = (ADCA.PRESCALER & ~ADC_PRESCALER_gm) | ADC_PRESCALER_DIV128_gc; //set prescalar
//ADCA, channel zero, cur_1
ADCA.CH0.CTRL = (ADCA.CH0.CTRL & ~ADC_CH_INPUTMODE_gm) | ADC_CH_INPUTMODE_DIFFWGAIN_gc; //set differential with gain
ADCA.CH0.CTRL = (ADCA.CH0.CTRL & ~ADC_CH_GAIN_gm) | ADC_CH_GAIN_2X_gc; //set gain
ADCA.CH0.MUXCTRL = (uint8_t) ADC_CH_MUXPOS_PIN5_gc | ADC_CH_MUXNEG_GND_MODE4_gc;
//ADCA.CH0.MUXCTRL = (uint8_t) ADC_CH_MUXPOS_PIN5_gc | ADC_CH_MUXNEG_INTGND_MODE4_gc;
ADCA.CH0.INTCTRL = ADC_CH_INTMODE_COMPLETE_gc | ADC_CH_INTLVL_HI_gc;
//ADCA, channel one, cur_2
ADCA.CH1.CTRL = (ADCA.CH1.CTRL & ~ADC_CH_INPUTMODE_gm) | ADC_CH_INPUTMODE_DIFFWGAIN_gc; //set differential with gain
ADCA.CH1.CTRL = (ADCA.CH1.CTRL & ~ADC_CH_GAIN_gm) | ADC_CH_GAIN_2X_gc; //set gain
ADCA.CH1.MUXCTRL = (uint8_t) ADC_CH_MUXPOS_PIN7_gc | ADC_CH_MUXNEG_GND_MODE4_gc;
//ADCA.CH1.MUXCTRL = (uint8_t) ADC_CH_MUXPOS_PIN7_gc | ADC_CH_MUXNEG_INTGND_MODE4_gc;
ADCA.CH1.INTCTRL = ADC_CH_INTMODE_COMPLETE_gc | ADC_CH_INTLVL_HI_gc;
//ADCA, channel two, cur_3
ADCA.CH2.CTRL = (ADCA.CH2.CTRL & ~ADC_CH_INPUTMODE_gm) | ADC_CH_INPUTMODE_DIFFWGAIN_gc; //set differential with gain
ADCA.CH2.CTRL = (ADCA.CH2.CTRL & ~ADC_CH_GAIN_gm) | ADC_CH_GAIN_2X_gc; //set gain
ADCA.CH2.MUXCTRL = (uint8_t) ADC_CH_MUXPOS_PIN6_gc | ADC_CH_MUXNEG_GND_MODE4_gc;
//ADCA.CH2.MUXCTRL = (uint8_t) ADC_CH_MUXPOS_PIN6_gc | ADC_CH_MUXNEG_INTGND_MODE4_gc;
ADCA.CH2.INTCTRL = ADC_CH_INTMODE_COMPLETE_gc | ADC_CH_INTLVL_HI_gc;
ADCA.EVCTRL = (ADCA.EVCTRL & ~ADC_SWEEP_gm) | ADC_SWEEP_012_gc; //set sweep over channels 0,1,2
ADCA.EVCTRL = (ADCA.EVCTRL & ~ADC_EVSEL_gm) | ADC_EVSEL_567_gc; //event channels 5,6,7 as inputs
ADCA.EVCTRL = (ADCA.EVCTRL & ~ADC_EVACT_gm) | ADC_EVACT_SWEEP_gc; //event channel 5 triggers adc sweep
ADCA.CTRLA |= ADC_ENABLE_bm; //enable ADCA
ADCA.CH2.INTCTRL = ADC_CH_INTMODE_COMPLETE_gc | ADC_CH_INTLVL_LO_gc; //set up interrupts on ADC CH2
ADCA.INTFLAGS = ADC_CH2IF_bm; //clear interrupt flag
//set up RTC for PID
//OSC.CTRL |= OSC_RC32KEN_bm;
//do {} while ( ( OSC.STATUS & OSC_RC32KRDY_bm ) == 0);/* Wait for the 32kHz oscillator to stabilize. */
//CLK.RTCCTRL = (~CLK_RTCSRC_gm) | CLK_RTCSRC_RCOSC32_gc; //use 32.768 kHz from internal 32.768 kHz RC oscillator
//RTC.PER = 0xFFFF;
//RTC.CNT = 0; RTC.COMP = 0; //set count, compare values to zero
//RTC.CTRL = ( RTC.CTRL & ~RTC_PRESCALER_gm ) | RTC_PRESCALER_DIV1_gc; //set rtc prescalar and start counter
//set up pwm on TCC0
PORTC.DIRSET = PIN0_bm | PIN1_bm | PIN2_bm | PIN3_bm;
TCC0.PER = 0x4000; //set up 16 bit resolution for now, at 32 MHz with 1 clock divider, this is 500Hz PWM frequency
TCC0.CTRLB = ( TCC0.CTRLB & ~TC0_WGMODE_gm ) | TC_WGMODE_SINGLESLOPE_gc; //single slope mode
TCC0.CTRLB = ( TCC0.CTRLB & ~TC0_CCAEN_bm ) | TC0_CCAEN_bm; //set to output compare mode on channel A
TCC0.CTRLB = ( TCC0.CTRLB & ~TC0_CCBEN_bm ) | TC0_CCBEN_bm; //set to output compare mode on channel B
TCC0.CTRLB = ( TCC0.CTRLB & ~TC0_CCCEN_bm ) | TC0_CCCEN_bm; //set to output compare mode on channel C
TCC0.CTRLB = ( TCC0.CTRLB & ~TC0_CCDEN_bm ) | TC0_CCDEN_bm; //set to output compare mode on channel D
//set up pwm on TCD1
PORTD.DIRSET = PIN4_bm | PIN5_bm;
TCD1.PER = 0x4000; //set up 16 bit resolution for now, at 32 MHz with 1 clock divider, this is 500Hz PWM frequency
TCD1.CTRLB = ( TCD1.CTRLB & ~TC1_WGMODE_gm ) | TC_WGMODE_SINGLESLOPE_gc; //single slope mode
TCD1.CTRLB = ( TCD1.CTRLB & ~TC1_CCAEN_bm ) | TC1_CCAEN_bm; //set to output compare mode on channel A
TCD1.CTRLB = ( TCD1.CTRLB & ~TC1_CCBEN_bm ) | TC1_CCBEN_bm; //set to output compare mode on channel A
//set up ADC Sweep event
EVSYS.CH5MUX = EVSYS_CHMUX_TCC0_OVF_gc; //Configure event channel 5 to use TCC0 overflow as input
set_pwm_1(0,0);
set_pwm_2(0,0);
set_pwm_3(0,0);
//Start the PWM timer counter
TCC0.CTRLA = ( TCC0.CTRLA & ~TC0_CLKSEL_gm ) | TC_CLKSEL_DIV1_gc; //set clock divider and start
TCD1.CTRLA = ( TCD1.CTRLA & ~TC1_CLKSEL_gm ) | TC_CLKSEL_DIV1_gc; //set clock divider and start
PMIC.CTRL |= PMIC_LOLVLEX_bm;//enable low level interrupts
sei();
while (1) {
if(pid_mode){
//PID LOOP
pos_1 = ((int32_t)TCC1.PER)*mot_1_ovf + (int32_t)TCC1.CNT;
pos_2 = ((int32_t)TCE0.PER)*mot_2_ovf + (int32_t)TCE0.CNT;
pos_3 = ((int32_t)TCD0.PER)*mot_3_ovf + (int32_t)TCD0.CNT;
//calculate error