Diff: main.cpp

Revision:

0:305d008199cd

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 28 02:32:35 2015 +0000
@@ -0,0 +1,156 @@
+#include "mbed.h"
+#include "mbedWSEsbc.h"
+
+#define PI 3.14159
+#define LOG_FREQ 50.0
+#define CTRL_FREQ 50.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;
+    
+                        float k1 = -.4247;
+                        float k2 = -2.0; //-4.0138;
+                        float k3 = -.3187*4;
+                        float k4 = -.2712;
+                        //float l0 = .2;
+                        //float Kt = .5;
+                        //float Ra = 2.6;
+                        float x = 0;
+                        float xdot = 0;
+                        float theta = 0;
+                        float thetadot = 0;
+                        float l=.2;
+                        float old_x = 0;
+                        float old_theta = 0;
+
+    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_reset_counter(1); 
+    LS7366_write_DTR(2,0);    //zero encoder channel 1      
+    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!!
+                       x = l*mot_ang;  //Option 1.
+                       xdot = (x - old_x)/dt;
+                       theta = pend_ang;
+                       thetadot = (theta - old_theta)/dt;  
+                       pwm = (-(k1*thetadot) - (k2*theta) - (k3*xdot) - (k4*x));  
+                        //Reference control law.  qact = qi - k1a*(19-height1)- k2a*(16-height2);
+                        //Aging vars.
+                        old_x = x;
+                        old_theta = theta;
+                        
+                    }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 \r\n", t.read());//,pend_ang);,mot_ang,pwm, thetadot, xdot); //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
\ No newline at end of file