Skeleton code for Quanser
Dependencies: mbed mbedWSEsbc
main.cpp@0:68715e9af246, 2019-04-23 (annotated)
- Committer:
- jdawkins
- Date:
- Tue Apr 23 13:08:46 2019 +0000
- Revision:
- 0:68715e9af246
Skeleton Code for Quanser Inverted Pendulum
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdawkins | 0:68715e9af246 | 1 | #include "mbed.h" |
jdawkins | 0:68715e9af246 | 2 | #include "mbedWSEsbc.h" |
jdawkins | 0:68715e9af246 | 3 | |
jdawkins | 0:68715e9af246 | 4 | #define PI 3.14159 |
jdawkins | 0:68715e9af246 | 5 | #define LOG_FREQ 10.0 |
jdawkins | 0:68715e9af246 | 6 | #define CTRL_FREQ 150.0 |
jdawkins | 0:68715e9af246 | 7 | |
jdawkins | 0:68715e9af246 | 8 | |
jdawkins | 0:68715e9af246 | 9 | DigitalOut myled(LED1); |
jdawkins | 0:68715e9af246 | 10 | |
jdawkins | 0:68715e9af246 | 11 | float wrapToPi(float ang){ |
jdawkins | 0:68715e9af246 | 12 | while(ang > PI){ |
jdawkins | 0:68715e9af246 | 13 | ang-= 2*PI; |
jdawkins | 0:68715e9af246 | 14 | } |
jdawkins | 0:68715e9af246 | 15 | while(ang < -PI){ |
jdawkins | 0:68715e9af246 | 16 | ang+=2*PI; |
jdawkins | 0:68715e9af246 | 17 | } |
jdawkins | 0:68715e9af246 | 18 | return ang; |
jdawkins | 0:68715e9af246 | 19 | } |
jdawkins | 0:68715e9af246 | 20 | |
jdawkins | 0:68715e9af246 | 21 | int main() |
jdawkins | 0:68715e9af246 | 22 | { |
jdawkins | 0:68715e9af246 | 23 | float log_timer = 0; //initialize variable for log timer |
jdawkins | 0:68715e9af246 | 24 | bool run_flag = false; |
jdawkins | 0:68715e9af246 | 25 | bool run_ctrl = false; |
jdawkins | 0:68715e9af246 | 26 | |
jdawkins | 0:68715e9af246 | 27 | mbedWSEsbcInit(115200); //Initialize the mbed WSE Project SBC |
jdawkins | 0:68715e9af246 | 28 | wait(.2); //delay at beginning for voltage settle purposes |
jdawkins | 0:68715e9af246 | 29 | t.start(); // Set up timer |
jdawkins | 0:68715e9af246 | 30 | |
jdawkins | 0:68715e9af246 | 31 | //Pendulum Posiiton when code starts will be the reference position |
jdawkins | 0:68715e9af246 | 32 | LS7366_write_DTR(1,0); //zero encoder channel 1 |
jdawkins | 0:68715e9af246 | 33 | LS7366_quad_mode_x4(1); |
jdawkins | 0:68715e9af246 | 34 | LS7366_reset_counter(1); |
jdawkins | 0:68715e9af246 | 35 | LS7366_write_DTR(2,0); //zero encoder channel 1 |
jdawkins | 0:68715e9af246 | 36 | LS7366_quad_mode_x4(2); |
jdawkins | 0:68715e9af246 | 37 | LS7366_reset_counter(2); |
jdawkins | 0:68715e9af246 | 38 | |
jdawkins | 0:68715e9af246 | 39 | pc.printf("Quanser Inverted Pendulum Control Program\r\n"); |
jdawkins | 0:68715e9af246 | 40 | |
jdawkins | 0:68715e9af246 | 41 | t.reset(); // zero timer |
jdawkins | 0:68715e9af246 | 42 | float sampT = t.read(); |
jdawkins | 0:68715e9af246 | 43 | float tstop = 60; //initialize stop time |
jdawkins | 0:68715e9af246 | 44 | float pwm = 0; //initialize pwm variable |
jdawkins | 0:68715e9af246 | 45 | float dt = 1/CTRL_FREQ; // set control loop sample time |
jdawkins | 0:68715e9af246 | 46 | float mot_ang; |
jdawkins | 0:68715e9af246 | 47 | float pend_ang; |
jdawkins | 0:68715e9af246 | 48 | |
jdawkins | 0:68715e9af246 | 49 | while(1) { |
jdawkins | 0:68715e9af246 | 50 | |
jdawkins | 0:68715e9af246 | 51 | if(pc.readable()) { |
jdawkins | 0:68715e9af246 | 52 | char c = pc.getc(); |
jdawkins | 0:68715e9af246 | 53 | if(c == 'H' || c == 'h') { |
jdawkins | 0:68715e9af246 | 54 | pc.printf("Command List...\r\n"); |
jdawkins | 0:68715e9af246 | 55 | pc.printf("r - run closed loop controller\r\n"); |
jdawkins | 0:68715e9af246 | 56 | pc.printf("h - display this prompt and exit\r\n"); |
jdawkins | 0:68715e9af246 | 57 | pc.printf("Enter Command\r\n"); |
jdawkins | 0:68715e9af246 | 58 | |
jdawkins | 0:68715e9af246 | 59 | } |
jdawkins | 0:68715e9af246 | 60 | if(c == 'R' || c == 'r') { |
jdawkins | 0:68715e9af246 | 61 | pc.printf("Running Pendulum lift vertical to activate controller..\r\n",tstop); |
jdawkins | 0:68715e9af246 | 62 | run_flag = true; |
jdawkins | 0:68715e9af246 | 63 | } |
jdawkins | 0:68715e9af246 | 64 | |
jdawkins | 0:68715e9af246 | 65 | if(run_flag) { |
jdawkins | 0:68715e9af246 | 66 | t.reset(); |
jdawkins | 0:68715e9af246 | 67 | log_timer = 0; |
jdawkins | 0:68715e9af246 | 68 | while(1) { |
jdawkins | 0:68715e9af246 | 69 | |
jdawkins | 0:68715e9af246 | 70 | //Emergency Stop check |
jdawkins | 0:68715e9af246 | 71 | if(pc.readable()) { //if any key is hit turn of motor and break while loop |
jdawkins | 0:68715e9af246 | 72 | mot_control(1, 0.0); |
jdawkins | 0:68715e9af246 | 73 | break; |
jdawkins | 0:68715e9af246 | 74 | } |
jdawkins | 0:68715e9af246 | 75 | |
jdawkins | 0:68715e9af246 | 76 | // read encoder 1 and encoder 2 |
jdawkins | 0:68715e9af246 | 77 | float enc1 = (float)LS7366_read_counter(1); |
jdawkins | 0:68715e9af246 | 78 | float enc2 = (float)LS7366_read_counter(2); |
jdawkins | 0:68715e9af246 | 79 | |
jdawkins | 0:68715e9af246 | 80 | //Convert Encoder readings to angles relative to pendulum |
jdawkins | 0:68715e9af246 | 81 | pend_ang = wrapToPi((2*PI/4096)*enc1 - PI); |
jdawkins | 0:68715e9af246 | 82 | mot_ang = -(2*PI/4096)*enc2; |
jdawkins | 0:68715e9af246 | 83 | |
jdawkins | 0:68715e9af246 | 84 | |
jdawkins | 0:68715e9af246 | 85 | //Logic to exit loop if Arm moves too far |
jdawkins | 0:68715e9af246 | 86 | if(abs(mot_ang) > 90*(PI/180)) { |
jdawkins | 0:68715e9af246 | 87 | pc.printf("\r\nPast Safety Limit\r\n"); |
jdawkins | 0:68715e9af246 | 88 | run_ctrl = false; |
jdawkins | 0:68715e9af246 | 89 | break; |
jdawkins | 0:68715e9af246 | 90 | } |
jdawkins | 0:68715e9af246 | 91 | |
jdawkins | 0:68715e9af246 | 92 | //Logic to only run control if pendulum is near vertical |
jdawkins | 0:68715e9af246 | 93 | if(abs(pend_ang) < 20*(PI/180)){ |
jdawkins | 0:68715e9af246 | 94 | run_ctrl = true; |
jdawkins | 0:68715e9af246 | 95 | }else{ |
jdawkins | 0:68715e9af246 | 96 | run_ctrl = false; |
jdawkins | 0:68715e9af246 | 97 | } |
jdawkins | 0:68715e9af246 | 98 | |
jdawkins | 0:68715e9af246 | 99 | if(run_ctrl) { //If controller is active |
jdawkins | 0:68715e9af246 | 100 | // Control Law goes here!! |
jdawkins | 0:68715e9af246 | 101 | pwm = 0.0; |
jdawkins | 0:68715e9af246 | 102 | |
jdawkins | 0:68715e9af246 | 103 | }else{ |
jdawkins | 0:68715e9af246 | 104 | pwm = 0.0; |
jdawkins | 0:68715e9af246 | 105 | } |
jdawkins | 0:68715e9af246 | 106 | |
jdawkins | 0:68715e9af246 | 107 | |
jdawkins | 0:68715e9af246 | 108 | // Saturate PWM command to send no more than maximum value |
jdawkins | 0:68715e9af246 | 109 | if(pwm>1.0) { |
jdawkins | 0:68715e9af246 | 110 | pwm =1.0; |
jdawkins | 0:68715e9af246 | 111 | } |
jdawkins | 0:68715e9af246 | 112 | if(pwm<-1.0) { |
jdawkins | 0:68715e9af246 | 113 | pwm = -1.0; |
jdawkins | 0:68715e9af246 | 114 | } |
jdawkins | 0:68715e9af246 | 115 | |
jdawkins | 0:68715e9af246 | 116 | //Set the new output. |
jdawkins | 0:68715e9af246 | 117 | mot_control(1, pwm); |
jdawkins | 0:68715e9af246 | 118 | |
jdawkins | 0:68715e9af246 | 119 | |
jdawkins | 0:68715e9af246 | 120 | if((t.read()- log_timer) >= (1.0/LOG_FREQ)) { //If log sample time has passed since last write |
jdawkins | 0:68715e9af246 | 121 | pc.printf("%.2f, %.3f, %.3f, %.3f\r\n", t.read(),pend_ang,mot_ang,pwm); //write data to scren |
jdawkins | 0:68715e9af246 | 122 | log_timer = t.read(); |
jdawkins | 0:68715e9af246 | 123 | } |
jdawkins | 0:68715e9af246 | 124 | |
jdawkins | 0:68715e9af246 | 125 | led3=!led3; //Flash Led to Indicate program is running |
jdawkins | 0:68715e9af246 | 126 | wait(dt); //Wait for sample time of loop frequency |
jdawkins | 0:68715e9af246 | 127 | } // while t < tstop |
jdawkins | 0:68715e9af246 | 128 | mot_control(1, 0.0); // Turn off Motor once test is finished |
jdawkins | 0:68715e9af246 | 129 | run_flag = false; |
jdawkins | 0:68715e9af246 | 130 | run_ctrl = false; |
jdawkins | 0:68715e9af246 | 131 | pc.printf("\r\nEnter Command\r\n"); |
jdawkins | 0:68715e9af246 | 132 | } //if run flag |
jdawkins | 0:68715e9af246 | 133 | } // if pc.readable |
jdawkins | 0:68715e9af246 | 134 | }//while |
jdawkins | 0:68715e9af246 | 135 | }//main |