David Gordeuk / Mbed 2 deprecated 306Project

jnh

Dependencies:   mbed mbedWSEsbc

main.cpp@0:305d008199cd, 2015-04-28 (annotated)

Committer:
gordeezy
Date:
Tue Apr 28 02:32:35 2015 +0000
Revision:
0:305d008199cd
h;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gordeezy 0:305d008199cd 1 #include "mbed.h"
gordeezy 0:305d008199cd 2 #include "mbedWSEsbc.h"
gordeezy 0:305d008199cd 3
gordeezy 0:305d008199cd 4 #define PI 3.14159
gordeezy 0:305d008199cd 5 #define LOG_FREQ 50.0
gordeezy 0:305d008199cd 6 #define CTRL_FREQ 50.0
gordeezy 0:305d008199cd 7
gordeezy 0:305d008199cd 8
gordeezy 0:305d008199cd 9 DigitalOut myled(LED1);
gordeezy 0:305d008199cd 10
gordeezy 0:305d008199cd 11 float wrapToPi(float ang){
gordeezy 0:305d008199cd 12 while(ang > PI){
gordeezy 0:305d008199cd 13 ang-= 2*PI;
gordeezy 0:305d008199cd 14 }
gordeezy 0:305d008199cd 15 while(ang < -PI){
gordeezy 0:305d008199cd 16 ang+=2*PI;
gordeezy 0:305d008199cd 17 }
gordeezy 0:305d008199cd 18 return ang;
gordeezy 0:305d008199cd 19 }
gordeezy 0:305d008199cd 20
gordeezy 0:305d008199cd 21 int main()
gordeezy 0:305d008199cd 22 {
gordeezy 0:305d008199cd 23 float log_timer = 0; //initialize variable for log timer
gordeezy 0:305d008199cd 24 bool run_flag = false;
gordeezy 0:305d008199cd 25 bool run_ctrl = false;
gordeezy 0:305d008199cd 26
gordeezy 0:305d008199cd 27 float k1 = -.4247;
gordeezy 0:305d008199cd 28 float k2 = -2.0; //-4.0138;
gordeezy 0:305d008199cd 29 float k3 = -.3187*4;
gordeezy 0:305d008199cd 30 float k4 = -.2712;
gordeezy 0:305d008199cd 31 //float l0 = .2;
gordeezy 0:305d008199cd 32 //float Kt = .5;
gordeezy 0:305d008199cd 33 //float Ra = 2.6;
gordeezy 0:305d008199cd 34 float x = 0;
gordeezy 0:305d008199cd 35 float xdot = 0;
gordeezy 0:305d008199cd 36 float theta = 0;
gordeezy 0:305d008199cd 37 float thetadot = 0;
gordeezy 0:305d008199cd 38 float l=.2;
gordeezy 0:305d008199cd 39 float old_x = 0;
gordeezy 0:305d008199cd 40 float old_theta = 0;
gordeezy 0:305d008199cd 41
gordeezy 0:305d008199cd 42 mbedWSEsbcInit(115200); //Initialize the mbed WSE Project SBC
gordeezy 0:305d008199cd 43 wait(.2); //delay at beginning for voltage settle purposes
gordeezy 0:305d008199cd 44 t.start(); // Set up timer
gordeezy 0:305d008199cd 45
gordeezy 0:305d008199cd 46 //Pendulum Posiiton when code starts will be the reference position
gordeezy 0:305d008199cd 47 LS7366_write_DTR(1,0); //zero encoder channel 1
gordeezy 0:305d008199cd 48 LS7366_reset_counter(1);
gordeezy 0:305d008199cd 49 LS7366_write_DTR(2,0); //zero encoder channel 1
gordeezy 0:305d008199cd 50 LS7366_reset_counter(2);
gordeezy 0:305d008199cd 51
gordeezy 0:305d008199cd 52 pc.printf("Quanser Inverted Pendulum Control Program\r\n");
gordeezy 0:305d008199cd 53
gordeezy 0:305d008199cd 54 t.reset(); // zero timer
gordeezy 0:305d008199cd 55 float sampT = t.read();
gordeezy 0:305d008199cd 56 float tstop = 60; //initialize stop time
gordeezy 0:305d008199cd 57 float pwm = 0; //initialize pwm variable
gordeezy 0:305d008199cd 58 float dt = 1/CTRL_FREQ; // set control loop sample time
gordeezy 0:305d008199cd 59 float mot_ang;
gordeezy 0:305d008199cd 60 float pend_ang;
gordeezy 0:305d008199cd 61
gordeezy 0:305d008199cd 62 while(1) {
gordeezy 0:305d008199cd 63
gordeezy 0:305d008199cd 64 if(pc.readable()) {
gordeezy 0:305d008199cd 65 char c = pc.getc();
gordeezy 0:305d008199cd 66 if(c == 'H' || c == 'h') {
gordeezy 0:305d008199cd 67 pc.printf("Command List...\r\n");
gordeezy 0:305d008199cd 68 pc.printf("r - run closed loop controller\r\n");
gordeezy 0:305d008199cd 69 pc.printf("h - display this prompt and exit\r\n");
gordeezy 0:305d008199cd 70 pc.printf("Enter Command\r\n");
gordeezy 0:305d008199cd 71
gordeezy 0:305d008199cd 72 }
gordeezy 0:305d008199cd 73 if(c == 'R' || c == 'r') {
gordeezy 0:305d008199cd 74 pc.printf("Running Pendulum lift vertical to activate controller..\r\n",tstop);
gordeezy 0:305d008199cd 75 run_flag = true;
gordeezy 0:305d008199cd 76 }
gordeezy 0:305d008199cd 77
gordeezy 0:305d008199cd 78 if(run_flag) {
gordeezy 0:305d008199cd 79 t.reset();
gordeezy 0:305d008199cd 80 log_timer = 0;
gordeezy 0:305d008199cd 81 while(1) {
gordeezy 0:305d008199cd 82
gordeezy 0:305d008199cd 83 //Emergency Stop check
gordeezy 0:305d008199cd 84 if(pc.readable()) { //if any key is hit turn of motor and break while loop
gordeezy 0:305d008199cd 85 mot_control(1, 0.0);
gordeezy 0:305d008199cd 86 break;
gordeezy 0:305d008199cd 87 }
gordeezy 0:305d008199cd 88
gordeezy 0:305d008199cd 89 // read encoder 1 and encoder 2
gordeezy 0:305d008199cd 90 float enc1 = (float)LS7366_read_counter(1);
gordeezy 0:305d008199cd 91 float enc2 = (float)LS7366_read_counter(2);
gordeezy 0:305d008199cd 92
gordeezy 0:305d008199cd 93 //Convert Encoder readings to angles relative to pendulum
gordeezy 0:305d008199cd 94 pend_ang = wrapToPi((2*PI/4096)*enc1 - PI);
gordeezy 0:305d008199cd 95 mot_ang = -(2*PI/4096)*enc2;
gordeezy 0:305d008199cd 96
gordeezy 0:305d008199cd 97
gordeezy 0:305d008199cd 98 //Logic to exit loop if Arm moves too far
gordeezy 0:305d008199cd 99 if(abs(mot_ang) > 90*(PI/180)) {
gordeezy 0:305d008199cd 100 pc.printf("\r\nPast Safety Limit\r\n");
gordeezy 0:305d008199cd 101 run_ctrl = false;
gordeezy 0:305d008199cd 102 break;
gordeezy 0:305d008199cd 103 }
gordeezy 0:305d008199cd 104
gordeezy 0:305d008199cd 105 //Logic to only run control if pendulum is near vertical
gordeezy 0:305d008199cd 106 if(abs(pend_ang) < 20*(PI/180)){
gordeezy 0:305d008199cd 107 run_ctrl = true;
gordeezy 0:305d008199cd 108 }else{
gordeezy 0:305d008199cd 109 run_ctrl = false;
gordeezy 0:305d008199cd 110 }
gordeezy 0:305d008199cd 111
gordeezy 0:305d008199cd 112 if(run_ctrl) { //If controller is active
gordeezy 0:305d008199cd 113 // Control Law goes here!!
gordeezy 0:305d008199cd 114 x = l*mot_ang; //Option 1.
gordeezy 0:305d008199cd 115 xdot = (x - old_x)/dt;
gordeezy 0:305d008199cd 116 theta = pend_ang;
gordeezy 0:305d008199cd 117 thetadot = (theta - old_theta)/dt;
gordeezy 0:305d008199cd 118 pwm = (-(k1*thetadot) - (k2*theta) - (k3*xdot) - (k4*x));
gordeezy 0:305d008199cd 119 //Reference control law. qact = qi - k1a*(19-height1)- k2a*(16-height2);
gordeezy 0:305d008199cd 120 //Aging vars.
gordeezy 0:305d008199cd 121 old_x = x;
gordeezy 0:305d008199cd 122 old_theta = theta;
gordeezy 0:305d008199cd 123
gordeezy 0:305d008199cd 124 }else{
gordeezy 0:305d008199cd 125 pwm = 0.0;
gordeezy 0:305d008199cd 126 }
gordeezy 0:305d008199cd 127
gordeezy 0:305d008199cd 128
gordeezy 0:305d008199cd 129 // Saturate PWM command to send no more than maximum value
gordeezy 0:305d008199cd 130 if(pwm>1.0) {
gordeezy 0:305d008199cd 131 pwm =1.0;
gordeezy 0:305d008199cd 132 }
gordeezy 0:305d008199cd 133 if(pwm<-1.0) {
gordeezy 0:305d008199cd 134 pwm = -1.0;
gordeezy 0:305d008199cd 135 }
gordeezy 0:305d008199cd 136
gordeezy 0:305d008199cd 137 //Set the new output.
gordeezy 0:305d008199cd 138 mot_control(1, pwm);
gordeezy 0:305d008199cd 139
gordeezy 0:305d008199cd 140
gordeezy 0:305d008199cd 141 if((t.read()- log_timer) >= (1.0/LOG_FREQ)) { //If log sample time has passed since last write
gordeezy 0:305d008199cd 142 pc.printf("%.2f \r\n", t.read());//,pend_ang);,mot_ang,pwm, thetadot, xdot); //write data to scren
gordeezy 0:305d008199cd 143 log_timer = t.read();
gordeezy 0:305d008199cd 144 }
gordeezy 0:305d008199cd 145
gordeezy 0:305d008199cd 146 led3=!led3; //Flash Led to Indicate program is running
gordeezy 0:305d008199cd 147 wait(dt); //Wait for sample time of loop frequency
gordeezy 0:305d008199cd 148 } // while t < tstop
gordeezy 0:305d008199cd 149 mot_control(1, 0.0); // Turn off Motor once test is finished
gordeezy 0:305d008199cd 150 run_flag = false;
gordeezy 0:305d008199cd 151 run_ctrl = false;
gordeezy 0:305d008199cd 152 pc.printf("\r\nEnter Command\r\n");
gordeezy 0:305d008199cd 153 } //if run flag
gordeezy 0:305d008199cd 154 } // if pc.readable
gordeezy 0:305d008199cd 155 }//while
gordeezy 0:305d008199cd 156 }//main