David Gordeuk
/
306Project
jnh
main.cpp
- Committer:
- gordeezy
- Date:
- 2015-04-28
- Revision:
- 0:305d008199cd
File content as of revision 0:305d008199cd:
#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