File content as of revision 0:68715e9af246:

#include "mbed.h"
#include "mbedWSEsbc.h"

#define PI 3.14159
#define LOG_FREQ 10.0
#define CTRL_FREQ 150.0


DigitalOut myled(LED1);

float wrapToPi(float ang){
    while(ang > PI){
        ang-= 2*PI;
    }
    while(ang < -PI){
        ang+=2*PI;
    }
    return ang;
}

int main()
{
    float log_timer = 0;  //initialize variable for log timer
    bool run_flag = false;
    bool run_ctrl = false;

    mbedWSEsbcInit(115200);  //Initialize the mbed WSE Project SBC
    wait(.2);   //delay at beginning for voltage settle purposes
    t.start();  // Set up timer
    
    //Pendulum Posiiton when code starts will be the reference position
    LS7366_write_DTR(1,0);    //zero encoder channel 1  
    LS7366_quad_mode_x4(1);    
    LS7366_reset_counter(1); 
    LS7366_write_DTR(2,0);    //zero encoder channel 1      
    LS7366_quad_mode_x4(2);
    LS7366_reset_counter(2);  
    
    pc.printf("Quanser Inverted Pendulum Control Program\r\n");

    t.reset(); // zero timer
    float sampT = t.read();
    float tstop = 60;   //initialize stop time
    float pwm = 0;      //initialize pwm variable
    float dt = 1/CTRL_FREQ; // set control loop sample time
    float mot_ang;
    float pend_ang;
    
    while(1) {

        if(pc.readable()) {
            char c = pc.getc();
            if(c == 'H' || c == 'h') {
                pc.printf("Command List...\r\n");
                pc.printf("r - run closed loop controller\r\n");
                pc.printf("h - display this prompt and exit\r\n");
                pc.printf("Enter Command\r\n");

            }
            if(c == 'R' || c == 'r') {
                pc.printf("Running Pendulum lift vertical to activate controller..\r\n",tstop);
                run_flag = true;
            }

            if(run_flag) {
                t.reset();
                log_timer = 0;
                while(1) {

                    //Emergency Stop check
                    if(pc.readable()) { //if any key is hit turn of motor and break while loop
                        mot_control(1, 0.0);
                        break;
                    }

                    // read encoder 1 and encoder 2
                    float enc1 = (float)LS7366_read_counter(1);
                    float enc2 = (float)LS7366_read_counter(2);
                    
                    //Convert Encoder readings to angles relative to pendulum
                    pend_ang = wrapToPi((2*PI/4096)*enc1 - PI);                  
                    mot_ang = -(2*PI/4096)*enc2;

                    
                    //Logic to exit loop if Arm moves too far
                    if(abs(mot_ang) > 90*(PI/180)) { 
                        pc.printf("\r\nPast Safety Limit\r\n");
                        run_ctrl = false;
                        break;
                    }
                    
                    //Logic to only run control if pendulum is near vertical
                    if(abs(pend_ang) < 20*(PI/180)){
                        run_ctrl = true;
                    }else{
                        run_ctrl = false;                        
                    }

                    if(run_ctrl) { //If controller is active
                    // Control Law goes here!!
                        pwm = 0.0;

                    }else{
                        pwm = 0.0;
                    }
                    
                    
                    // Saturate PWM command to send no more than maximum value
                    if(pwm>1.0) {
                        pwm =1.0;
                    }
                    if(pwm<-1.0) {
                        pwm = -1.0;
                    }

                    //Set the new output.
                    mot_control(1, pwm);


                    if((t.read()- log_timer) >= (1.0/LOG_FREQ)) { //If log sample time has passed since last write
                        pc.printf("%.2f, %.3f, %.3f, %.3f\r\n", t.read(),pend_ang,mot_ang,pwm); //write data to scren
                        log_timer = t.read();
                    }

                    led3=!led3;  //Flash Led to Indicate program is running
                    wait(dt);   //Wait for sample time of loop frequency
                }  // while t < tstop
                mot_control(1, 0.0); // Turn off Motor once test is finished
                run_flag = false;
                run_ctrl = false;
                pc.printf("\r\nEnter Command\r\n");
            } //if run flag
        } // if pc.readable
    }//while
}//main