LAUNCHXL-F28379D: F28379D

Most is copy from segbot/github : but i cant strat it up becauseof of some issues with memory: //#############################################################################
// FILE: segbot_main.c
//
// TITLE: Segbot
//#############################################################################

// Included Files

#include "segbot.h"
#include "F2837xD_device.h"
#include "device.h"
#include "driverlib.h"
#include "segbot.h"
#include "F2837xD_Examples.h"
#include <stdint.h>
#include <math.h>
#include "buffer.h"



// Include buffer instance
buffer_t dataBuffer;

// Global variables
float accelXreading = 0.0;
float accely_offset = 0.0;
float accelz_offset = 0.0;
float adcina2Volts = 0.0;
float adcina3Volts = 0.0;
uint16_t calibration_count = 0;
uint16_t calibration_state = 0;
float kalman_K = 0.0;
float kalman_P = 0.0;
float kalman_tilt = 0.0;
uint16_t numSWIcalls = 0;
float leftVel = 0.0;
float leftVelPrev = 0.0;
float leftWheel = 0.0;
float LeftWheelArray[4] = {0.0};
float leftWheelPrev = 0.0;
float rightVel = 0.0;
float rightVelPrev = 0.0;
float rightWheel = 0.0;
float RightWheelArray[4] = {0.0};
float rightWheelPrev = 0.0;
float tilt_array[4] = {0.0};
float tilt_value = 0.0;
uint16_t timecount = 0;
float turn = 0.0;
float turnError = 0.0;
float turnErrorInt = 0.0;
float turnErrorIntPrev = 0.0;
float turnErrorPrev = 0.0;
float turnRate = 0.0;
float turnRatePrev = 0.0;
float turnRef = 0.0;
float turnRefPrev = 0.0;
uint16_t UARTPrint = 0;
float ubal = 0.0;
float wheelDiff = 0.0;
float wheelDiffPrev = 0.0;
float wheelDiffVel = 0.0;
float wheelDiffVelPrev = 0.0;
int16_t SpibNumCalls = -1;

void main(void) {
// System initialization
DINT;
InitSysCtrl();
InitGpio();
EINT;

// Initialize buffer
buf_clear(&dataBuffer);

// GPIO setup
// Blue LED on LaunchPad
GPIO_SetupPinMux(31, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(31, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO31 = 1;

// Red LED on LaunchPad
GPIO_SetupPinMux(34, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(34, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBSET.bit.GPIO34 = 1;

// Setup LEDs
// LED1 and PWM Pin
GPIO_SetupPinMux(22, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(22, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPACLEAR.bit.GPIO22 = 1;

// LED2
GPIO_SetupPinMux(52, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(52, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;

// Additional LEDs (LED3 to LED23)
// ...

// Other initializations
setupSpib();
init_eQEPs();
setupEPWM5();
setupADC();
setupDAC();

// Initialize timers
InitCpuTimers();
ConfigCpuTimer(&CpuTimer0, 200, 1000);
ConfigCpuTimer(&CpuTimer1, 200, 4000);
ConfigCpuTimer(&CpuTimer2, 200, 40000);
CpuTimer0Regs.TCR.all = 0x4000;
CpuTimer1Regs.TCR.all = 0x4000;
CpuTimer2Regs.TCR.all = 0x4000;

// Initialize serial communication
init_serial(&SerialA, 115200, serialRXA);

// Enable interrupts
IER |= M_INT1;
IER |= M_INT8;
IER |= M_INT9;
IER |= M_INT12;
IER |= M_INT13;
IER |= M_INT14;
IER |= M_INT6;

// Enable individual PIE interrupts
PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
PieCtrlRegs.PIEIER1.bit.INTx1 = 1;
PieCtrlRegs.PIEIER6.bit.INTx3 = 1;

// Global interrupt enable
EINT;
ERTM;

// Main loop
while(1) {
if (UARTPrint == 1) {
serial_printf(&SerialA, "tilt_value: %.3f, gyro_value: %.3f, wheel vel: (%.3f, %.3f)\r\n", tilt_value, gyro_value, leftWheel, rightWheel);
UARTPrint = 0;
}
}
}

// Rest of the code with comments for each function
// ...


__interrupt void ADCA_ISR(void) {
GpioDataRegs.GPBSET.bit.GPIO52 = 1;

float adca2out = AdcaResultRegs.ADCRESULT0;
float adca3out = AdcaResultRegs.ADCRESULT1;
adcina2Volts = adca2out * (3.0 / 4095.0);
adcina3Volts = adca3out * (3.0 / 4095.0);

GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPIFFRX.bit.RXFFIL = 8;
SpibRegs.SPITXBUF = ((0x8000) | (0x3A00));
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;
SpibRegs.SPITXBUF = 0;

AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
GpioDataRegs.GPBCLEAR.bit.GPIO52 = 1;
}

__interrupt void SPIB_isr(void) {
SPIB_isr_count++;

GpioDataRegs.GPCSET.bit.GPIO66 = 1;
int16_t temp = SpibRegs.SPIRXBUF;
int16_t accelXraw = SpibRegs.SPIRXBUF;
int16_t accelYraw = SpibRegs.SPIRXBUF;
int16_t accelZraw = SpibRegs.SPIRXBUF;
temp = SpibRegs.SPIRXBUF;
int16_t gyroXraw = SpibRegs.SPIRXBUF;
int16_t gyroYraw = SpibRegs.SPIRXBUF;
int16_t gyroZraw = SpibRegs.SPIRXBUF;
accelXreading = accelXraw * 4.0 / 32767.0;
accelYreading = accelYraw * 4.0 / 32767.0;
accelZreading = accelZraw * 4.0 / 32767.0;
gyroXreading = gyroXraw * 250.0 / 32767.0;
gyroYreading = gyroYraw * 250.0 / 32767.0;
gyroZreading = gyroZraw * 250.0 / 32767.0;
leftWheel = -readEncLeft();
rightWheel = -readEncRight();

if (calibration_state == 0) {
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 1;
calibration_count = 0;
}
} else if (calibration_state == 1) {
accelx_offset += accelXreading;
accely_offset += accelYreading;
accelz_offset += accelZreading;
gyrox_offset += gyroXreading;
gyroy_offset += gyroYreading;
gyroz_offset += gyroZreading;
calibration_count++;
if (calibration_count == 2000) {
calibration_state = 2;
accelx_offset /= 2000.0;
accely_offset /= 2000.0;
accelz_offset /= 2000.0;
gyrox_offset /= 2000.0;
gyroy_offset /= 2000.0;
gyroz_offset /= 2000.0;
calibration_count = 0;
doneCal = 1;
}
} else if (calibration_state == 2) {
accelXreading -= (accelx_offset);
accelYreading -= (accely_offset);
accelZreading -= (accelz_offset - accelzBalancePoint);
gyroXreading -= gyrox_offset;
gyroYreading -= gyroy_offset;
gyroZreading -= gyroz_offset;

float tiltrate = (gyroXreading * M_PI) / 180.0;
float pred_tilt, z, y, S;
pred_tilt = kalman_tilt + T * tiltrate;
pred_P = kalman_P + Q;
z = -accelZreading;
y = z - pred_tilt;
S = pred_P + R;
kalman_K = pred_P / S;
kalman_tilt = pred_tilt + kalman_K * y;
kalman_P = (1 - kalman_K) * pred_P;
SpibNumCalls++;
tilt_array[SpibNumCalls] = kalman_tilt;
gyro_array[SpibNumCalls] = tiltrate;
LeftWheelArray[SpibNumCalls] = -readEncLeft();
RightWheelArray[SpibNumCalls] = -readEncRight();
if (SpibNumCalls >= 3) {
tilt_value = (tilt_array[0] + tilt_array[1] + tilt_array[2] + tilt_array[3]) / 4.0;
gyro_value = (gyro_array[0] + gyro_array[1] + gyro_array[2] + gyro_array[3]) / 4.0;
leftWheel = (LeftWheelArray[0] + LeftWheelArray[1] + LeftWheelArray[2] + LeftWheelArray[3]) / 4.0;
rightWheel = (RightWheelArray[0] + RightWheelArray[1] + RightWheelArray[2] + RightWheelArray[3]) / 4.0;
SpibNumCalls = -1;
PieCtrlRegs.PIEIFR12.bit.INTx9 = 1;
}
}
timecount++;
if ((timecount % 200) == 0) {
if (doneCal == 0) GpioDataRegs.GPATOGGLE.bit.GPIO31 = 1;
GpioDataRegs.GPBTOGGLE.bit.GPIO34 = 1;
UARTPrint = 1;
}

SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1;
SpibRegs.SPIFFRX.bit.RXFFINTCLR = 1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP6;
}

__interrupt void SWI_isr(void) {
PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
asm(" NOP");
EINT;

leftVel = 0.6 * leftVelPrev + 100 * (leftWheel - leftWheelPrev);
rightVel = 0.6 * rightVelPrev + 100 * (rightWheel - rightWheelPrev);

ubal = -K1 * tilt_value - K2 * gyro_value - K3 * ((leftVel + rightVel) / 2.0);

wheelDiff = leftWheel - rightWheel;
wheelDiffVel = 0.333 * wheelDiffVelPrev + 166.667 * (wheelDiff - wheelDiffPrev);
turnError = turnRef - wheelDiff;
turnErrorInt = turnErrorIntPrev + 0.004 * ((turnError + turnErrorPrev) / 2);
turnRef = turnRefPrev + 0.004 * ((turnRate + turnRatePrev) / 2);
turn = Kp * turnError + Ki * turnErrorInt - Kd * wheelDiffVel;
if (fabs(turn) > 3) turnErrorInt = turnErrorIntPrev;
if (turn >= 4) turn = 4;
if (turn <= -4) turn = -4;

uLeft = (ubal / 2) + turn + FwdBackOffset;
uRight = (ubal / 2) - turn + FwdBackOffset;
if (uLeft > 10) uLeft = 10;
if (uLeft < -10) uLeft = -10;
if (uRight > 10) uRight = 10;
if (uRight < -10) uRight = -10;
setEPWM6A(uLeft);
setEPWM6B(uRight);

leftVelPrev = leftVel;
leftWheelPrev = leftWheel;
rightVelPrev = rightVel;
rightWheelPrev = rightWheel;
wheelDiffPrev = wheelDiff;
wheelDiffVelPrev = wheelDiffVel;
turnErrorIntPrev = turnErrorInt;
turnErrorPrev = turnError;
turnRefPrev = turnRef;
turnRatePrev = turnRate;

numSWIcalls++;
DINT;
}

float readEncLeft(void) {
int32_t raw = 0;
uint32_t QEP_maxvalue = 0xFFFFFFFFU;
raw = EQep1Regs.QPOSCNT;
if (raw >= QEP_maxvalue / 2) raw -= QEP_maxvalue;
return (raw * (-2 * M_PI / (80 * 18.7)));
}

float readEncRight(void) {
int32_t raw = 0;
uint32_t QEP_maxvalue = 0xFFFFFFFFU;
raw = EQep2Regs.QPOSCNT;
if (raw >= QEP_maxvalue / 2) raw -= QEP_maxvalue;
return (raw * (2 * M_PI / (80 * 18.7)));
}

void setEPWM6A(float controleffort) {
controleffort = -controleffort;
if (controleffort > 10) controleffort = 10;
if (controleffort < -10) controleffort = -10;
if (controleffort >= 0) GpioDataRegs.GPACLEAR.bit.GPIO29 = 1;
else GpioDataRegs.GPASET.bit.GPIO29 = 1;
EPwm6Regs.CMPA.bit.CMPA = (fabs(controleffort) / 10.0) * EPwm6Regs.TBPRD;
}

void setEPWM6B(float controleffort) {
controleffort = -controleffort;
if (controleffort > 10) controleffort = 10;
if (controleffort < -10) controleffort = -10;
if (controleffort >= 0) GpioDataRegs.GPBSET.bit.GPIO32 = 1;
else GpioDataRegs.GPBCLEAR.bit.GPIO32 = 1;
EPwm6Regs.CMPB.bit.CMPB = (fabs(controleffort) / 10.0) * EPwm6Regs.TBPRD;
}

void setDACA(float dacouta0) {
if (dacouta0 > 3.0) dacouta0 = 3.0;
if (dacouta0 < 0.0) dacouta0 = 0.0;
DacaRegs.DACVALS.bit.DACVALS = (int)(dacouta0 / 3.0 * 4095);
}

void setDACB(float dacouta1) {
if (dacouta1 > 3.0) dacouta1 = 3.0;
if (dacouta1 < 0.0) dacouta1 = 0.0;
DacbRegs.DACVALS.bit.DACVALS = (int)(dacouta1 / 3.0 * 4095);
}

void serialRXA(serial_t *s, char data) {
numRXA++;
if (data == 'a') {
turnRate = turnRate - 0.2;
} else if (data == 'd') {
turnRate = turnRate + 0.2;
} else if (data == 'w') {
FwdBackOffset = FwdBackOffset - 0.2;
} else if (data == 's') {
FwdBackOffset = FwdBackOffset + 0.2;
} else {
turnRate = 0;
FwdBackOffset = 0;
}

// Write data to buffer
buf_write_1(&dataBuffer, data);
}

__interrupt void cpu_timer0_isr(void) {
CpuTimer0.InterruptCount++;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP1;
}

__interrupt void cpu_timer1_isr(void) {
CpuTimer1.InterruptCount++;
}

__interrupt void cpu_timer2_isr(void) {
CpuTimer2.InterruptCount++;
}

void init_eQEPs(void) {
EALLOW;
GpioCtrlRegs.GPAPUD.bit.GPIO20 = 1;
GpioCtrlRegs.GPAPUD.bit.GPIO21 = 1;
GpioCtrlRegs.GPAQSEL2.bit.GPIO20 = 2;
GpioCtrlRegs.GPAQSEL2.bit.GPIO21 = 2;
EDIS;
GPIO_SetupPinMux(20, GPIO_MUX_CPU1, 1);
GPIO_SetupPinMux(21, GPIO_MUX_CPU1, 1);
EQep1Regs.QEPCTL.bit.QPEN = 0;
EQep1Regs.QDECCTL.bit.QSRC = 0;
EQep1Regs.QPOSCTL.all = 0x0;
EQep1Regs.QCAPCTL.all = 0x0;
EQep1Regs.QEINT.all = 0x0;
EQep1Regs.QPOSMAX = 0xFFFFFFFF;
EQep1Regs.QEPCTL.bit.FREE_SOFT = 2;
EQep1Regs.QEPCTL.bit.QPEN = 1;
EQep1Regs.QPOSCNT = 0;

EALLOW;
GpioCtrlRegs.GPBPUD.bit.GPIO54 = 1;
GpioCtrlRegs.GPBPUD.bit.GPIO55 = 1;
GpioCtrlRegs.GPBQSEL2.bit.GPIO54 = 2;
GpioCtrlRegs.GPBQSEL2.bit.GPIO55 = 2;
EDIS;
GPIO_SetupPinMux(54, GPIO_MUX_CPU1, 5);
GPIO_SetupPinMux(55, GPIO_MUX_CPU1, 5);
EQep2Regs.QEPCTL.bit.QPEN = 0;
EQep2Regs.QDECCTL.bit.QSRC = 0;
EQep2Regs.QPOSCTL.all = 0x0;
EQep2Regs.QCAPCTL.all = 0x0;
EQep2Regs.QEINT.all = 0x0;
EQep2Regs.QPOSMAX = 0xFFFFFFFF;
EQep2Regs.QEPCTL.bit.FREE_SOFT = 2;
EQep2Regs.QEPCTL.bit.QPEN = 1;
EQep2Regs.QPOSCNT = 0;
}

void setupEPWM5(void) {
EALLOW;
EPwm5Regs.ETSEL.bit.SOCAEN = 0;
EPwm5Regs.TBCTL.bit.CTRMODE = 3;
EPwm5Regs.ETSEL.bit.SOCASEL = 2;
EPwm5Regs.ETPS.bit.SOCAPRD = 1;
EPwm5Regs.TBCTR = 0x0;
EPwm5Regs.TBPHS.bit.TBPHS = 0x0000;
EPwm5Regs.TBCTL.bit.PHSEN = 0;
EPwm5Regs.TBCTL.bit.CLKDIV = 0;
EPwm5Regs.TBPRD = 50000;
EPwm5Regs.ETSEL.bit.SOCAEN = 1;
EPwm5Regs.TBCTL.bit.CTRMODE = TB_COUNT_UP;
EDIS;
}

void setupADC(void) {
EALLOW;
AdcaRegs.ADCCTL2.bit.PRESCALE = 6;
AdcbRegs.ADCCTL2.bit.PRESCALE = 6;
AdccRegs.ADCCTL2.bit.PRESCALE = 6;
AdcdRegs.ADCCTL2.bit.PRESCALE = 6;
AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);
AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);
AdcSetMode(ADC_ADCC, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);
AdcSetMode(ADC_ADCD, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE);
AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdccRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcdRegs.ADCCTL1.bit.INTPULSEPOS = 1;
AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdccRegs.ADCCTL1.bit.ADCPWDNZ = 1;
AdcdRegs.ADCCTL1.bit.ADCPWDNZ = 1;
DELAY_US(1000);
AdcaRegs.ADCSOC0CTL.bit.CHSEL = 0x2;
AdcaRegs.ADCSOC0CTL.bit.ACQPS = 14;
AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 0xD;
AdcaRegs.ADCSOC1CTL.bit.CHSEL = 0x3;
AdcaRegs.ADCSOC1CTL.bit.ACQPS = 14;
AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 0xD;
AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 0;
AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1;
AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1;
EDIS;
}

void setupDAC(void) {
EALLOW;
DacaRegs.DACCTL.bit.LOADMODE = 0;
DacaRegs.DACOUTEN.bit.DACOUTEN = 1;
DacaRegs.DACCTL.bit.DACREFSEL = 1;
DacbRegs.DACOUTEN.bit.DACOUTEN = 1;
DacbRegs.DACCTL.bit.LOADMODE = 0;
DacbRegs.DACCTL.bit.DACREFSEL = 1;
EDIS;
}



void setupSpib(void) {
// Initialiser GPIO for SPI-bruk
GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(2, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPASET.bit.GPIO2 = 1;
GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
GPIO_SetupPinMux(63, GPIO_MUX_CPU1, 15);
GPIO_SetupPinMux(64, GPIO_MUX_CPU1, 15);
GPIO_SetupPinMux(65, GPIO_MUX_CPU1, 15);
EALLOW;
GpioCtrlRegs.GPBPUD.bit.GPIO63 = 0;
GpioCtrlRegs.GPCPUD.bit.GPIO64 = 0;
GpioCtrlRegs.GPCPUD.bit.GPIO65 = 0;
GpioCtrlRegs.GPBQSEL2.bit.GPIO63 = 3;
GpioCtrlRegs.GPCQSEL1.bit.GPIO64 = 3;
GpioCtrlRegs.GPCQSEL1.bit.GPIO65 = 3;
EDIS;

// Initialiser SPIB-registrene
SpibRegs.SPICCR.bit.SPISWRESET = 0;
SpibRegs.SPICTL.bit.CLK_PHASE = 1;
SpibRegs.SPICCR.bit.CLKPOLARITY = 0;
SpibRegs.SPICTL.bit.MASTER_SLAVE = 1;
SpibRegs.SPICCR.bit.SPICHAR = 0xF;
SpibRegs.SPICTL.bit.TALK = 0x1;
SpibRegs.SPIPRI.bit.FREE = 1;
SpibRegs.SPICTL.bit.SPIINTENA = 0;
SpibRegs.SPIBRR.bit.SPI_BIT_RATE = 0x32;
SpibRegs.SPISTS.all = 0x0000;
SpibRegs.SPIFFTX.bit.SPIRST = 0x1;
SpibRegs.SPIFFTX.bit.SPIFFENA = 0x1;
SpibRegs.SPIFFTX.bit.TXFIFO = 0;
SpibRegs.SPIFFTX.bit.TXFFINTCLR = 1;
SpibRegs.SPIFFRX.bit.RXFIFORESET = 0;
SpibRegs.SPIFFRX.bit.RXFFOVFCLR = 1;
SpibRegs.SPIFFRX.bit.RXFFINTCLR = 0x1;
SpibRegs.SPIFFRX.bit.RXFFIENA = 0x1;
SpibRegs.SPIFFCT.bit.TXDLY = 0x00;
SpibRegs.SPICCR.bit.SPISWRESET = 0x1;
SpibRegs.SPIFFTX.bit.TXFIFO = 0x1;
SpibRegs.SPIFFRX.bit.RXFIFORESET = 1;
SpibRegs.SPICTL.bit.SPIINTENA = 1;
SpibRegs.SPIFFRX.bit.RXFFIL = 0x10;

// Initialiser SPI-kommunikasjon med slaven
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x1300 | 0x0000);
SpibRegs.SPITXBUF = (0x0000 | 0x0000);
SpibRegs.SPITXBUF = (0x0000 | 0x0000);
SpibRegs.SPITXBUF = (0x0000 | 0x0013);
SpibRegs.SPITXBUF = (0x0200 | 0x0000);
SpibRegs.SPITXBUF = (0x0800 | 0x0006);
SpibRegs.SPITXBUF = (0x0000 | 0x0000);

// Vent til mottaksbufferen er fylt opp
while (SpibRegs.SPIFFRX.bit.RXFFST != 7);
GpioDataRegs.GPCSET.bit.GPIO66 = 1; // Slave Select High

// Les av verdiene fra mottaksbufferen
int i;
float temp;
for (i = 0; i < 7; i++) {
temp = SpibRegs.SPIRXBUF;
// Legg til en dummy operasjon for å unngå advarsel om ubrukt variabel
temp += 0.0;
}

DELAY_US(10); // Delay 10us to allow time for the MPU-2950 to get ready for next transfer.

// Fortsett initialisering av MPU-9250-registere
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1; // Slave Select Low
SpibRegs.SPITXBUF = (0x2300 | 0x0000);
SpibRegs.SPITXBUF = (0x4000 | 0x008C);
SpibRegs.SPITXBUF = (0x0200 | 0x0088);
SpibRegs.SPITXBUF = (0x0C00 | 0x000A);
while (SpibRegs.SPIFFRX.bit.RXFFST != 4);
GpioDataRegs.GPCSET.bit.GPIO66 = 1; // Slave Select High
for (i = 0; i < 4; i++) {
temp = SpibRegs.SPIRXBUF;
temp += 0.0; // Dummy operation
}
DELAY_US(10);

// Flere MPU-registere
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x2A00 | 0x0081);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
temp += 0.0; // Dummy operation
DELAY_US(10);

// Fortsett med flere overføringer
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x3800 | 0x0001);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
temp += 0.0; // Dummy operation
DELAY_US(10);

// Fortsett med resten av initialiseringen på samme måte...

GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x3A00 | 0x0001);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x6400 | 0x0001);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x6700 | 0x0003);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x6A00 | 0x0020);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x6B00 | 0x0001);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x7500 | 0x0071);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x7700 | 0x0000);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x7800 | 0x0000);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x7A00 | 0x0000);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x7B00 | 0x0000);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x7D00 | 0x0021);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(10);
GpioDataRegs.GPCCLEAR.bit.GPIO66 = 1;
SpibRegs.SPITXBUF = (0x7E00 | 0x0050);
while (SpibRegs.SPIFFRX.bit.RXFFST != 1);
GpioDataRegs.GPCSET.bit.GPIO66 = 1;
temp = SpibRegs.SPIRXBUF;
DELAY_US(50);



// Clear SPIB interrupt source just in case it was issued due to any of the above
SpibRegs.SPIFFRX.bit.RXFFOVFCLR=1;
SpibRegs.SPIFFRX.bit.RXFFINTCLR=1;
PieCtrlRegs.PIEACK.all = PIEACK_GROUP6;
}

segbot.h

#ifndef SEGBOT_H_
#define SEGBOT_H_

#include "F2837xD_device.h"
#include "F2837xD_Examples.h"
#include "f28379dSerial.h"
#include "driverlib.h"

// Globale variabler

extern uint32_t numRXA;
extern uint32_t SPIB_isr_count;
extern float K1, K2, K3;
extern float Kp, Ki, Kd;
extern float adcina2Volts, adcina3Volts;
extern float T;
extern float Q;
extern float R;
extern float pred_P;
extern buffer_t dataBuffer;
extern serial_t SerialA;
extern float uLeft, uRight;
extern float accelxBalancePoint, accelyBalancePoint, accelzBalancePoint;
extern float accelx_offset, accely_offset, accelz_offset, accelXreading, accelYreading, accelZreading;
extern float gyroXreading, gyroYreading, gyroZreading, gyrox_offset, gyroy_offset, gyroz_offset;
extern float kalman_tilt, kalman_P, kalman_K;
extern float leftWheel, rightWheel, leftVel, rightVel, ubal, tilt_value, gyro_value;
extern float FwdBackOffset, turn, turnError, turnErrorInt, turnRef, turnRate, wheelDiff, wheelDiffVel;
extern float turnErrorIntPrev, turnErrorPrev, turnRefPrev, turnRatePrev, leftVelPrev, rightVelPrev, leftWheelPrev, rightWheelPrev, wheelDiffPrev, wheelDiffVelPrev;
extern uint32_t doneCal;
extern float tilt_array[4], gyro_array[4], LeftWheelArray[4], RightWheelArray[4];


// Funksjonsprototyper
void F28x_usDelay(long);
void ConfigCpuTimer(struct CPUTIMER_VARS *, float, float);
void InitCpuTimers(void);
void InitGpio(void);
void InitSysCtrl(void);
uint16_t init_serial(serial_t *, Uint32, void (*)(serial_t *, char));
void GPIO_SetupPinMux(Uint16, Uint16, Uint16);
void GPIO_SetupPinOptions(Uint16, Uint16, Uint16);
void AdcSetMode(Uint16, Uint16, Uint16);
uint16_t serial_printf(serial_t *, char *, ...);

void setupSpib(void);
void init_eQEPs(void);
void setupEPWM5(void);
void setupADC(void);
void setupDAC(void);
void serialRXA(serial_t *s, char data);
float readEncLeft(void);
float readEncRight(void);
void setEPWM6A(float controleffort);
void setEPWM6B(float controleffort);
void setDACA(float dacouta0);
void setDACB(float dacouta1);

#endif /* SEGBOT_H_ */

buffer.h 

/*
* buffer.h
*
* Created on: 5. jul. 2024
* Author: obsfe
*/

#ifndef __BUFFER_H__
#define __BUFFER_H__

#define BUF_SIZE (4*1000 + 8)

typedef volatile struct buffer_s {
volatile char buf[BUF_SIZE];
volatile Uint16 head, tail, size;
} buffer_t;

extern inline void init_buffer(buffer_t *b)
{
b->size = b->head = b->tail = 0;
}
#define buf_clear(b) init_buffer(b)

#ifdef _FLASH
#pragma CODE_SECTION(buf_write_1, ".TI.ramfunc");
#endif
extern inline uint16_t buf_write_1(buffer_t *b, char data)
{
if (b->size == BUF_SIZE) return 2;
b->buf[b->head] = data;
b->head = (b->head+1)%BUF_SIZE;
b->size++;
return 0;
}

#ifdef _FLASH
#pragma CODE_SECTION(buf_read_1, ".TI.ramfunc");
#endif
extern inline uint16_t buf_read_1(buffer_t *b, Uint16 offset, char *data)
{
if (b->size == 0 || b->size < offset) return 3;
*data = b->buf[ (b->tail+offset)%BUF_SIZE ];
return 0;
}

#ifdef _FLASH
#pragma CODE_SECTION(buf_remove, ".TI.ramfunc");
#endif
extern inline uint16_t buf_remove(buffer_t *b, Uint16 len)
{
len = (len > b->size) ? b->size : len;
b->tail = (b->tail+len)%BUF_SIZE;
b->size -= len;
return len;
}

#endif /* __BUFFER_H__ */