jnh

Dependencies:   mbed mbedWSEsbc

Files at this revision

API Documentation at this revision

Comitter:
gordeezy
Date:
Tue Apr 28 02:32:35 2015 +0000
Commit message:
h;

Changed in this revision

main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
mbedWSEsbc.lib Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 305d008199cd main.cpp
--- /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
diff -r 000000000000 -r 305d008199cd mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Tue Apr 28 02:32:35 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/487b796308b0
\ No newline at end of file
diff -r 000000000000 -r 305d008199cd mbedWSEsbc.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbedWSEsbc.lib	Tue Apr 28 02:32:35 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/jebradshaw/code/mbedWSEsbc/#9041603c430e