•

Adrian Marquez

•

Shanbo Zhang

Published December 18, 2025

ME 461 LQR + L1 Adaptive Controller for Segbot

Guaranteed robustness and compensate for the effect of unmodeled dynamics & disturbances

IntermediateShowcase (no instructions)5 hours73

ME 461 LQR + L1 Adaptive Controller for Segbot

Things used in this project

Hardware components

Texas Instruments LAUNCHXL-F28379D C2000 Delfino LaunchPad

thingSoC ESP32 WiFi Module

Software apps and online services

Texas Instruments Code Composer Studio

LabVIEW Community Edition

Story

Our Project

Baseline (PID Segbot) Disturbance Test

Our Project (L1+LQR Segbot) Disturbance and Uneven Terrain Test

Project Explanation

Last year’s, an ME 461 group's Segbot project focused on adaptive balancing and did not explicitly use a full model of the Segbot dynamics; this project’s goal is model-based controller design and trajectory tracking in addition to self-balancing. We derived the dynamic model of the Segbot, linearized it, and supported real-time control by game controllers.

The LQR controller provides a stronger robustness guarantee than a hand-tuned PID controller in class and ensures an infinite gain margin and at least a 60-degree phase margin. In addition, the L1 controller helps automatically compensate for the effects of unmodeled dynamics and disturbances, such as uneven terrain, mass changes, and shifts of the center of mass. At first, we’d like to derive a dynamic model of the Segbot. Due to the Segbot's asymmetric structure and complex geometry, we measured key parameters experimentally. In particular, we estimated moments of inertia by pendulum-based measurements. We also measured the relationship between motor voltage and wheel output torque through a DC motor model. After measuring the fundamental parameters of the Segbot in our model, we obtained its nonlinear dynamics. From there, we linearized the model about the upright equilibrium, yielding a continuous-time state-space representation of the Segbot dynamics, which is used for control design in the following sections.

Then, we extended the Lab 7 LabVIEW framework to support real-time control using a game controller (wired or wireless). User inputs from joystick motion and button presses are captured through the Windows API and mapped to high-level Segbot commands, enabling intuitive manual control of the robot’s behavior. This interface allows users to interactively command the Segbot during experiments and facilitates testing of balancing and trajectory tracking under user-specified inputs.

In our final system, the Segbot demonstrated much greater stability under conditions of uneven terrain, mass changes, and changes in center of mass. We set up boards of varying heights to create uneven terrain, and the Segbot went through them without tipping over. We also kept adding batteries while it was running (rather than placing them at the start), and the Segbot didn't flip or crash.

Sensors and Actuators

Sensors and Actuators and our Control Loop

Algorithm (L1+LQR)

The Segbot is modeled as an underactuated two-wheeled inverted pendulum with:

S. Kim and S. Kwon, "Nonlinear Optimal Control Design for Underactuated Two-Wheeled Inverted Pendulum Mobile Platform," in IEEE/ASME Transactions on Mechatronics, vol. 22, no. 6, pp. 2803-2808, Dec. 2017

1) Linearized model used for control design

Linearize the segbot dynamics about the upright (zero) equilibrium:

State and input:

η = [x, x_dot, θ, θ_dot, ψ, ψ_dot]^T

u = [T_L, T_R]^T

Linear state-space form:

η_dot = A η + B u, where A and B are matrices with values based on the segbot's physical parameters

2) Integral LQR baseline (trajectory tracking)

Define an integral-of-tracking-error state:

z_dot = η − η_ref

Augmented state:

x_aug = [z; η]

LQR objective:

J = ∫ ( x_aug^T Q x_aug + u^T R u ) dt

LQR feedback law (continuous-time):

u_LQR = −K x_aug

In the code this is implemented as two gain rows (one for each wheel):

u_b_left = -K_left · x

u_b_right = -K_right · x

The controller state is built directly from onboard sensing + integrators, e.g.:

tilt and tilt-rate come from IMU processing (Kalman estimate + short averaging), wheel encoders give speed and wheel-difference (turning), and integral states accumulate tracking error for better steady-state tracking.

The code uses trapezoidal integration for the integral states:

I[k] = I[k-1] + Ts * (e[k-1] + e[k]) / 2

the gain K comes from minimizing

J = ∫ (x^T Q x + u^T R u) dt

leading to K from the algebraic Riccati equation solution.

Detailed LQR Explanation: https://underactuated.mit.edu/lqr.html

3) L1 adaptive augmentation (disturbance robustness)

Model disturbances/uncertainty:

η_dot = A η + B u + B σ(t)

Total control:

u = u_LQR + u_L1

Core L1 idea: keep robustness while allowing fast adaptation

u_L1(s) = −C(s) σ_hat(s)

Total command sent to motors :

u_left = u_b_left + u_L1_left

u_right = u_b_right + u_L1_right

State predictor (discrete-time update each sample):

x_hat[k] = x_hat[k-1] + Ts * x_hat_dot[k-1]

with a predictor derivative of the form:

x_hat_dot = A x + Bm (u_b + u_L1 + sigma_m_hat) + Bum sigma_um_hat + Ae (x_hat - x)

Uncertainty estimate (from prediction error):

x_tilde = x_hat - x

sigma_hat = G * x_tilde

Split into “matched” (affects input channel) vs “unmatched” parts:

sigma_m_hat = first 2 components of sigma_hat

sigma_um_hat = remaining components (mapped through B_um)

Bandwidth-limited adaptive control with low pass filter:

u_L1[k] = -( alpha * u_L1[k-1] + (1 - alpha) * sigma_m_hat[k] )

alpha = exp(-omega * Ts)

That low-pass filtering is what enables fast estimation and filtered control action to preserve robustness margins.

Detailed comparison of augmented L1 adaptive system to a baseline controller:

Michini, B. and J. How. " L1 Adaptive Control for Indoor Autonomous Vehicles: Design Process and Flight Testing." AIAA Guidance, Navigation, and Control Conference 10-13 August 2009, Chicago, Illinois, AIAA 2009-5754. https://dspace.mit.edu/handle/1721.1/57900

Our Team

Not in this shot: Hyungsoo Kang (with us virtually from Korea!).

//#############################################################################
// FILE:   LAB6starter_main.c
//
// TITLE:  Lab Starter
//#############################################################################

// Included Files
#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>
#include <string.h>
#include <math.h>
#include <limits.h>
#include "F28x_Project.h"
#include "driverlib.h"
#include "device.h"
#include "F28379dSerial.h"
#include "LEDPatterns.h"
#include "song.h"
#include "dsp.h"
#include "fpu32/fpu_rfft.h"

#define PI          3.1415926535897932384626433832795
#define TWOPI       6.283185307179586476925286766559
#define HALFPI      1.5707963267948966192313216916398
// The Launchpad's CPU Frequency set to 200 you should not change this value
#define LAUNCHPAD_CPU_FREQUENCY 200

#define WHEEL_RADIUS 0.0593
#define SEGBOT_WIDTH 0.173

// ----- code for CAN start here -----
#include "F28379dCAN.h"
//#define TX_MSG_DATA_LENGTH    4
//#define TX_MSG_OBJ_ID         0  //transmit

#define RX_MSG_DATA_LENGTH    8
#define RX_MSG_OBJ_ID_1       1  //measurement from sensor 1
#define RX_MSG_OBJ_ID_2       2  //measurement from sensor 2
#define RX_MSG_OBJ_ID_3       3  //quality from sensor 1
#define RX_MSG_OBJ_ID_4       4  //quality from sensor 2
// ----- code for CAN end here -----

// Nth order FIR LPF
#define NSIZE 21

// Interrupt Service Routines predefinition
__interrupt void cpu_timer0_isr(void);
__interrupt void cpu_timer1_isr(void);
__interrupt void cpu_timer2_isr(void);
__interrupt void SWI_isr(void);
// ----- code for CAN start here -----
__interrupt void can_isr(void);
// ----- code for CAN end here -----
__interrupt void SPIB_isr(void);
void setupSpib(void);

void init_eQEPs(void);
float readEncLeft(void);
float readEncRight(void);
void setEPWM2A(float);
void setEPWM2B(float);

__interrupt void ADCA_ISR (void);

void L1_AC(void);

void multiplyMatrixVector_1(float mat[][7], float vec[], float result[]);
void multiplyMatrixVector_2(float mat[][2], float vec[], float result[]);
void multiplyMatrixVector_3(float mat[][5], float vec[], float result[]);
void vector_add2(float vec1[], float vec2[], float result[], int size);
void vector_add3(float vec1[], float vec2[], float vec3[], float result[], int size);
void vector_add4(float vec1[], float vec2[], float vec3[], float vec4[],float result[], int size);
void vector_sub(float vec1[], float vec2[], float result[], int size);
void vector_assign(float vec[], float result[], int size);
void vector_scale(float scale, float vec[], float result[], int size);

// Count variables
uint32_t numTimer0calls = 0;
uint32_t numSWIcalls = 0;
extern uint32_t numRXA;
uint16_t UARTPrint = 0;
uint16_t LEDdisplaynum = 0;
int16_t accelx_raw = 0;
int16_t accely_raw = 0;
int16_t accelz_raw = 0;
int16_t gyrox_raw = 0;
int16_t gyroy_raw = 0;
int16_t gyroz_raw = 0;

float accelx = 0;
float accely = 0;
float accelz = 0;

float gyrox = 0;
float gyroy = 0;
float gyroz = 0;

int32_t SpibNumCalls = 0;

// ----- code for CAN start here -----
// volatile uint32_t txMsgCount = 0;
// extern uint16_t txMsgData[4];

volatile uint32_t rxMsgCount_1 = 0;
volatile uint32_t rxMsgCount_3 = 0;
extern uint16_t rxMsgData[8];

uint32_t dis_raw_1[2];
uint32_t dis_raw_3[2];
uint32_t dis_1 = 0;
uint32_t dis_3 = 0;

uint32_t quality_raw_1[4];
uint32_t quality_raw_3[4];
float quality_1 = 0.0;
float quality_3 = 0.0;

uint32_t lightlevel_raw_1[4];
uint32_t lightlevel_raw_3[4];
float lightlevel_1 = 0.0;
float lightlevel_3 = 0.0;

uint32_t measure_status_1 = 0;
uint32_t measure_status_3 = 0;

volatile uint32_t errorFlag = 0;
// ----- code for CAN end here -----

float LeftWheel;
float RightWheel;

float PosLeft;
float PosRight;

float uLeft=5.0;
float uRight=5.0;

float vLeft;
float vRight;

float PosLeft_prev=0;
float PosRight_prev=0;

float Vref=0.25;
float turn=0;

float e_turn;

float e_Left;
float e_Left_prev=0;

float e_Right;
float e_Right_prev=0;

float I_Left;
float I_Left_prev=0;

float I_Right;
float I_Right_prev=0;

float Kp=3;
float Ki=20;
float Kd=0.08;
float Kturn=3;

float x, x_prev=0, y, y_prev=0, phi;
float x_dot, x_dot_prev=0, y_dot, y_dot_prev=0;

float theta_avg, theta_avg_prev=0;
float theta_avg_dot;

float distright, distfront;
float Kpright = 0.001, Kpfront = .0002, ref_right = 200, ref_front = 1400;
uint16_t right_wall_follow=1;

float printLV3 = 0;
float printLV4 = 0;
float printLV5 = 0;
float printLV6 = 0;
float printLV7 = 0;
float printLV8 = 0;
float x = 0;
float y = 0;
float bearing = 0;
extern uint16_t NewLVData;
extern float fromLVvalues[LVNUM_TOFROM_FLOATS];
extern LVSendFloats_t DataToLabView;
extern char LVsenddata[LVNUM_TOFROM_FLOATS*4+2];
extern uint16_t newLinuxCommands;
extern float LinuxCommands[CMDNUM_FROM_FLOATS];

int16_t adca0result;
int16_t adca1result;
float adcina2_in_volts;
float adcina3_in_volts;
int32_t adca1calls=0;

float jystk_h[NSIZE+1] = {0};
float yk_h = 0;
float jystk_v[NSIZE+1] = {0};
float yk_v = 0;

// b=fir1(21,75/500) 21th order FIR LPF
// Change #define NSIZE to 21 at the top of this C file
float b[22]={   -2.3890045153263611e-03,
    -3.3150057635348224e-03,
    -4.6136191242627002e-03,
    -4.1659855521681268e-03,
    1.4477422497795286e-03,
    1.5489414225159667e-02,
    3.9247886844071371e-02,
    7.0723964095458614e-02,
    1.0453473887246176e-01,
    1.3325672639406205e-01,
    1.4978314227429904e-01,
    1.4978314227429904e-01,
    1.3325672639406205e-01,
    1.0453473887246176e-01,
    7.0723964095458614e-02,
    3.9247886844071371e-02,
    1.5489414225159667e-02,
    1.4477422497795286e-03,
    -4.1659855521681268e-03,
    -4.6136191242627002e-03,
    -3.3150057635348224e-03,
    -2.3890045153263611e-03};

// Needed global Variables
float accelx_offset = 0;
float accely_offset = 0;
float accelz_offset = 0;
float gyrox_offset = 0;
float gyroy_offset = 0;
float gyroz_offset = 0;
float accelzBalancePoint = -.76;
int16 IMU_data[9];
uint16_t temp=0;
int16_t doneCal = 0;
float tilt_value = 0;
float tilt_array[4] = {0, 0, 0, 0};
float gyro_value = 0;
float gyro_array[4] = {0, 0, 0, 0};
float LeftWheel = 0;
float RightWheel = 0;
float LeftWheelArray[4] = {0,0,0,0};
float RightWheelArray[4] = {0,0,0,0};

// Kalman Filter vars
float T = 0.001; //sample rate, 1ms
float Q = 0.01; // made global to enable changing in runtime
float R = 25000;//50000;
float kalman_tilt = 0;
float kalman_P = 22.365;
int16_t AverageIndex = -1;
float pred_P = 0;
float kalman_K = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;

float velLeft, velLeft_prev=0;
float velRight, velRight_prev=0;

float LeftWheel_prev=0;
float RightWheel_prev=0;

float gyro_value_prev=0;
float gyrorate_dot, gyrorate_dot_prev=0;

float ubal;

float WhlDiff, WhlDiff_prev=0;
float vel_WhlDiff, vel_WhlDiff_prev=0;

float turnref, turnref_prev=0;
float turnrate, turnrate_prev=0;
float errorDiff, errorDiff_prev;
float intDiff, intDiff_prev=0;

float eSpeed, eSpeed_prev=0;
float Segbot_refSpeed;
float I_eSpeed, I_eSpeed_prev=0;
float ForwardBackwardCommand;
float KpSpeed=0.35, KiSpeed=1.5;

float speed;

float ub_Left, ub_Left_prev=0;
float ub_Right, ub_Right_prev=0;

float uL1_Left, uL1_Left_prev=0;
float uL1_Right, uL1_Right_prev=0;

// State
float state[7], state_prev[7]={0};

//// LQR gain with Q = 20*eye(7) and R = eye(2)
//float K_Left[7] = {2.8591,    2.7709,  -20.1854,  -25.7847,   -4.5884,   -4.8893,   -2.0922};
//float K_Right[7] = {2.8591,   -2.7709,  -20.1854,  -25.7847,   -4.5884,    4.8893,    2.0922};

//// LQR gain with Q = 30*eye(7) and R = eye(2)
//float K_Left[7] = {3.4215,    3.2780,  -20.5360,  -27.3243,   -5.1258,   -5.7568,   -2.5801};
//float K_Right[7] = {3.4215,   -3.2780,  -20.5360,  -27.3243,   -5.1258,    5.7568,    2.5801};

// LQR gain with Q = 20*eye(7) + diag([180 0 180 0 0 0 0]) and R = eye(2)
float K_Left[7] = {8.9655,    2.7709,  -25.8695,  -32.2835,   -5.3964,   -4.8893,   -2.0922};
float K_Right[7] = {8.9655,   -2.7709,  -25.8695,  -32.2835,   -5.3964,    4.8893,    2.0922};

// L1
float state_hat[7], state_hat_prev[7]={0};
float state_hat_dot[7], state_hat_dot_prev[7]={0};
float sigma_m_hat[2], sigma_m_hat_prev[2]={0};
float sigma_um_hat[5], sigma_um_hat_prev[5]={0};

// LTI system
float A[7][7] = { {0,         0,   -1.0000,         0,         0,         0,         0},
                  {0,         0,         0,         0,         0,   -1.0000,         0},
                  {0,         0,  -20.7373,   -4.5423,         0,         0,         0},
                  {0,         0,         0,         0,    1.0000,         0,         0},
                  {0,         0,  189.7571,   84.2654,         0,         0,         0},
                  {0,         0,         0,         0,         0,         0,    1.0000},
                  {0,         0,         0,         0,         0,         0,  -23.7928}
};

float Bm[7][2] = { {0,         0},
                   {0,         0},
                   {1.0731,    1.0731},
                   {0,         0},
                   {-9.8190,   -9.8190},
                   {0,         0},
                   {-14.2331,   14.2331}
};

float Bum[7][5] = { {1.0000,         0,         0,         0,         0},
                    {     0,    1.0000,         0,         0,         0},
                    {     0,         0,    9.8190,         0,         0},
                    {     0,         0,         0,    1.0000,         0},
                    {     0,         0,    1.0731,         0,         0},
                    {     0,         0,         0,         0,    1.0000},
                    {     0,         0,         0,         0,         0}
};


float adaptation_gain[7][7] = { {         0,         0,   -1.3475,         0,   12.3302,         0,    8.6079},
                                {         0,         0,   -1.3475,         0,   12.3302,         0,   -8.6079},
                                { -245.0333,         0,         0,         0,         0,         0,         0},
                                {         0, -245.0333,         0,         0,         0,         0,         0},
                                {         0,         0,  -24.6605,         0,   -2.6950,         0,    0.0000},
                                {         0,         0,         0, -245.0333,         0,         0,         0},
                                {         0,         0,         0,         0,         0, -245.0333,         0}
};

float Ae[7][7] = { {-10,         0,         0,         0,         0,         0,         0},
                     {0,       -10,         0,         0,         0,         0,         0},
                     {0,         0,       -10,         0,         0,         0,         0},
                     {0,         0,         0,       -10,         0,         0,         0},
                     {0,         0,         0,         0,       -10,         0,         0},
                     {0,         0,         0,         0,         0,       -10,         0},
                     {0,         0,         0,         0,         0,         0,       -10}
};

void main(void)
{
    // PLL, WatchDog, enable Peripheral Clocks
    // This example function is found in the F2837xD_SysCtrl.c file.
    InitSysCtrl();

    InitGpio();

    // 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;

    // 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(94, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(94, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCCLEAR.bit.GPIO94 = 1;

    // LED3
    GPIO_SetupPinMux(95, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(95, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCCLEAR.bit.GPIO95 = 1;

    // LED4
    GPIO_SetupPinMux(97, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(97, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDCLEAR.bit.GPIO97 = 1;

    // LED5
    GPIO_SetupPinMux(111, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(111, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDCLEAR.bit.GPIO111 = 1;

    // LED6
    GPIO_SetupPinMux(130, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(130, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO130 = 1;

    // LED7
    GPIO_SetupPinMux(131, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(131, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO131 = 1;

    // LED8
    GPIO_SetupPinMux(25, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(25, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO25 = 1;

    // LED9
    GPIO_SetupPinMux(26, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(26, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO26 = 1;

    // LED10
    GPIO_SetupPinMux(27, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(27, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPACLEAR.bit.GPIO27 = 1;

    // LED11
    GPIO_SetupPinMux(60, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(60, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBCLEAR.bit.GPIO60 = 1;

    // LED12
    GPIO_SetupPinMux(61, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(61, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBCLEAR.bit.GPIO61 = 1;

    // LED13
    GPIO_SetupPinMux(157, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(157, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO157 = 1;

    // LED14
    GPIO_SetupPinMux(158, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(158, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO158 = 1;

    // LED15
    GPIO_SetupPinMux(159, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(159, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPECLEAR.bit.GPIO159 = 1;

    // LED16
    GPIO_SetupPinMux(160, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(160, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPFCLEAR.bit.GPIO160 = 1;

    //WIZNET Reset
    GPIO_SetupPinMux(0, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(0, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO0 = 1;

    //ESP8266 Reset
    GPIO_SetupPinMux(1, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(1, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO1 = 1;

    //SPIRAM  CS  Chip Select
    GPIO_SetupPinMux(19, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(19, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO19 = 1;

    //DRV8874 #1 DIR  Direction
    GPIO_SetupPinMux(29, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(29, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO29 = 1;

    //DRV8874 #2 DIR  Direction
    GPIO_SetupPinMux(32, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(32, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPBSET.bit.GPIO32 = 1;

    //DAN28027  CS  Chip Select
    GPIO_SetupPinMux(9, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(9, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPASET.bit.GPIO9 = 1;

    //MPU9250  CS  Chip Select
    GPIO_SetupPinMux(66, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(66, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPCSET.bit.GPIO66 = 1;

    //WIZNET  CS  Chip Select
    GPIO_SetupPinMux(125, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(125, GPIO_OUTPUT, GPIO_PUSHPULL);
    GpioDataRegs.GPDSET.bit.GPIO125 = 1;

    //PushButton 1
    GPIO_SetupPinMux(4, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(4, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 2
    GPIO_SetupPinMux(5, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(5, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 3
    GPIO_SetupPinMux(6, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(6, GPIO_INPUT, GPIO_PULLUP);

    //PushButton 4
    GPIO_SetupPinMux(7, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(7, GPIO_INPUT, GPIO_PULLUP);

    //Joy Stick Pushbutton
    GPIO_SetupPinMux(8, GPIO_MUX_CPU1, 0);
    GPIO_SetupPinOptions(8, GPIO_INPUT, GPIO_PULLUP);

    // ----- code for CAN start here -----
    //GPIO17 - CANRXB
    GPIO_SetupPinMux(17, GPIO_MUX_CPU1, 2);
    GPIO_SetupPinOptions(17, GPIO_INPUT, GPIO_ASYNC);

    //GPIO12 - CANTXB
    GPIO_SetupPinMux(12, GPIO_MUX_CPU1, 2);
    GPIO_SetupPinOptions(12, GPIO_OUTPUT, GPIO_PUSHPULL);
    // ----- code for CAN end here -----



    // ----- code for CAN start here -----
    // Initialize the CAN controller
    InitCANB();

    // Set up the CAN bus bit rate to 1000 kbps
    setCANBitRate(200000000, 1000000);

    // Enables Interrupt line 0, Error & Status Change interrupts in CAN_CTL register.
    CanbRegs.CAN_CTL.bit.IE0= 1;
    CanbRegs.CAN_CTL.bit.EIE= 1;
    // ----- code for CAN end here -----

    // Clear all interrupts and initialize PIE vector table:
    // Disable CPU interrupts
    DINT;

    // Initialize the PIE control registers to their default state.
    // The default state is all PIE interrupts disabled and flags
    // are cleared.
    // This function is found in the F2837xD_PieCtrl.c file.
    InitPieCtrl();

    // Disable CPU interrupts and clear all CPU interrupt flags:
    IER = 0x0000;
    IFR = 0x0000;

    // Initialize the PIE vector table with pointers to the shell Interrupt
    // Service Routines (ISR).
    // This will populate the entire table, even if the interrupt
    // is not used in this example.  This is useful for debug purposes.
    // The shell ISR routines are found in F2837xD_DefaultIsr.c.
    // This function is found in F2837xD_PieVect.c.
    InitPieVectTable();

    // Interrupts that are used in this example are re-mapped to
    // ISR functions found within this project
    EALLOW;  // This is needed to write to EALLOW protected registers
    PieVectTable.TIMER0_INT = &cpu_timer0_isr;
    PieVectTable.TIMER1_INT = &cpu_timer1_isr;
    PieVectTable.TIMER2_INT = &cpu_timer2_isr;
    PieVectTable.SCIA_RX_INT = &RXAINT_recv_ready;
    PieVectTable.SCIB_RX_INT = &RXBINT_recv_ready;
    PieVectTable.SCIC_RX_INT = &RXCINT_recv_ready;
    PieVectTable.SCID_RX_INT = &RXDINT_recv_ready;
    PieVectTable.SCIA_TX_INT = &TXAINT_data_sent;
    PieVectTable.SCIB_TX_INT = &TXBINT_data_sent;
    PieVectTable.SCIC_TX_INT = &TXCINT_data_sent;
    PieVectTable.SCID_TX_INT = &TXDINT_data_sent;
    PieVectTable.SPIB_RX_INT = &SPIB_isr;

    PieVectTable.EMIF_ERROR_INT = &SWI_isr;

    PieVectTable.ADCA1_INT = &ADCA_ISR;
    // ----- code for CAN start here -----
    PieVectTable.CANB0_INT = &can_isr;
    // ----- code for CAN end here -----
    EDIS;    // This is needed to disable write to EALLOW protected registers


    // Initialize the CpuTimers Device Peripheral. This function can be
    // found in F2837xD_CpuTimers.c
    InitCpuTimers();

    // Configure CPU-Timer 0, 1, and 2 to interrupt every given period:
    // 200MHz CPU Freq,                       Period (in uSeconds)
    ConfigCpuTimer(&CpuTimer0, LAUNCHPAD_CPU_FREQUENCY, 1000);
    ConfigCpuTimer(&CpuTimer1, LAUNCHPAD_CPU_FREQUENCY, 4000);
    ConfigCpuTimer(&CpuTimer2, LAUNCHPAD_CPU_FREQUENCY, 40000);

    // Enable CpuTimer Interrupt bit TIE
    CpuTimer0Regs.TCR.all = 0x4000;
    CpuTimer1Regs.TCR.all = 0x4000;
    CpuTimer2Regs.TCR.all = 0x4000;

    init_serialSCIA(&SerialA,115200);

    EALLOW;
    EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
    EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
    EPwm5Regs.ETSEL.bit.SOCASEL = 2; // Select Event when counter equal to PRD
    EPwm5Regs.ETPS.bit.SOCAPRD = 1; // Generate pulse on 1st event ( pulse  is the same as  trigger )
    EPwm5Regs.TBCTR = 0x0; // Clear counter
    EPwm5Regs.TBPHS.bit.TBPHS = 0x0000; // Phase is 0
    EPwm5Regs.TBCTL.bit.PHSEN = 0; // Disable phase loading
    EPwm5Regs.TBCTL.bit.CLKDIV = 0; // divide by 1 50Mhz Clock
    EPwm5Regs.TBPRD = 50000; // Set Period to 1ms sample. Input clock is 50MHz.
    // Notice here that we are not setting CMPA or CMPB because we are not using the PWM signal
    EPwm5Regs.ETSEL.bit.SOCAEN = 1; //enable SOCA
    EPwm5Regs.TBCTL.bit.CTRMODE = 0; //unfreeze, and enter up count mode
    EDIS;

    EALLOW;
    //write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcbRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdccRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcdRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4
    AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    AdcSetMode(ADC_ADCB, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    AdcSetMode(ADC_ADCC, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    AdcSetMode(ADC_ADCD, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    //Set pulse positions to late
    AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcbRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdccRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    AdcdRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    //power up the ADCs
    AdcaRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcbRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdccRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    AdcdRegs.ADCCTL1.bit.ADCPWDNZ = 1;
    //delay for 1ms to allow ADC time to power up
    DELAY_US(1000);

    //Select the channels to convert and end of conversion flag
    //Many statements commented out, To be used when using ADCA or ADCB

    //ADCA
    AdcaRegs.ADCSOC0CTL.bit.CHSEL = 2; //SOC0 will convert Channel you choose Does not have to be A0
    AdcaRegs.ADCSOC0CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC0CTL.bit.TRIGSEL = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC0
    AdcaRegs.ADCSOC1CTL.bit.CHSEL = 3; //SOC1 will convert Channel you choose Does not have to be A1
    AdcaRegs.ADCSOC1CTL.bit.ACQPS = 99; //sample window is acqps + 1 SYSCLK cycles = 500ns
    AdcaRegs.ADCSOC1CTL.bit.TRIGSEL = 13;// EPWM5 ADCSOCA or another trigger you choose will trigger SOC1
    AdcaRegs.ADCINTSEL1N2.bit.INT1SEL = 1; //set to last SOC that is converted and it will set INT1 flag ADCA1
    AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
    EDIS;

    // Enable DACA and DACB outputs
    EALLOW;
    DacaRegs.DACOUTEN.bit.DACOUTEN = 1; //enable dacA output-->uses ADCINA0
    DacaRegs.DACCTL.bit.LOADMODE = 0; //load on next sysclk
    DacaRegs.DACCTL.bit.DACREFSEL = 1; //use ADC VREF as reference voltage

    DacbRegs.DACOUTEN.bit.DACOUTEN = 1; //enable dacB output-->uses ADCINA1
    DacbRegs.DACCTL.bit.LOADMODE = 0; //load on next sysclk
    DacbRegs.DACCTL.bit.DACREFSEL = 1; //use ADC VREF as reference voltage
    EDIS;

    setupSpib();

    // Enable CPU int1 which is connected to CPU-Timer 0, CPU int13
    // which is connected to CPU-Timer 1, and CPU int 14, which is connected
    // to CPU-Timer 2:  int 12 is for the SWI.  
    IER |= M_INT1;
    IER |= M_INT6;
    IER |= M_INT8;  // SCIC SCID
    IER |= M_INT9;  // SCIA  CANB
    IER |= M_INT12;
    IER |= M_INT13;
    IER |= M_INT14;

    // Enable TINT0 in the PIE: Group 1 interrupt 7
    PieCtrlRegs.PIEIER1.bit.INTx7 = 1;
    // Enable SWI in the PIE: Group 12 interrupt 9
    PieCtrlRegs.PIEIER12.bit.INTx9 = 1;
    PieCtrlRegs.PIEIER6.bit.INTx3 = 1;  //SPiB
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1; // Enable ADCA1 in the PIE: Group 1 interrupt 1
    // ----- code for CAN start here -----
    // Enable CANB in the PIE: Group 9 interrupt 7
    PieCtrlRegs.PIEIER9.bit.INTx7 = 1;
    // ----- code for CAN end here -----

    // ----- code for CAN start here -----
    // Enable the CAN interrupt signal
    CanbRegs.CAN_GLB_INT_EN.bit.GLBINT0_EN = 1;
    // ----- code for CAN end here -----

    init_serialSCIC(&SerialC,115200);
    init_serialSCID(&SerialD,115200);

    init_eQEPs();

    // EPWM2A EPWM2B which drive DC motors
    EPwm2Regs.TBCTL.bit.CTRMODE=0;
    EPwm2Regs.TBCTL.bit.FREE_SOFT=2;
    EPwm2Regs.TBCTL.bit.PHSEN=0;
    EPwm2Regs.TBCTL.bit.CLKDIV=0;

    EPwm2Regs.TBCTR=0;

    EPwm2Regs.TBPRD=2500;

    EPwm2Regs.CMPA.bit.CMPA=0;
    EPwm2Regs.CMPB.bit.CMPB=0;

    EPwm2Regs.AQCTLA.bit.CAU=1;
    EPwm2Regs.AQCTLA.bit.ZRO=2;

    EPwm2Regs.AQCTLB.bit.CBU=1;
    EPwm2Regs.AQCTLB.bit.ZRO=2;

    EPwm2Regs.TBPHS.bit.TBPHS=0;

    GPIO_SetupPinMux(2, GPIO_MUX_CPU1, 1); //GPIO PinName, CPU, Mux Index
    GPIO_SetupPinMux(3, GPIO_MUX_CPU1, 1); //GPIO PinName, CPU, Mux Index

    // Enable global Interrupts and higher priority real-time debug events
    EINT;  // Enable Global interrupt INTM
    ERTM;  // Enable Global realtime interrupt DBGM
    // ----- code for CAN start here -----

    //    // Transmit Message
    //    // Initialize the transmit message object used for sending CAN messages.
    //    // Message Object Parameters:
    //    //      Message Object ID Number: 0
    //    //      Message Identifier: 0x1
    //    //      Message Frame: Standard
    //    //      Message Type: Transmit
    //    //      Message ID Mask: 0x0
    //    //      Message Object Flags: Transmit Interrupt
    //    //      Message Data Length: 4 Bytes
    //    //
    //    CANsetupMessageObject(CANB_BASE, TX_MSG_OBJ_ID, 0x1, CAN_MSG_FRAME_STD,
    //                           CAN_MSG_OBJ_TYPE_TX, 0, CAN_MSG_OBJ_TX_INT_ENABLE,
    //                           TX_MSG_DATA_LENGTH);

    // Measured Distance from 1
    // Initialize the receive message object 1 used for receiving CAN messages.
    // Message Object Parameters:
    //      Message Object ID Number: 1
    //      Message Identifier: 0x060b0101
    //      Message Frame: Standard
    //      Message Type: Receive
    //      Message ID Mask: 0x0
    //      Message Object Flags: Receive Interrupt
    //      Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
    //      for a Receive mailbox)
    //
    CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_1, 0x060b0101, CAN_MSG_FRAME_EXT,
                          CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
                          RX_MSG_DATA_LENGTH);

    // Measured Distance from 2
    // Initialize the receive message object 2 used for receiving CAN messages.
    // Message Object Parameters:
    //      Message Object ID Number: 2
    //      Message Identifier: 0x060b0102
    //      Message Frame: Standard
    //      Message Type: Receive
    //      Message ID Mask: 0x0
    //      Message Object Flags: Receive Interrupt
    //      Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
    //      for a Receive mailbox)
    //

    CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_2, 0x060b0103, CAN_MSG_FRAME_EXT,
                          CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
                          RX_MSG_DATA_LENGTH);

    // Measurement Quality from 1
    // Initialize the receive message object 2 used for receiving CAN messages.
    // Message Object Parameters:
    //      Message Object ID Number: 3
    //      Message Identifier: 0x060b0201
    //      Message Frame: Standard
    //      Message Type: Receive
    //      Message ID Mask: 0x0
    //      Message Object Flags: Receive Interrupt
    //      Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
    //      for a Receive mailbox)
    //

    CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_3, 0x060b0201, CAN_MSG_FRAME_EXT,
                          CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
                          RX_MSG_DATA_LENGTH);

    // Measurement Quality from 2
    // Initialize the receive message object 2 used for receiving CAN messages.
    // Message Object Parameters:
    //      Message Object ID Number: 4
    //      Message Identifier: 0x060b0202
    //      Message Frame: Standard
    //      Message Type: Receive
    //      Message ID Mask: 0x0
    //      Message Object Flags: Receive Interrupt
    //      Message Data Length: 8 Bytes (Note that DLC field is a "don't care"
    //      for a Receive mailbox)
    //

    CANsetupMessageObject(CANB_BASE, RX_MSG_OBJ_ID_4, 0x060b0203, CAN_MSG_FRAME_EXT,
                          CAN_MSG_OBJ_TYPE_RX, 0, CAN_MSG_OBJ_RX_INT_ENABLE,
                          RX_MSG_DATA_LENGTH);

    //
    // Start CAN module operations
    //
    CanbRegs.CAN_CTL.bit.Init = 0;
    CanbRegs.CAN_CTL.bit.CCE = 0;

    //    // Initialize the transmit message object data buffer to be sent
    //    txMsgData[0] = 0x12;
    //    txMsgData[1] = 0x34;
    //    txMsgData[2] = 0x56;
    //    txMsgData[3] = 0x78;


    //    // Loop Forever - A message will be sent once per second.
    //    for(;;)
    //    {
    //
    //        CANsendMessage(CANB_BASE, TX_MSG_OBJ_ID, TX_MSG_DATA_LENGTH, txMsgData);
    //        txMsgCount++;
    //        DEVICE_DELAY_US(1000000);
    //    }


    // ----- code for CAN end here -----

    
    // IDLE loop. Just sit and loop forever (optional):
    while(1)
    {
        if (UARTPrint == 1 ) {
            //serial_printf(&SerialA,"Num Timer2:%ld Num SerialRX: %ld\r\n",CpuTimer2.InterruptCount,numRXA);

//          serial_printf(&SerialA,"a_x: %.3f a_y: %.3f a_z: %.3f  gyro_x: %.3f gyro_y: %.3f gyro_z: %.3f\r\n",accelx,accely,accelz,gyrox,gyroy,gyroz);
//          serial_printf(&SerialA,"D1: %ld D2: %ld",dis_1,dis_3);
//            serial_printf(&SerialA,"St1: %ld St2: %ld\n\r",measure_status_1,measure_status_3);

//            serial_printf(&SerialA,"LeftWheel: %.3f RightWheel: %.3f PosLeft: %.3f PosRight: %.3f\n\r",LeftWheel,RightWheel,PosLeft,PosRight);

//            serial_printf(&SerialA,"vLeft: %.3f vRight: %.3f\n\r",vLeft,vRight);
//            serial_printf(&SerialA,"Joystick x voltage value: %.3f, Joystick y voltage value: %.3f accel_z: %.3f gyro_x: %.3f Left motor angle: %.3f Right motor angle: %.3f\n\r", yk_h, yk_v,accelz,gyrox,LeftWheel,RightWheel);
            serial_printf(&SerialA,"tilt_value: %.3f gyro_value: %.3f Left motor angle: %.3f Right motor angle: %.3f\n\r", tilt_value,gyro_value,LeftWheel,RightWheel);

            UARTPrint = 0;
        }
    }
}


// SWI_isr,  Using this interrupt as a Software started interrupt
__interrupt void SWI_isr(void) {

    // These three lines of code allow SWI_isr, to be interrupted by other interrupt functions
    // making it lower priority than all other Hardware interrupts.
    PieCtrlRegs.PIEACK.all = PIEACK_GROUP12;
    asm("       NOP");                    // Wait one cycle
    EINT;                                 // Clear INTM to enable interrupts



    // Insert SWI ISR Code here.......
    velLeft = 0.6 * velLeft_prev + 100 * (LeftWheel - LeftWheel_prev);
    velRight = 0.6 * velRight_prev + 100 * (RightWheel - RightWheel_prev);

//    gyrorate_dot = 0.6 * gyrorate_dot_prev + 100 * (gyro_value - gyro_value_prev);

//    ubal = 60 * tilt_value + 4.5 * gyro_value + 1.1 * (velLeft + velRight)/2.0 + 0.1 * gyrorate_dot;

    WhlDiff = RightWheel - LeftWheel;

    vel_WhlDiff = 0.333 * vel_WhlDiff_prev + 166.667 * (WhlDiff - WhlDiff_prev);

    turnref = turnref_prev + 0.004 * (turnrate_prev + turnrate)/2;

    errorDiff = turnref - (WHEEL_RADIUS/SEGBOT_WIDTH) * WhlDiff;

    intDiff = intDiff_prev + 0.004 * (errorDiff_prev + errorDiff)/2;

//    turn = Kp * errorDiff + Ki * intDiff - Kd * vel_WhlDiff;

//    if(fabs(turn) >= 3) intDiff = intDiff_prev;
//
//    if(turn >= 4) turn = 4;
//    if(turn <= -4) turn = -4;

    speed = WHEEL_RADIUS * (velLeft + velRight)/2;

    eSpeed = Segbot_refSpeed - speed;

    I_eSpeed = I_eSpeed_prev + 0.004 * (eSpeed_prev + eSpeed)/2;

//    ForwardBackwardCommand = KpSpeed * eSpeed + KiSpeed * I_eSpeed;
//
//    if(fabs(ForwardBackwardCommand) >= 3) I_eSpeed = I_eSpeed_prev;
//
//    if(ForwardBackwardCommand >= 4) turn = 4;
//    if(ForwardBackwardCommand <= -4) turn = -4;

//    float state[8] = {I_eSpeed, intDiff, I_speed, speed, tilt_value, gyro_value, (WHEEL_RADIUS/SEGBOT_WIDTH) * WhlDiff, (WHEEL_RADIUS/SEGBOT_WIDTH) * vel_WhlDiff};
    state[0] = I_eSpeed;
    state[1] = intDiff;
    state[2] = speed;
    state[3] = tilt_value;
    state[4] = gyro_value;
    state[5] = (WHEEL_RADIUS/SEGBOT_WIDTH) * WhlDiff;
    state[6] = (WHEEL_RADIUS/SEGBOT_WIDTH) * vel_WhlDiff;

    ub_Right = 0;
    for(int i=0;i<8;i++) {
        ub_Right -= K_Right[i]*state[i];
    }

    ub_Left = 0;
    for(int i=0;i<8;i++) {
        ub_Left -= K_Left[i]*state[i];
    }

    if(fabs(ub_Right) >= 10 || fabs(ub_Left) >= 10) {
        intDiff = 0.95 * intDiff_prev;
        I_eSpeed = 0.95 * I_eSpeed_prev;
    }

    L1_AC();

    uLeft = ub_Left + uL1_Left;
    uRight = ub_Right + uL1_Right;

    setEPWM2A(uRight);
    setEPWM2B(-uLeft);

    // Save the current values to the "prev" variables
    velLeft_prev = velLeft;
    velRight_prev = velRight;

    LeftWheel_prev = LeftWheel;
    RightWheel_prev = RightWheel;

//    gyro_value_prev = gyro_value;
//    gyrorate_dot_prev = gyrorate_dot;

    WhlDiff_prev = WhlDiff;
    vel_WhlDiff_prev = vel_WhlDiff;

    intDiff_prev = intDiff;
    errorDiff_prev = errorDiff;

    turnref_prev = turnref;
    turnrate_prev = turnrate;

    eSpeed_prev = eSpeed;
    I_eSpeed_prev = I_eSpeed;

    ub_Left_prev = ub_Left;
    ub_Right_prev = ub_Right;

    vector_assign(state, state_prev, 8);

    // Communication with LabVIEW
    phi = WHEEL_RADIUS/SEGBOT_WIDTH * (RightWheel - LeftWheel);
    theta_avg = 0.5 * (RightWheel + LeftWheel);
    theta_avg_dot = (theta_avg - theta_avg_prev)/0.004;
    x_dot = WHEEL_RADIUS * theta_avg_dot * cos(phi);
    y_dot = WHEEL_RADIUS * theta_avg_dot * sin(phi);
    x = x_prev + 0.004 * (x_dot + x_dot_prev)/2;
    y = y_prev + 0.004 * (y_dot+y_dot_prev)/2;

    theta_avg_prev = theta_avg;

    x_dot_prev = x_dot;
    y_dot_prev = y_dot;

    x_prev = x;
    y_prev = y;

    if (NewLVData == 1) {
        NewLVData = 0;
        Segbot_refSpeed = fromLVvalues[0];
        turnrate = fromLVvalues[1];
        printLV3 = fromLVvalues[2];
        printLV4 = fromLVvalues[3];
        printLV5 = fromLVvalues[4];
        printLV6 = fromLVvalues[5];
        printLV7 = fromLVvalues[6];
        printLV8 = fromLVvalues[7];
    }
...

This file has been truncated, please download it to see its full contents.

ME 461 LQR + L1 Adaptive Controller for Segbot