David Gordeuk
jnh
Revision 0:305d008199cd, committed 2015-04-28
- Comitter:
- gordeezy
- Date:
- Tue Apr 28 02:32:35 2015 +0000
- Commit message:
- h;
Changed in this revision
--- /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
--- /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
--- /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