![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
jnh
Diff: main.cpp
- Revision:
- 0:305d008199cd
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