Inverted Pendulum Cart (In Progress)

/*>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
MSP430F2272 Project Template Version 5.0 (April, 2019)
CCS					v8.1.0
MSP-CGT-18 			18.1.2.LTS

(ME 461)			Spring 2019
Written by: 		Steve Keres, Dan Block
Contributors: 		S. R. Platt
					Rick Rekoske
College of Engineering Control Systems Lab
University of Illinois at Urbana-Champaign
<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/

#include <msp430.h>
#include "msp430f2272.h"
#include "UART.h"
#define PI 3.1415926

// default global variables
unsigned int printCount         = 0;
unsigned int displayCount       = 0;
unsigned int printCompare       = 100;
unsigned char printNow          = 0;
// declare global variables
unsigned long enc1raw           = 0;
unsigned long enc1count         = 0;
unsigned char flagRX            = 0;
unsigned char flagTX            = 0;
unsigned long sendCount         = 0;
unsigned long recvCount         = 0;
unsigned long enc1countOld      = 0;
long enc1net                    = 0;
long enc1netOld                 = 0;
long enc1diff                   = 0;
float enc1rad                   = 0.0;
float enc1radOld                = 0.0;
float enc1vel                   = 0.0;
float enc1velOld                = 0.0;
float enc1ref                   = -5.0;
unsigned long refCount          = 0;
float u1                        = 0.0;
float kp                        = 300.0;//50.0;
float kd                        = 0.02;//0.01;
unsigned char flagInitializeEncoder = 1;
unsigned int refPeriod          = 400;

 void main(void) {
	WDTCTL = WDTPW | WDTHOLD;	// stop watchdog timer

    // Set up uC to run at approximately 16 MHz
    if(CALBC1_16MHZ == 0xFF || CALDCO_16MHZ == 0xFF) while(1);
    DCOCTL  				= CALDCO_16MHZ;
    BCSCTL1 				= CALBC1_16MHZ;

    /*>>>>>>>>>>>>>>>>>>>>>>> Register Initializations >>>>>>>>>>>>>>>>>>>>>>>*/

    // Port 1 config
    P1SEL 			   &= ~0x01;			// P1.1 as GPIO
    P1DIR    		   |=  0x01;            // P1.1 as Output

    // Port 3 Config
    P3SEL       |=  0x0E;               // P3.1 as UCB0SIMO, P3.2 as UCB0SOMI, P3.3 as UCB0CLK
    P3SEL       &= ~0x80;               // P3.7 as GPIO
    ADC10AE0    &= ~0x80;               // P3.7 as GPIO
    P3DIR       |=  0x80;               // P3.7 as DO
    P3OUT       &= ~0x80;               // Clear CS and hold low

    // Port 4 Config
    P4SEL |= 0x02;                      // P4.1 as Timer_B3.TB1
    P4DIR |= 0x02;                      // P4.1 as Timer_B3.TB1

    // Timer A config
    TACCTL0 			= CCIE;             // Enable interrupt
    TACCR0  			= 40000;            // 200 Hz, 1 ms
    TACTL   			= TASSEL_2 + MC_1 + ID_1;  // SMCLK, Up mode

    // Timer B Config
    TBCCTL1 = CLLD_2 + OUTMOD_7;        // TBCLx loads when TBR counts to 0, Reset/set
    TBCCR1 = 400;                       // 50% Duty cycle
    TBCCR0 = 800;                       // 20kHz PWM
    TBCTL = TBSSEL_2 + MC_1;            // SMCLK, Up mode

    // UCB0 SPI Config
    UCB0CTL1 = UCSSEL_2                 // SMCLK
             + UCSWRST;                 // Enabled (already enabled by default)
    UCB0CTL0 = UCCKPH                   // Phase; capture/change
             + UCCKPL                   // Polarity; inactive high
             + UCMSB                    // MSB first
             + UC7BIT*0                 // 8 bit character format
             + UCMST                    // Master mode
             + UCMODE_0                 // 3-wire SPI
             + UCSYNC;                  // Synchronous mode
    UCB0BR0  = 32;                       // 2MHz bit clock
    UCB0BR1  = 0;                       //
    UCB0CTL1 &= ~UCSWRST;               // Pull out of reset
    IE2      |= UCB0TXIE + UCB0RXIE;    // Enable interrupts
    IFG2     &= ~(UCB0TXIFG + UCB0RXIFG);// Clear flags

    /*<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<*/

    // enable global interrupt
    _BIS_SR(GIE);

    // initialize UART serial communication
    UARTinit(115200,1);

    while(1){
        if(printNow){
            displayCount++;
            if(displayCount >= 100){
                displayCount    = 0;
            }
//            UARTprintf("%2d %7.3f %6.3f\n\r", displayCount, enc1rad, enc1vel);
//            UARTprintf("%2d %7.3f %6.3f\n\r", displayCount, enc1rad, enc1ref);
            UARTprintf("%2d %7.3f %6.3f\n\r", displayCount, enc1rad, u1);
            printNow = 0;
        }
    }

}


// Timer A0 interrupt service routine
#pragma vector=TIMERA0_VECTOR
__interrupt void TimerA_ISR(void) {

    printCount++;
    if(printCount >= printCompare){
        // enters every 250 ms
        printCount  			= 0;
        P1OUT       	   	   ^= 0x01;
        printNow    			= 1;
    }

    // TimerA_ISR code here:
    if(flagInitializeEncoder == 1){                         // wait to obtain first encoder reading
        flagInitializeEncoder   += 1;
    } else if(flagInitializeEncoder == 2){
        flagInitializeEncoder   = 0;
        enc1countOld            = enc1count;                // zero enc1net
    } else{
        // manage encoder count wrap around
        // max speed measured: 710 counts in 5ms
        if(abs(enc1count - enc1countOld) > 800){
            if(enc1count > enc1countOld){                   // 0 -> 4095 (negative direction)
                enc1diff                = (enc1count - 4095) - enc1countOld;
                enc1net                 += enc1diff;
            } else{                                         // 4095 -> 0 (positive direction
                enc1diff                = (enc1count + 4095) - enc1countOld;
                enc1net                 += enc1diff;
            }
        } else{                                             // no wrap around
            enc1diff                = enc1count - enc1countOld;
            enc1net                 += enc1diff;
        }
    }
    // calculate radians and radians per second
    enc1rad                 = enc1net*0.0015343554;         // (2*PI/4095)
    enc1vel                 = enc1diff*0.30687108;          // (2*PI/4095)*(1/0.005)

    // closed-loop control
    // saturate control value to remain within 0% - 100%
    // assign u1
    u1                      = kp*(enc1ref - enc1rad) - kd*enc1vel;
    if(u1 >= 400.0){
        u1                      = 400.0;
    } else if(u1 <= -400.0){
        u1                      = -400.0;
    }
    if((enc1rad >= 10.0) || (enc1rad <= -10.0)){
        u1                      = 0.0;
//        TBCCR1                  = 400;
    }
    TBCCR1                  = 400 + (int)u1;

    // update Old variables
    enc1countOld            = enc1count;
    enc1netOld              = enc1net;
    enc1radOld              = enc1rad;
    enc1velOld              = enc1vel;

    // generate reference signal
    refCount++;
    if(refCount >= refPeriod){
        refCount                = 0;
        enc1ref                 = -enc1ref;
    }

    // initiate SPI to get encoder reading
    // CS is always low with only one encoder wired
    // on the next Timer_A loop, the encoder reading should be the most recent
    flagTX = 1;
    flagRX = 1;
    UCB0TXBUF = 0xA0;

}


// USCI Transmit ISR - called when TXBUF is empty
#pragma vector=USCIAB0TX_VECTOR
__interrupt void USCI0TX_ISR(void) {

    if((IFG2&UCA0TXIFG) && (IE2&UCA0TXIE)) { // USCI_A0 requested TX interrupt
        if(printfFlag) {
            if(currentIndex == txCount) {
                sendDone 			= 1;
                printfFlag 			= 0;
                IFG2 			   &= ~UCA0TXIFG;
            } else {
	            UCA0TXBUF 			= printBuff[currentIndex];
	            currentIndex++;
            }
        }

        IFG2 				   &= ~UCA0TXIFG;
    }

	if((IFG2&UCB0TXIFG) && (IE2&UCB0TXIE)) { // USCI_B0 requested TX interrupt
        if(flagTX){
            flagTX = 0;
            UCB0TXBUF = 0x0A;
            sendCount++;
        }
		IFG2 				   &= ~UCB0TXIFG;
	}

}


// USCI Receive ISR - called when shift register has been transferred to RXBUF
// Indicates completion of TX/RX operation
#pragma vector=USCIAB0RX_VECTOR
__interrupt void USCI0RX_ISR(void) {

	if((IFG2&UCA0RXIFG) && (IE2&UCA0RXIE)) { // USCI_A0 requested RX interrupt

		IFG2 				   &= ~UCA0RXIFG;
	}

	if((IFG2&UCB0RXIFG) && (IE2&UCB0RXIE)) { // USCI_B0 requested RX interrupt
	    if(flagRX){
            flagRX          = 0;
            enc1raw         = (((unsigned int)UCB0RXBUF)<<8);
        } else{
            enc1raw         = enc1raw + (unsigned int)UCB0RXBUF;
//            enc1countOld    = enc1count;
            enc1count       = (enc1raw>>3)&(0x0FFF);
            recvCount++;
        }
		IFG2 				   &= ~UCB0RXIFG;
	}

}