•

Zac Kocinski

•

Xiang Zhang

Published December 20, 2024

Dancing Balancing Robot

Balancing robot that performs a dance choreography to Jingle Bell Rock, with 13 pre-programmed dance moves.

AdvancedFull instructions provided96

Things used in this project

Hardware components

Texas Instruments LAUNCHXL-F28379D C2000 Delfino LaunchPad

Servo motor

Software apps and online services

Texas Instruments Code Composer Studio

LabVIEW Community Edition

Hand tools and fabrication machines

Soldering iron (generic)

3D Printer (generic)

Story

Introduction

This build is a project for ME461: Computer Control of Mechanical Systems. For our project, we programmed the stock robot to perform a dance choreography to a song while balancing on its two rear wheels. The robot also featured a spring-loaded, servo-activated projectile launcher as an potential addition to the routine.

Choreography

We programmed the robot to perform 13 distinct "dance moves", as shown below. All moves were performed with the robot balancing on its rear wheels using measurement from the built-in gyroscope and Kalman filtering.

Complete list of robot dance moves

We then incorporated the dance moves into a detailed choreography. To get into the Christmas spirit, we chose the song Jingle Bell Rock for the choreography The dance routine uses a case structure to switch between dance moves; each move is implemented as a C function that modifies the turn rate and speed of the motors to satisfy its intended movement.

void spin(void){
    turnrate = 2;
    Segbot_refSpeed = 0;
}
void spin_1(void){
    turnrate = -2;
    Segbot_refSpeed = 0;
}
void fastspin(void){
    turnrate = 3;
    Segbot_refSpeed = 0;
}
void fastspin_1(void){
    turnrate = -3;
    Segbot_refSpeed = 0;
}
void stop_spin(void){
    turnrate = 0;
    Segbot_refSpeed = 0;
}
void forward(void){
    turnrate = 0;
    Segbot_refSpeed = 1.5;
}
void backward(void){
    turnrate = 0;
    Segbot_refSpeed = -1.5;
}
void fastforward(void){
    turnrate = 0;
    Segbot_refSpeed = 2.5;
}
void fastbackward(void){
    turnrate = 0;
    Segbot_refSpeed = -2.5;
}
void hookright_forward(void){
    turnrate = 1;
    Segbot_refSpeed = 1;
}
void hookright_backward(void){
    turnrate = 1;
    Segbot_refSpeed = -1;
}
void hookleft_forward(void){
    turnrate = -1;
    Segbot_refSpeed = 1;
}
void hookleft_backward(void){
    turnrate = -1;
    Segbot_refSpeed = -1;
}

Jingle Bell Rock is a 34 measure song and we decided to change dance moves every 2 measures, thus there are 17 cases. This required a change in dance routine, but led to a simpler implementation on the robot.

To play the song, we utilized the piezoelectric buzzer on our breakout board. In order to implement this, for each note, we sent a signal to the buzzer consisting of a square wave with a 50% duty cycle, with varying frequencies for different notes. These notes were hard-coded into our C program.

#define FUDGEFACTORNOTE 1
#define C4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/261.63))
#define D4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/293.66))
#define E4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/329.63))
#define F4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/349.23))
#define G4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/392.00))
#define A4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/440.00))
#define B4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/493.88))
#define C5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/523.25))
#define D5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/587.33))
#define E5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/659.25))
#define F5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/698.46))
#define G5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/783.99))
#define A5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/880.00))
#define B5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/987.77))
#define D4SHARPNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/311.13))
#define F4SHARPNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/369.99))
#define G4SHARPNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/415.3))
#define A4FLATNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/415.3))
#define C5SHARPNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/554.37))
#define A5FLATNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/830.61))

#define OFFNOTE 1

Design

We also added a spring-loaded projectile launcher to incorporate launching an object as part of the dance choreography. This was a design inspired by a marble launcher design previously designed by a team member for a class project for ME270: Design for Manufacturability:

Original projectile launcher design

This projectile launcher is a spring-loaded mechanism that utilizes a switch to store and release energy into the spring. The modified version, however, was intended to be servo-activated instead of hand-activated, larger in scale, and launch a 3-D printed "coin" with a Christmas tree on it.

Final projectile launcher design

Controls

To activate the robot, three buttons on the breakout board were used: one to begin the routine, one to stop the routine.

LabView was used to activate the servo for the projectile launcher on a specified key press. The provided LabVIEW code structure receives 8 floating point values from the C code to LabVIEW, and returns back 8 other floating point values. This essentially allows LabView to activate eight functions (or more, depending on how the values are communicated), however, not all of the outputs were used.

Originally, we had planned on using a PlayStation controller to activate all functions remotely. We had planned to use the deprecated LabView MakerHub library for the Xbox One controller, using DS4Windows to interface the PlayStation controller as an Xbox controller. While the library's sample programs showed early success, after many hours of work, we were unfortunately unsuccessful in obtaining input from the controller in our own LabView programs, as the lack of debugging help from navigating a deprecated resource proved difficult.

//#############################################################################
// FILE:   FinalProject_main.c
//
// TITLE:  Final Project
//#############################################################################

// 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 "JingleBellRock.h" // ZSK: Included JingleBellRock instead of the original song
#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

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

// ZSK: variables defined and brought in for the song
extern uint16_t jbr_array[JBR_LENGTH]; // ZSK: bring in the array that holds our song
int16_t song_index = 0; // ZSK: manipulatable counting variable for indexing
// ZSK: additional variables brought in to implement buttons to test song
int16_t ReadPushButtons(void); // ZSK: Prototyping push button function
uint16_t song_counter = 0; // ZSK: Iterative variable for our song
uint16_t button_value = 0; // ZSK: Holds the value for what buttons are being pressed

// ZHX predefinition for servo motor
void setEPWM8A_RCServo(float angle);
void setEPWM8B_RCServo(float angle);

// ZHX define variables for servo
int16_t updown=0;
float dancount3=90;

// ZHX predefiniton for labview communication
float printLV1 = 0;
float printLV2 = 0;
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];

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

//ZHX EX2 predefinition for functions
void setEPWM2A(float controleffort);
void setEPWM2B(float controleffort);

// Count variables
uint32_t numTimer0calls = 0;


uint32_t numTimer2calls = 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;

//JLS: Lab 7, exercise 5 -- initialize ForwardBackwardCommand for uRight & uLeft calculation
float ForwardBackwardCommand = 0;
float eSpeed = 0; //JLS: speed error
float eSpeed_1 = 0;

//JLS: speed gains for lab 7, exercise 5
float KpSpeed = 0.35;
float KiSpeed = 1.5;
float IK_eSpeed = 0;
float IK_eSpeed_1=0;

//JLS: difference between the left wheel's angle in radians & the right wheel's angle in radians
float WhlDiff = 0;
float WhlDiff_1 = 0; //JLS: old WhlDiff value
float vel_WhlDiff = 0; //JLS: velocity & old velocity (83)
float vel_WhlDiff_1 = 0;

float turnref = 0; //JLS: labeled as "turn angle" in the block diagram
float turnref_1 = 0; //EEC: Saved value of turnref for the previous increment

//JLS: turnref is not directly the angle the robot will turn to!
float errorDiff = 0; //JLS: errorDiff = turnref  - WhlDiff; integrated using trapezoidal rule
float errorDiff_1 = 0; // ZSK: this will be used to integrate using trapezoidal rule
float intDiff = 0; // ZSK: stores our trapezoidal integration
float intDiff_1 = 0; // ZSK: we will use this to protect against integral windup
float WhlSpeedAvg = 0; //JLS: wheel speed average

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=0.0;
float RightWheel=0.0;

float distanceL=0.0;
float distanceR=0.0;
// ZHX ex2 uLeft and uRight are two control variables
float uLeft=5.0;
float uRight=5.0;

float PosLeft_K=0.0;
float PosRight_K=0.0;
float PosLeft_K_1=0.0;
float PosRight_K_1=0.0;
float VLeftK=0.0;
float VRightK=0.0;

//float Kp=3;
//float Ki=25;
float Kturn=3;
float eturn=0.0;
float turn=0.0;
float Vref=0.0;

float ek_L=0.0;
float ek_1_L=0.0;
float Ik_L=0.0;
float Ik_1_L=0.0;

float ek_R=0.0;
float ek_1_R=0.0;
float Ik_R=0.0;
float Ik_1_R=0.0;


//ZHX ex6 define variables

float R_Wh=0.0593;
float W_R=0.173;
float theta_l=0.0;
float theta_r=0.0;
float theta_l_prev=0.0;
float theta_r_prev=0.0;
float theta_ave=0.0;
float theta_ave_dot=0.0;
float x_dot=0.0;
float y_dot=0.0;
float x_dot_prev=0.0;
float y_dot_prev=0.0;

//ZHX ex7 define variables

float Kp_right=0.001;
float Kp_front=0.0002;
float ref_right=200;
float ref_front=1400;
float distright=0.0;
float distfront=0.0;
float vel_Right=0.25;
float Vel_front=0.25;
float threshold_1=400;
float threshold_2=500;
float 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.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];


//ZHX ex1 paste from LAB4

float yk1=0;
float yk2=0;
float xk_1[22] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;
float xk_2[22] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0} ;

interrupt void ADCA_ISR (void);

int16_t adca0result=0;
int16_t adca1result=0;


int32_t ADCA1_COUNT=0;

//Here we are doing 21st-order low pass FIR filter with 75Hz cutoff frequency whatever.
// ZHX we type b=fir1(22,75/500) in the matlab

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

// ZHX ex2 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 SpibNumCalls = -1;
float pred_P = 0;
float kalman_K = 0;
int32_t timecount = 0;
int16_t calibration_state = 0;
int32_t calibration_count = 0;


float vel_Left = 0;
float vel_Left_1=0;
float vel_Right_1=0;
float LeftWheelPrev=0;
float RightWheelPrev=0;
float gyro_value_1 = 0;
float ubal = 0;

float gyrorate_dot=0;
float gyrorate_dot_1=0;
float K_1=-60;
float K_2=-4.5;
float K_3=-1.1;
float K_4=-0.1;

float Kp=3.0;
float Ki=20.0;
float Kd=0.08;

float turnrate = 0.0;
float turnrate_1 = 0.0;

float Segbot_refSpeed=0.0;

// ZSK: prototyping of the movement functions
void spin(void);
void spin_1(void);
void fastspin(void);
void fastspin_1(void);
void stop_spin(void);
void turn_CW(float input_angle);
void turn_CCW(float input_angle);
void forward(void);
void backward(void);
void fastforward(void);
void fastbackward(void);
void hookright_forward(void);
void hookright_backward(void);
void hookleft_forward(void);
void hookleft_backward(void);
// ZSK: create a state variable for spinning
uint16_t spin_state = 0;
float song_status=0;

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;
    // ----- code for CAN start here -----
    PieVectTable.CANB0_INT = &can_isr;
    //ZHX ex1 paste from lab4, tell processor to call defined function &ADCA_ISR

    PieVectTable.ADCA1_INT= &ADCA_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, 150000); // ZSK: CpuTimer1 will be used for the song, see notes for calculations
    ConfigCpuTimer(&CpuTimer2, LAUNCHPAD_CPU_FREQUENCY, 4000);

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

    // ZSK: Bring in the the following code is for the setup of EPWM9A
    // ZSK: EPWM9A will be used for the buzzer
    EPwm9Regs.TBCTL.bit.CTRMODE = 0; /* ZSK Make it into up-count mode */
    EPwm9Regs.TBCTL.bit.FREE_SOFT = 2; /* ZSK Set free soft into free run mode */
    EPwm9Regs.TBCTL.bit.PHSEN = 0; //JLS: disable phase loading (do not load time-base counter from TBPHS)
    EPwm9Regs.TBCTL.bit.CLKDIV = 1; /* ZSK Set clock division to divide by 2 */
    EPwm9Regs.TBCTR = 0; // JLS: starts timer at zero
    EPwm9Regs.TBPRD = 0; /* ZSK: Start the period at 0 so no sound plays */
    EPwm9Regs.AQCTLA.bit.CAU = 0; //JLS: when CMPA is reached, do nothing
    EPwm9Regs.AQCTLA.bit.PRD = 3; // ZSK: when TBCTR = TBPRD, toggle the EPWM9A output
    EPwm9Regs.TBPHS.bit.TBPHS = 0; // ZSK: Set the phase to 0, typical to include based on TI examples
    GPIO_SetupPinMux(16, GPIO_MUX_CPU1, 5); // ZSK: Set GPIO16 to a PWM pin (EPWM9A)

    // ZHX EX3 we want the RC servo carrier frequency be 50Hz 50*TBPRD=3125000,so TBPRD=62500
    EPwm8Regs.TBPRD=62500; // ZHX EX3 TBPRD is a 16-bit register,the largest number we can set is 2^16-1=65535

    EPwm8Regs.CMPA.bit.CMPA=5000;// ZHX EX3 the intialvalue of CMPA and CMPB is commanding the servo to 8% duty cycle 0.08*62500(TBPRD)=5000
    EPwm8Regs.CMPB.bit.CMPB=5000;

    EPwm8Regs.AQCTLA.bit.CAU=1; // EEC - Set output low when counter reaches CMPA value
    EPwm8Regs.AQCTLA.bit.ZRO=2; // EEC - Set output high when counter reaches zero
    EPwm8Regs.AQCTLB.bit.CBU=1; // EEC - Set output low when counter reaches CMPB value
    EPwm8Regs.AQCTLB.bit.ZRO=2; // EEC - Set output high when counter reaches zero

    EPwm8Regs.TBPHS.bit.TBPHS=0;

    GPIO_SetupPinMux(14,GPIO_MUX_CPU1,1);// EPWM8A is GPIO14
    GPIO_SetupPinMux(15,GPIO_MUX_CPU1,1);// EPWM8B is GPIO15

    // ZHX EX1 EPWM8A and 8B controls 2 rc servos.
    EPwm8Regs.TBCTL.bit.CLKDIV=4; // ZHX EX3 the frequency of EPWM8 is 50MHz 50000000/2^4=3125000
    EPwm8Regs.TBCTL.bit.PHSEN=0; // EEC - disable the phase loading which means do not load the TBCTR from the TBPHS(time base phase register)
    EPwm8Regs.TBCTL.bit.CTRMODE=0; // EEC - count up mode. CTRMODE takes 2 bits in TBCTL,down count mode is 1;up-down count mode is 2; freeze counter operation is 3
    EPwm8Regs.TBCTL.bit.FREE_SOFT=2; // EEC - free run so that the PWM continues when you set a break point in your code. FREE_SOFT takes 2 bits in TBCTL.
    EPwm8Regs.TBCTR=0; //EEC - Reset the counter to zero for consistency

    //ZHX ex1 paste from LAB4 using EPWM5 to trigger ADCA channel

    EALLOW;
    // ZHX EX1.1 we use EPWM5 as a timer to trigger ADCD conversion sequence(sample ADCIND0 and ADCIND1)
    EPwm5Regs.ETSEL.bit.SOCAEN = 0; // Disable SOC on A group
    EPwm5Regs.TBCTL.bit.CTRMODE = 3; // freeze counter
    EPwm5Regs.ETSEL.bit.SOCASEL = 2; // ZHX EX1.1 SOCASEL has 3 bits, 2 to binary is 010. It enable event time-base counter equal to period(TBCTR=TBPRD)
    EPwm5Regs.ETPS.bit.SOCAPRD = 1; // ZHX EX1.1 SOCAPRD has 2 bits. It generate pulse on 1st event
    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; // ZHX EX1.1 Sample period to 1ms,so sample frequency is 1000Hz and input clock is 50MHz, so the TBPRD=50M/1000=50000.
    //EPwm5Regs.TBPRD = 12500; // ZHX EX4 the sample rate of microphone is 0.25ms so TBPRD=50M/4000=12500Hz
    //EPwm5Regs.TBPRD = 5000; // ZHX EX4 For the band pass filter with sample rate of 10000Hz
    // 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; //ZHX EX1.1 Counter Mode(CTRMODE 2 bits) unfreeze, and enter up count mode
    EDIS;

    init_serialSCIA(&SerialA,115200);
    // ZHX EX2 following lines are copied from Lab 3 for intialize the pinmux for EPWM2. EPWM2A and 2B drive the robot's DC motors.EPWM2A controls right motor and EPWM2B controls left motor
    EPwm2Regs.TBCTL.bit.CLKDIV=0; // EEC - set the CLKDIV be 1. CLKDIV takes 3 bits in TBCTL rigister the smallest number we could set is 0 and the largest number is 7(111 in decimal)
    EPwm2Regs.TBCTL.bit.PHSEN=0; // EEC - disable the phase loading which means do not load the TBCTR from the TBPHS(time base phase register)
    EPwm2Regs.TBCTL.bit.CTRMODE=0; // EEC - count up mode. CTRMODE takes 2 bits in TBCTL,down count mode is 1;up-down count mode is 2; freeze counter operation is 3
    EPwm2Regs.TBCTL.bit.FREE_SOFT=2; // EEC - free run so that the PWM continues when you set a break point in your code. FREE_SOFT takes 2 bits in TBCTL.

    EPwm2Regs.TBCTR=0; //EEC - Reset the counter to zero for consistency

    EPwm2Regs.TBPRD=2500; //EEC - Sets the period to 2500 clock cycles

    EPwm2Regs.CMPA.bit.CMPA=0; // the duty cycle at beginning is 0% for the right motor. CMPA(counter compare a register) determine the duty cycle for the right motor.
    EPwm2Regs.CMPB.bit.CMPB=0; // the duty cycle at beginning is 0% for the left motor. CMPA(counter compare a register) determine the duty cycle for the left motor.

    EPwm2Regs.AQCTLA.bit.CAU=1; // EEC - Set output low when counter reaches CMPA value
    EPwm2Regs.AQCTLA.bit.ZRO=2; // EEC - Set output high when counter reaches zero
    // ZHX EX1 different with EPWM12A, EPWM2B had additional AQCTLB and CMPB.
    EPwm2Regs.AQCTLB.bit.CBU=1; // EEC - Set output low when counter reaches CMPB value
    EPwm2Regs.AQCTLB.bit.ZRO=2; // EEC - Set output high when counter reaches zero

    EPwm2Regs.TBPHS.bit.TBPHS=0;

    GPIO_SetupPinMux(2,GPIO_MUX_CPU1,1); // EPWM2A is GPIO2
    GPIO_SetupPinMux(3,GPIO_MUX_CPU1,1); // EPWM2B is GPIO3

    EALLOW;
    //write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4

    //ZHX ex1 paste from lab4
    EALLOW;
    //write configurations for all ADCs ADCA, ADCB, ADCC, ADCD
    AdcaRegs.ADCCTL2.bit.PRESCALE = 6; //set ADCCLK divider to /4

    AdcSetMode(ADC_ADCA, ADC_RESOLUTION_12BIT, ADC_SIGNALMODE_SINGLE); //read calibration settings
    //Set pulse positions to late
    AdcaRegs.ADCCTL1.bit.INTPULSEPOS = 1;
    //power up the ADCs
    AdcaRegs.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; //ZHX EX3 We have already used channel 0 and 1, so we use channel 2 hereSOC0 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; //ZHX EX3 SOC1 will convert Channel you choose Does not have to be A1, In this case we choose channel 3
    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; //ZHX EX3 set to last SOC that is converted and it will set INT1 flag ADCA1, In this case we use SOC0 and SOC1,so the last SOC is 1.
    AdcaRegs.ADCINTSEL1N2.bit.INT1E = 1; //enable INT1 flag
    AdcaRegs.ADCINTFLGCLR.bit.ADCINT1 = 1; //make sure INT1 flag is cleared
    EDIS;
    //ZHX ex1.3 called setupSPIB in main()
    setupSpib();
    init_eQEPs();

    // 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
    // ----- code for CAN start here -----
    // Enable CANB in the PIE: Group 9 interrupt 7
    PieCtrlRegs.PIEIER9.bit.INTx7 = 1;
    // ----- code for CAN end here -----

    //ZHX enable PE interrupt 1.1(ADCA1)
    PieCtrlRegs.PIEIER1.bit.INTx1 = 1;

    // ----- 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);
    // 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)
    {
        //serial_printf(&SerialA,"Value: %.3f",fromLVvalues[3]);
        if (UARTPrint == 1 ) {
            //serial_printf(&SerialA,"Num Timer2:%ld Num SerialRX: %ld\r\n",CpuTimer2.InterruptCount,numRXA);
            //            serial_printf(&SerialA,"a:%.3f,%.3f,%.3f  g:%.3f,%.3f,%.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);
            // ZHX ex1.7 Print the filtered value of both rotation potentiometers of the small joystick, the accelerometer z value, the gyro x value and both motor angle
            serial_printf(&SerialA,"x direction: %.3f, y direction: %.3f, accelerometer z: %.3f, gyro x: %.3f\r\n",yk2,yk1,accelz,gyrox);
            serial_printf(&SerialA,"LeftWheel: %.3f RightWheel: %.3f\r\n",LeftWheel,RightWheel);
            serial_printf(&SerialA,"Value: %.3f\r\n",fromLVvalues[3]);

            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
    //ZHX ex3 the transfer function is 125s/(s+125), and the discrete transfer function is100z-100)/(z-0.6)=Y(z)/V(z)
    vel_Right = 0.6*vel_Right_1 + 100*(RightWheel - RightWheelPrev);
    vel_Left = 0.6*vel_Left_1 + 100*(LeftWheel - LeftWheelPrev);

    gyrorate_dot=0.6*gyrorate_dot_1+100*(gyro_value-gyro_value_1);
    //ZHX ex3 the state feedback controller equation is u=-kx states are tile,gyro,average of left and right motor velocities and gyrorate_dot
    ubal= -K_1*tilt_value-K_2*gyro_value - K_3*(vel_Left+vel_Right)/2.0-K_4*gyrorate_dot;
    //ZHX ex3 ubal is controleffort for both motors
    //    uLeft=ubal/2;
    //    uRight=ubal/2;
    //JLS: variables for Lab 7, exercise 4
    turnref = turnref_1 + 0.5*0.004*(turnrate+turnrate_1);//EEC Calculate turnref using turnrate and Euler integration
    WhlDiff = LeftWheel-RightWheel;
    //EEC: calculates the wheel difference using discrete time filter
    vel_WhlDiff = 0.333*vel_WhlDiff_1 + 166.667*( WhlDiff - WhlDiff_1 );
    errorDiff = turnref - WhlDiff; // ZSK: error between the read angle and the input turning angle
    intDiff = intDiff_1 + 0.004 * (errorDiff + errorDiff_1)/2;
    turn = Kp*errorDiff + Ki*intDiff - Kd*vel_WhlDiff; // ZSK: calculate the PID control to give the turn command

    if(fabs(turn)>3){ // ZSK: if statement used to protect from integral windup
        intDiff = intDiff_1;
    }
    // ZHX ex4 saturate turn between -4 and 4
    if (turn>4){
        turn=4;
    }
    if (turn<-4){
        turn=-4;
    }

    //    uRight = ubal/2.0 - turn;
    //    uLeft = ubal/2.0 + turn;

    //JLS:calculate average wheel velocity WhlSpeedAvg & error speed eSpeed
    WhlSpeedAvg = ( vel_Left + vel_Right ) / 2.0;
    eSpeed = ( Segbot_refSpeed  - WhlSpeedAvg );
    IK_eSpeed = IK_eSpeed_1 + 0.004*(eSpeed + eSpeed_1)/2;
    ForwardBackwardCommand = KpSpeed*eSpeed + KiSpeed*IK_eSpeed;
    //ZHX ex5 saturate IK_eSpeed at its current value when absolutue value of ForwardBackwardCommand is larger than 3
    if(fabs(ForwardBackwardCommand) > 3){
        IK_eSpeed=IK_eSpeed_1;
    }
    // ZHx ex5 saturate ForwardBackwardCommand between -4 and 4.
    if (ForwardBackwardCommand>4){
        ForwardBackwardCommand=4;
    }
    if (ForwardBackwardCommand<-4){
        ForwardBackwardCommand=-4;
    }

    uRight = ubal/2.0 - turn - ForwardBackwardCommand;
    uLeft = ubal/2.0 + turn - ForwardBackwardCommand;


    setEPWM2A(uRight);
    setEPWM2B(-uLeft);

    // Saving old variables
    RightWheelPrev = RightWheel;
    LeftWheelPrev = LeftWheel;
    vel_Right_1 = vel_Right;
    vel_Left_1 = vel_Left;
    gyro_value_1 = gyro_value;
    gyrorate_dot_1 = gyrorate_dot;

    vel_WhlDiff_1 = vel_WhlDiff; //EEC: wheel difference for the previous increment
    WhlDiff_1 = WhlDiff;
    errorDiff_1 = errorDiff; // ZSK: set the past variable before computing the new one
    intDiff_1 = intDiff;

    turnref_1 = turnref;
    turnrate_1 = turnrate;

    IK_eSpeed_1 = IK_eSpeed;
    eSpeed_1 = eSpeed;
    //ZHX ex5 copy fron interrupt function from lab 6(kinematic equations + communication with labview)
    theta_l=LeftWheel;
    theta_r=RightWheel;
    bearing=R_Wh/W_R*(theta_r-theta_l);
    theta_ave=0.5*(theta_r+theta_l);
    theta_ave_dot=0.5*(theta_r - theta_r_prev + theta_l - theta_l_prev)/0.004;
    theta_r_prev=theta_r;
    theta_l_prev=theta_l;
    x_dot=R_Wh*theta_ave_dot*cos(bearing);
    y_dot=R_Wh*theta_ave_dot*sin(bearing);

    //ZHX ex6 Using trapezoidal rule
    x=x+0.5*0.004*(x_dot+x_dot_prev);
    y=y+0.5*0.004*(y_dot+y_dot_prev);
    x_dot_prev=x_dot;
    y_dot_prev=y_dot;

...

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

#define FUDGEFACTORNOTE 1
#define C4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/261.63))
#define D4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/293.66))
#define E4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/329.63))
#define F4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/349.23))
#define G4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/392.00))
#define A4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/440.00))
#define B4NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/493.88))
#define C5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/523.25))
#define D5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/587.33))
#define E5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/659.25))
#define F5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/698.46))
#define G5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/783.99))
#define A5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/880.00))
#define B5NOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/987.77))
#define D4SHARPNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/311.13))
#define F4SHARPNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/369.99))
#define G4SHARPNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/415.3))
#define A4FLATNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/415.3))
#define C5SHARPNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/554.37))
#define A5FLATNOTE ((uint16_t)(((FUDGEFACTORNOTE*50000000/2)/2)/830.61))


#define OFFNOTE 1

#define JBR_LENGTH 544 // ZSK: how many items there are in the array
uint16_t jbr_array[JBR_LENGTH] = {
C5NOTE, // ZSK: beginning of measure 1
OFFNOTE,
C5NOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
B4NOTE,
B4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE, // ZSK: beginning of measure 2
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
E4NOTE,
E4NOTE,
E4NOTE,
E4NOTE,
E4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
A4NOTE, // ZSK: beginning of measure 3
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
A4NOTE,
OFFNOTE,
OFFNOTE,
E4NOTE,
E4NOTE,
OFFNOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE, // ZSK: beginning of measure 4
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
A4NOTE,
OFFNOTE,
OFFNOTE,
F4NOTE,
F4NOTE,
F4NOTE,
F4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
D4NOTE, // ZSK: beginning of measure 5
OFFNOTE,
E4NOTE,
OFFNOTE,
F4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
G4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
D4NOTE, // ZSK: beginning of measure 6
OFFNOTE,
E4NOTE,
OFFNOTE,
F4NOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
G4NOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,// ZSK: beginning of measure 7
OFFNOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
A4FLATNOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
A4FLATNOTE,
OFFNOTE,
A4NOTE,
A4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,// ZSK: beginning of measure 8
A4NOTE,
OFFNOTE,
OFFNOTE,
D4SHARPNOTE,
OFFNOTE,
E4NOTE,
E4NOTE,
E4NOTE,
E4NOTE,
E4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE, // ZSK: beginning of measure 9 (same as 1)
OFFNOTE,
C5NOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
B4NOTE,
B4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE, // ZSK: beginning of measure 10 (same as 2)
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
E4NOTE,
E4NOTE,
E4NOTE,
E4NOTE,
E4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
A4NOTE, // ZSK: beginning of measure 11
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
E4NOTE,
E4NOTE,
E4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE, // ZSK: beginning of measure 12 (same as 4)
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
A4NOTE,
OFFNOTE,
OFFNOTE,
F4NOTE,
F4NOTE,
F4NOTE,
F4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
D4NOTE, // ZSK: beginning of measure 13 (same as 5)
OFFNOTE,
E4NOTE,
OFFNOTE,
F4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
G4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
D4NOTE, // ZSK: beginning of measure 14 (same as 6)
OFFNOTE,
E4NOTE,
OFFNOTE,
F4NOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
G4NOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,// ZSK: beginning of measure 15
OFFNOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
B4NOTE,
B4NOTE,
OFFNOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,// ZSK: beginning of measure 16
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
D5NOTE,// ZSK: beginning of measure 17
D5NOTE,
D5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
D5NOTE,// ZSK: beginning of measure 18
D5NOTE,
D5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
C5NOTE,// ZSK: beginning of measure 19
C5NOTE,
OFFNOTE,
OFFNOTE,
B4NOTE,
B4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,
A4NOTE,
OFFNOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
E4NOTE,// ZSK: beginning of measure 20
E4NOTE,
E4NOTE,
E4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
D5NOTE,// ZSK: beginning of measure 21
D5NOTE,
D5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
D5NOTE,// ZSK: beginning of measure 22
D5NOTE,
D5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,// ZSK: beginning of measure 23
OFFNOTE,
G4NOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
B4NOTE,// ZSK: beginning of measure 24
OFFNOTE,
A4NOTE,
A4NOTE,
OFFNOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE, // ZSK: beginning of measure 25 (same as 9)
OFFNOTE,
C5NOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
B4NOTE,
B4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE, // ZSK: beginning of measure 26 (same as 10)
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
E4NOTE,
E4NOTE,
E4NOTE,
E4NOTE,
E4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
A4NOTE, // ZSK: beginning of measure 27 (same as 11)
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
E4NOTE,
E4NOTE,
E4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,// ZSK: beginning of measure 28
A4NOTE,
A4NOTE,
A4NOTE,
A4NOTE,
A4NOTE,
A4NOTE,
A4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,// ZSK: beginning of measure 29
A4NOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
D5NOTE,
D5NOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
A4FLATNOTE,// ZSK: beginning of measure 30
A4FLATNOTE,
OFFNOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
D5NOTE,
D5NOTE,
D5NOTE,
D5NOTE,
D5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,// ZSK: beginning of measure 31
OFFNOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,// ZSK: beginning of measure 32 (same as 31)
OFFNOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
G4NOTE,
G4NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,// ZSK: beginning of measure 33
OFFNOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
A4NOTE,
OFFNOTE,
B4NOTE,
OFFNOTE,
C5NOTE,
OFFNOTE,
D5NOTE,
D5NOTE,
D5NOTE,
OFFNOTE,
C5NOTE,// ZSK: beginning of measure 34
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
C5NOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
OFFNOTE,
};

Dancing Balancing Robot

Things used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Introduction

Choreography

Design

Controls

Code

FinalProject_main.c

JingleBellRock.h

Credits

Ethan Cho

Zac Kocinski

Xiang Zhang

Comments

Embed the widget on your own site

Dancing Balancing Robot

Dancing Balancing Robot

Things used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Introduction

Choreography

Design

Controls

Code

FinalProject_main.c

JingleBellRock.h

Credits

Ethan Cho

Zac Kocinski

Xiang Zhang

Comments

Related channels and tags