main.cpp

00001 #include "mbed.h"
00002 #include "mbedWSEsbc.h"
00003 
00004 #define PI 3.14159
00005 #define LOG_FREQ 10.0
00006 #define CTRL_FREQ 150.0
00007 
00008 
00009 DigitalOut myled(LED1);
00010 
00011 float wrapToPi(float ang){
00012     while(ang > PI){
00013         ang-= 2*PI;
00014     }
00015     while(ang < -PI){
00016         ang+=2*PI;
00017     }
00018     return ang;
00019 }
00020 
00021 int main()
00022 {
00023     float log_timer = 0;  //initialize variable for log timer
00024     bool run_flag = false;
00025     bool run_ctrl = false;
00026 
00027     mbedWSEsbcInit(115200);  //Initialize the mbed WSE Project SBC
00028     wait(.2);   //delay at beginning for voltage settle purposes
00029     t.start();  // Set up timer
00030     
00031     //Pendulum Posiiton when code starts will be the reference position
00032     LS7366_write_DTR(1,0);    //zero encoder channel 1  
00033     LS7366_quad_mode_x4(1);    
00034     LS7366_reset_counter(1); 
00035     LS7366_write_DTR(2,0);    //zero encoder channel 1      
00036     LS7366_quad_mode_x4(2);
00037     LS7366_reset_counter(2);  
00038     
00039     pc.printf("Quanser Inverted Pendulum Control Program\r\n");
00040 
00041     t.reset(); // zero timer
00042     float sampT = t.read();
00043     float tstop = 60;   //initialize stop time
00044     float pwm = 0;      //initialize pwm variable
00045     float dt = 1/CTRL_FREQ; // set control loop sample time
00046     float mot_ang;
00047     float pend_ang;
00048     
00049     while(1) {
00050 
00051         if(pc.readable()) {
00052             char c = pc.getc();
00053             if(c == 'H' || c == 'h') {
00054                 pc.printf("Command List...\r\n");
00055                 pc.printf("r - run closed loop controller\r\n");
00056                 pc.printf("h - display this prompt and exit\r\n");
00057                 pc.printf("Enter Command\r\n");
00058 
00059             }
00060             if(c == 'R' || c == 'r') {
00061                 pc.printf("Running Pendulum lift vertical to activate controller..\r\n",tstop);
00062                 run_flag = true;
00063             }
00064 
00065             if(run_flag) {
00066                 t.reset();
00067                 log_timer = 0;
00068                 while(1) {
00069 
00070                     //Emergency Stop check
00071                     if(pc.readable()) { //if any key is hit turn of motor and break while loop
00072                         mot_control(1, 0.0);
00073                         break;
00074                     }
00075 
00076                     // read encoder 1 and encoder 2
00077                     float enc1 = (float)LS7366_read_counter(1);
00078                     float enc2 = (float)LS7366_read_counter(2);
00079                     
00080                     //Convert Encoder readings to angles relative to pendulum
00081                     pend_ang = wrapToPi((2*PI/4096)*enc1 - PI);                  
00082                     mot_ang = -(2*PI/4096)*enc2;
00083 
00084                     
00085                     //Logic to exit loop if Arm moves too far
00086                     if(abs(mot_ang) > 90*(PI/180)) { 
00087                         pc.printf("\r\nPast Safety Limit\r\n");
00088                         run_ctrl = false;
00089                         break;
00090                     }
00091                     
00092                     //Logic to only run control if pendulum is near vertical
00093                     if(abs(pend_ang) < 20*(PI/180)){
00094                         run_ctrl = true;
00095                     }else{
00096                         run_ctrl = false;                        
00097                     }
00098 
00099                     if(run_ctrl) { //If controller is active
00100                     // Control Law goes here!!
00101                         pwm = 0.0;
00102 
00103                     }else{
00104                         pwm = 0.0;
00105                     }
00106                     
00107                     
00108                     // Saturate PWM command to send no more than maximum value
00109                     if(pwm>1.0) {
00110                         pwm =1.0;
00111                     }
00112                     if(pwm<-1.0) {
00113                         pwm = -1.0;
00114                     }
00115 
00116                     //Set the new output.
00117                     mot_control(1, pwm);
00118 
00119 
00120                     if((t.read()- log_timer) >= (1.0/LOG_FREQ)) { //If log sample time has passed since last write
00121                         pc.printf("%.2f, %.3f, %.3f, %.3f\r\n", t.read(),pend_ang,mot_ang,pwm); //write data to scren
00122                         log_timer = t.read();
00123                     }
00124 
00125                     led3=!led3;  //Flash Led to Indicate program is running
00126                     wait(dt);   //Wait for sample time of loop frequency
00127                 }  // while t < tstop
00128                 mot_control(1, 0.0); // Turn off Motor once test is finished
00129                 run_flag = false;
00130                 run_ctrl = false;
00131                 pc.printf("\r\nEnter Command\r\n");
00132             } //if run flag
00133         } // if pc.readable
00134     }//while
00135 }//main