main.c

/*
 * atkbbb.c
 *
 * Created: 9/21/2018 4:18:52 PM
 * Author : Jake
 */

#include <avr/io.h>
#include <avr/interrupt.h>
#include "hardware.h"
#include "fastmath.h"
#include "pin.h"
#include "adc.h"

// PC1 is D2 / PWM
// PC7 is EN
// PC2 is IN1
// PC3 is IN2

void clock_init(void){
	OSC.XOSCCTRL = OSC_XOSCSEL_XTAL_256CLK_gc | OSC_FRQRANGE_12TO16_gc; // select external source
	OSC.CTRL = OSC_XOSCEN_bm; // enable external source
	while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // wait for external
	OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | OSC_PLLFAC0_bm | OSC_PLLFAC1_bm; // select external osc for pll, do pll = source * 3
	//OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | OSC_PLLFAC1_bm; // pll = source * 2 for 32MHz std clock
	OSC.CTRL |= OSC_PLLEN_bm; // enable PLL
	while (!(OSC.STATUS & OSC_PLLRDY_bm)); // wait for PLL to be ready
	CCP = CCP_IOREG_gc; // enable protected register change
	CLK.CTRL = CLK_SCLKSEL_PLL_gc; // switch to PLL for main clock
}

void uarts_init(void){
	rb_init(&up0rbrx);
	rb_init(&up0rbtx);
	pin_init(&up0rxled, &PORTF, PIN0_bm, 0, 1);
	pin_init(&up0txled, &PORTF, PIN1_bm, 1, 1);
	uart_init(&up0, &USARTF0, &PORTF, PIN2_bm, PIN3_bm, &up0rbrx, &up0rbtx, &up0rxled, &up0txled);

	uart_start(&up0, SYSTEM_BAUDA, SYSTEM_BAUDB);

	ups[0] = &up0;
}

void atkps_init(void){
	atkport_init(&atkp0, 0, &up0);
}

// -------------------------------- PWM GEN CODE

void set_pwm_ccb(uint16_t val){
	// clamp
	if(val > 4095){
		val = 4095;
	}

	uint8_t vall = (uint8_t) val;
	uint8_t valh = (uint8_t) (val >> 8);

	TCC0.CCBBUFL = vall;
	TCC0.CCBBUFH = valh;
}

// this is ~16khz
void hbridge_pwm_init(void){
	PORTC.DIRSET = PIN1_bm;

	TCC0.CTRLA = TC_CLKSEL_DIV1_gc;
	TCC0.CTRLB = TC_WGMODE_DSBOTH_gc | (1 << 5);

	uint16_t per = 4096;
	uint8_t perl = (uint8_t) per;
	uint8_t perh = (uint8_t) (per >> 8);

	TCC0.PERBUFL = perl;
	TCC0.PERBUFH = perh;

	set_pwm_ccb(1);
}

// -------------------------------- CONTROL TICKER

// this is ~ 4khz
void control_ticker_init(void){
	TCD1.CTRLA = TC_CLKSEL_DIV8_gc;
	TCD1.CTRLB = TC_WGMODE_NORMAL_gc;

	uint16_t per = 4096;
	uint8_t perl = (uint8_t) per;
	uint8_t perh = (uint8_t) (per >> 8);

	TCD1.PERL = perl;
	TCD1.PERH = perh;

	TCD1.INTCTRLA = TC_OVFINTLVL_MED_gc;
}

// -------------------------------- ADC CODE

void init_adc(void){
	/*
	inits adc for single-ended operation, unsigned, on PB0
	*/
	ADCB.CALL = SP_ReadCalibrationByte(PROD_SIGNATURES_START + ADCBCAL0_offset);
	ADCB.CALH = SP_ReadCalibrationByte(PROD_SIGNATURES_START + ADCBCAL1_offset);

	ADCB.CTRLB = ADC_RESOLUTION_12BIT_gc;// | ADC_CONMODE_bm; // conmode is for signed ADC

	ADCB.REFCTRL = ADC_REFSEL_INTVCC_gc;

	ADCB.PRESCALER = ADC_PRESCALER_DIV16_gc;

	ADCB.CH0.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc;
	ADCB.CH0.MUXCTRL = ADC_CH_MUXPOS_PIN0_gc;

	ADCB.CH0.INTCTRL = ADC_CH_INTMODE_COMPLETE_gc | ADC_CH_INTLVL_MED_gc;

	// start it up
	ADCB.CTRLA |= ADC_ENABLE_bm;
}

void kick_adc(void){
	ADCB.CH0.CTRL |= ADC_CH_START_bm;
}

uint16_t lookup[128] = {
	2047, 2147, 2247, 2347, 2446, 2544, 2641, 2736, 2830, 2922, 3011, 3099, 3184, 3266, 3345, 3421, 3494, 3563, 3629, 3691, 3749, 3802, 3852, 3897, 3938, 3974, 4005, 4032, 4054, 4071, 4084, 4091, 4094, 4091, 4084, 4071, 4054, 4032, 4005, 3974, 3938, 3897, 3852, 3802, 3749, 3691, 3629, 3563, 3494, 3421, 3345, 3266, 3184, 3099, 3011, 2922, 2830, 2736, 2641, 2544, 2446, 2347, 2247, 2147, 2047, 1947, 1847, 1747, 1648, 1550, 1453, 1358, 1264, 1172, 1083, 995, 910, 828, 749, 673, 600, 531, 465, 403, 345, 292, 242, 197, 156, 120, 89, 62, 40, 23, 10, 3, 0, 3, 10, 23, 40, 62, 89, 120, 156, 197, 242, 292, 345, 403, 465, 531, 600, 673, 749, 828, 910, 995, 1083, 1172, 1264, 1358, 1453, 1550, 1648, 1747, 1847, 1947
};

void init_dac(void){
	// DAC0 on PB2
	// DAC1 on PB3
	DACB.CTRLB = (DACB.CTRLB & ~DAC_CHSEL_gm) | DAC_CHSEL_DUAL_gc;
	DACB.CTRLC = (DACB.CTRLC & ~(DAC_REFSEL_gm | DAC_LEFTADJ_bm)) | DAC_REFSEL_AVCC_gc;
	DACB.CTRLA = (DACB.CTRLA & ~DAC_CH0EN_bm) | DAC_CH1EN_bm | DAC_ENABLE_bm;
}

void init_encoder(void){
	pin_init(&spiEncCsPin, &PORTD, PIN4_bm, 4, 1);
	spi_init(&spiEnc, &USARTD1, &PORTD, PIN6_bm, PIN7_bm, PIN5_bm, &spiEncCsPin);
	spi_start(&spiEnc, 1);
	ams5047_init(&ams5047, &spiEnc);
}

void init_control_vars(){
	// counter for poscontrol
	ctcntr = 0;

	// current control variables
	cc_r = 0; // current control reading
	cc_l = 0; // current control last
	cc_t = 750; // current control target
	cc_err = 0; // current control error
	cc_edt = 0; // current control error derivative
	cc_ei = 0; // current control error integral

	// current control parameters
	cc_kpn = 18;
	cc_kpdp = 2;

	cc_kin = 20;
	cc_kidp = 5;

	// control encoder variables
	pc_r = 0; // position control reading
	pc_l = 0; // position control last reading
	pc_t = 8192; // target ! // 8192 mid
	pc_err = 0; // position control error
	pc_edt = 0; // position control error derivative
	pc_ei = 0; // position control error integral

	pc_kpn = 24;
	pc_kpdp = 3;

	pc_kin = 4;
	pc_kidp = 8;

	pwm_output = 0;
}

// control hooks

void set_pc_t(uint32_t pcktval){
	if(pcktval > 16384){
		pcktval = 16384;
	}
	pc_t = pcktval;
}

void set_cc_t(uint16_t val){
	if(val > 4096){
		val = 4096;
	}
	cc_t = val;
}

// comment this code out
// write hwinterface for setting setpoint
// write wraparound / shortest-walk-to-pt
// swing some weight
// write update tuning variables
// draw new hardware, print

void control(){
	ctcntr ++;
	pin_set(&stldbg);
	if(ctcntr & 0b00000100){
		// position loop: 1/4th control loop
		ctcntr = 0;

		// get that encoder value
		pc_l = pc_r;
		ams5047_read(&ams5047, &pc_r);
		//DACB.CH1DATA = pc_r >> 2;

		// get that error
		pc_err = (int32_t)pc_t - (int32_t)pc_r;
		DACB.CH1DATA = abs(pc_err) >> 2;
		// k term
		pc_kterm = (pc_err * pc_kpn) >> pc_kpdp;

		// roll on to that integral, then limit
		pc_iterm += (pc_err * pc_kin) >> pc_kidp;
		if(pc_iterm > 4096){
			pc_iterm = 4096;
		} else if (pc_iterm < -4096){
			pc_iterm = -4096;
		}

		pc_term = pc_kterm + pc_iterm;

		// find direction based on current error
		if(pc_term < 0){
			pin_set(&stlerr);
			pin_set(&pin_inv);
		} else {
			pin_clear(&stlerr);
			pin_clear(&pin_inv);
		}

		// set targets in abs() ?
		pc_term = abs(pc_term);
		set_cc_t(pc_term);
		DACB.CH1DATA = cc_t;
	}
	// do pzwork if ~ 4th cc tick

	cc_l = cc_r;
	cc_r = ADCB.CH0.RES;

	// error
	cc_err = (int32_t)cc_t - (int32_t)cc_r;

	// integral, and limits
	// rethink this ?
	cc_ei += cc_err;
	if(cc_ei > 2048){
		cc_ei = 2048;
	} else if (cc_ei < -2048){
		cc_ei = -2048;
	}

	// check for zero-pwm and then assert
	pwm_output = ((cc_err * cc_kpn) >> cc_kpdp) + ((cc_ei * cc_kin) >> cc_kidp);
	if(pwm_output < 0){
		pwm_output = 0;
		//pin_clear(&stlerr);
	}
	//DACB.CH1DATA = pwm_output;
	set_pwm_ccb(pwm_output);
	pin_clear(&stldbg);
}

int main(void)
{
	// start clock
	clock_init();

	// start networking hardware
	uarts_init();
	atkps_init();

	// allow interrupts (and set handlers below)
	sei();
	PMIC.CTRL |= PMIC_LOLVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_HILVLEN_bm;

	// lights
	pin_init(&stlclk, &PORTE, PIN2_bm, 2, 1);
	pin_init(&stlerr, &PORTE, PIN3_bm, 3, 1);
	pin_set(&stlerr);

	// debug tick
	pin_init(&stldbg, &PORTB, PIN1_bm, 1, 1);
	pin_clear(&stldbg);

	// hbridge begin
	pin_init(&pin_en, &PORTC, PIN7_bm, 7, 1);
	pin_clear(&pin_en);
	pin_init(&pin_inv, &PORTC, PIN6_bm, 6, 1);
	pin_init(&pin_in1, &PORTC, PIN2_bm, 2, 1);
	pin_init(&pin_in2, &PORTC, PIN3_bm, 3, 1);

	// set direction '0'
	pin_clear(&pin_inv);
	pin_set(&pin_in2);
	pin_clear(&pin_in1); // in2 = 4.3v ?

	hbridge_pwm_init();

	pin_set(&pin_en);

	// adc begin
	init_adc();

	// dac begin
	init_dac();

	// spi begin
	init_encoder();

	// begin control ticker
	init_control_vars();
	control_ticker_init();

	uint16_t tck = 0;
	uint16_t angle = 0;
    while (1)
    {
		atkport_scan(&atkp0, 2);
		tck ++;
		if(!fastModulo(tck, 4096)){
			pin_toggle(&stlclk);
		}
    }
}


ISR(USARTF0_RXC_vect){
	uart_rxhandler(&up0);
}

ISR(USARTF0_DRE_vect){
	uart_txhandler(&up0);
}

ISR(TCD1_OVF_vect){
	//pin_toggle(&stlerr);
	kick_adc();
}

ISR(ADCB_CH0_vect){
	control();
}