Code used for Active suspension experiment in ES301 Fall 2014
Dependencies: PID mbed mbedWSEsbc
main.cpp@0:ac9d425a8b57, 2015-10-20 (annotated)
- Committer:
- jdawkins
- Date:
- Tue Oct 20 19:35:43 2015 +0000
- Revision:
- 0:ac9d425a8b57
Initial Commit of Active Suspension Demo code
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdawkins | 0:ac9d425a8b57 | 1 | #include "mbed.h" |
jdawkins | 0:ac9d425a8b57 | 2 | #include "mbedWSEsbc.h" |
jdawkins | 0:ac9d425a8b57 | 3 | |
jdawkins | 0:ac9d425a8b57 | 4 | #define PI 3.14159 |
jdawkins | 0:ac9d425a8b57 | 5 | #define LOG_FREQ 100; |
jdawkins | 0:ac9d425a8b57 | 6 | #define CTRL_FREQ 1000; |
jdawkins | 0:ac9d425a8b57 | 7 | |
jdawkins | 0:ac9d425a8b57 | 8 | |
jdawkins | 0:ac9d425a8b57 | 9 | DigitalOut myled(LED1); |
jdawkins | 0:ac9d425a8b57 | 10 | |
jdawkins | 0:ac9d425a8b57 | 11 | int main() { |
jdawkins | 0:ac9d425a8b57 | 12 | |
jdawkins | 0:ac9d425a8b57 | 13 | float Kp = 0.0028;//2*7.92; Ku = 25;Tuned with ziger-nichols |
jdawkins | 0:ac9d425a8b57 | 14 | float Ki = 0.002;//1.5*8; |
jdawkins | 0:ac9d425a8b57 | 15 | float Kd = 0.000;//0*0.115; |
jdawkins | 0:ac9d425a8b57 | 16 | float dt = 0.001; |
jdawkins | 0:ac9d425a8b57 | 17 | float err,err_i,err_d,err_old=0; |
jdawkins | 0:ac9d425a8b57 | 18 | float pwm = 0; |
jdawkins | 0:ac9d425a8b57 | 19 | float write_timer = 0; |
jdawkins | 0:ac9d425a8b57 | 20 | bool is_square = false; |
jdawkins | 0:ac9d425a8b57 | 21 | bool run_flag = false; |
jdawkins | 0:ac9d425a8b57 | 22 | |
jdawkins | 0:ac9d425a8b57 | 23 | mbedWSEsbcInit(921600); //Initialize the mbed WSE Project SBC |
jdawkins | 0:ac9d425a8b57 | 24 | wait(.2); //delay at beginning for voltage settle purposes |
jdawkins | 0:ac9d425a8b57 | 25 | t.start(); // Set up timer |
jdawkins | 0:ac9d425a8b57 | 26 | |
jdawkins | 0:ac9d425a8b57 | 27 | |
jdawkins | 0:ac9d425a8b57 | 28 | pc.printf("Quanser Active Suspension Demo\r\n"); |
jdawkins | 0:ac9d425a8b57 | 29 | |
jdawkins | 0:ac9d425a8b57 | 30 | |
jdawkins | 0:ac9d425a8b57 | 31 | t.reset(); // zero timer |
jdawkins | 0:ac9d425a8b57 | 32 | float sampT = t.read(); |
jdawkins | 0:ac9d425a8b57 | 33 | float tstop = 10; |
jdawkins | 0:ac9d425a8b57 | 34 | |
jdawkins | 0:ac9d425a8b57 | 35 | float sine_amp = 1000.0; |
jdawkins | 0:ac9d425a8b57 | 36 | float freq = 1; |
jdawkins | 0:ac9d425a8b57 | 37 | |
jdawkins | 0:ac9d425a8b57 | 38 | while(1) { |
jdawkins | 0:ac9d425a8b57 | 39 | |
jdawkins | 0:ac9d425a8b57 | 40 | if(pc.readable()) |
jdawkins | 0:ac9d425a8b57 | 41 | { |
jdawkins | 0:ac9d425a8b57 | 42 | char c = pc.getc(); |
jdawkins | 0:ac9d425a8b57 | 43 | |
jdawkins | 0:ac9d425a8b57 | 44 | if(c == 'A' || c == 'a') |
jdawkins | 0:ac9d425a8b57 | 45 | { |
jdawkins | 0:ac9d425a8b57 | 46 | pc.scanf("%f",&sine_amp); |
jdawkins | 0:ac9d425a8b57 | 47 | } |
jdawkins | 0:ac9d425a8b57 | 48 | if(c == 'F' || c == 'f') |
jdawkins | 0:ac9d425a8b57 | 49 | { |
jdawkins | 0:ac9d425a8b57 | 50 | pc.scanf("%f",&freq); |
jdawkins | 0:ac9d425a8b57 | 51 | } |
jdawkins | 0:ac9d425a8b57 | 52 | if(c == 'Q' || c == 'q') |
jdawkins | 0:ac9d425a8b57 | 53 | { |
jdawkins | 0:ac9d425a8b57 | 54 | is_square = true; |
jdawkins | 0:ac9d425a8b57 | 55 | } |
jdawkins | 0:ac9d425a8b57 | 56 | if(c == 'R' || c == 'r') |
jdawkins | 0:ac9d425a8b57 | 57 | { |
jdawkins | 0:ac9d425a8b57 | 58 | run_flag = true; |
jdawkins | 0:ac9d425a8b57 | 59 | } |
jdawkins | 0:ac9d425a8b57 | 60 | if(c == 'S' || c == 's') |
jdawkins | 0:ac9d425a8b57 | 61 | { |
jdawkins | 0:ac9d425a8b57 | 62 | is_square = false; |
jdawkins | 0:ac9d425a8b57 | 63 | } |
jdawkins | 0:ac9d425a8b57 | 64 | if(c == 'T' || c == 't') |
jdawkins | 0:ac9d425a8b57 | 65 | { |
jdawkins | 0:ac9d425a8b57 | 66 | pc.scanf("%f",&tstop); |
jdawkins | 0:ac9d425a8b57 | 67 | } |
jdawkins | 0:ac9d425a8b57 | 68 | if(c == 'u' || c == 'U') |
jdawkins | 0:ac9d425a8b57 | 69 | { |
jdawkins | 0:ac9d425a8b57 | 70 | pc.printf("Enter value for Amplitude (0 - 5000 max). Current Amp is %.1f\r\n", sine_amp); |
jdawkins | 0:ac9d425a8b57 | 71 | pc.scanf("%f",&sine_amp); |
jdawkins | 0:ac9d425a8b57 | 72 | pc.printf("Enter value for Frequency in Hertz. Current Freq is %.1f Hz\r\n", freq); |
jdawkins | 0:ac9d425a8b57 | 73 | pc.scanf("%f",&freq); |
jdawkins | 0:ac9d425a8b57 | 74 | |
jdawkins | 0:ac9d425a8b57 | 75 | pc.printf("Enter value for Run Duration in Seconds\r\n", freq); |
jdawkins | 0:ac9d425a8b57 | 76 | pc.scanf("%f",&tstop); |
jdawkins | 0:ac9d425a8b57 | 77 | |
jdawkins | 0:ac9d425a8b57 | 78 | } |
jdawkins | 0:ac9d425a8b57 | 79 | |
jdawkins | 0:ac9d425a8b57 | 80 | if(c == ' ') |
jdawkins | 0:ac9d425a8b57 | 81 | { |
jdawkins | 0:ac9d425a8b57 | 82 | mot_control(1, 0.0); |
jdawkins | 0:ac9d425a8b57 | 83 | wait(2); |
jdawkins | 0:ac9d425a8b57 | 84 | pc.printf("Stopped and Gains Zeroed\r\n"); |
jdawkins | 0:ac9d425a8b57 | 85 | pc.printf("Press any key to continue...\r\n"); |
jdawkins | 0:ac9d425a8b57 | 86 | while(!pc.readable()); |
jdawkins | 0:ac9d425a8b57 | 87 | } |
jdawkins | 0:ac9d425a8b57 | 88 | |
jdawkins | 0:ac9d425a8b57 | 89 | if(abs(sine_amp) >5000) |
jdawkins | 0:ac9d425a8b57 | 90 | sine_amp = 5000; |
jdawkins | 0:ac9d425a8b57 | 91 | |
jdawkins | 0:ac9d425a8b57 | 92 | if(run_flag){ |
jdawkins | 0:ac9d425a8b57 | 93 | err_i = 0; |
jdawkins | 0:ac9d425a8b57 | 94 | err = 0; |
jdawkins | 0:ac9d425a8b57 | 95 | err_d = 0; |
jdawkins | 0:ac9d425a8b57 | 96 | err_old = 0; |
jdawkins | 0:ac9d425a8b57 | 97 | LS7366_write_DTR(1,0); //zero encoder channel 1 |
jdawkins | 0:ac9d425a8b57 | 98 | LS7366_reset_counter(1); |
jdawkins | 0:ac9d425a8b57 | 99 | LS7366_write_DTR(2,0); //zero encoder channel 1 |
jdawkins | 0:ac9d425a8b57 | 100 | LS7366_reset_counter(2); |
jdawkins | 0:ac9d425a8b57 | 101 | t.reset(); |
jdawkins | 0:ac9d425a8b57 | 102 | while(t.read() < tstop){ |
jdawkins | 0:ac9d425a8b57 | 103 | // update the set point |
jdawkins | 0:ac9d425a8b57 | 104 | float enc_cmd = sine_amp * sin(2*PI*freq*t.read()); |
jdawkins | 0:ac9d425a8b57 | 105 | |
jdawkins | 0:ac9d425a8b57 | 106 | if(is_square){ |
jdawkins | 0:ac9d425a8b57 | 107 | if(enc_cmd > 0){ |
jdawkins | 0:ac9d425a8b57 | 108 | enc_cmd = sine_amp; |
jdawkins | 0:ac9d425a8b57 | 109 | }else if (enc_cmd < 0){ |
jdawkins | 0:ac9d425a8b57 | 110 | enc_cmd = -sine_amp; |
jdawkins | 0:ac9d425a8b57 | 111 | }else{ |
jdawkins | 0:ac9d425a8b57 | 112 | enc_cmd = 0; |
jdawkins | 0:ac9d425a8b57 | 113 | } |
jdawkins | 0:ac9d425a8b57 | 114 | } |
jdawkins | 0:ac9d425a8b57 | 115 | |
jdawkins | 0:ac9d425a8b57 | 116 | // if(t.read() > tstop){ |
jdawkins | 0:ac9d425a8b57 | 117 | // enc_cmd = 0; |
jdawkins | 0:ac9d425a8b57 | 118 | //} |
jdawkins | 0:ac9d425a8b57 | 119 | //Update the process variable. |
jdawkins | 0:ac9d425a8b57 | 120 | float enc1 = (float)LS7366_read_counter(1); |
jdawkins | 0:ac9d425a8b57 | 121 | |
jdawkins | 0:ac9d425a8b57 | 122 | err = enc_cmd - enc1; |
jdawkins | 0:ac9d425a8b57 | 123 | err_i += err*dt; |
jdawkins | 0:ac9d425a8b57 | 124 | err_d = (err-err_old)/dt; |
jdawkins | 0:ac9d425a8b57 | 125 | |
jdawkins | 0:ac9d425a8b57 | 126 | pwm = -(Kp*err + Kd*err_d +Ki*err_i); |
jdawkins | 0:ac9d425a8b57 | 127 | |
jdawkins | 0:ac9d425a8b57 | 128 | if(pwm>1.0){ |
jdawkins | 0:ac9d425a8b57 | 129 | pwm =1.0; |
jdawkins | 0:ac9d425a8b57 | 130 | |
jdawkins | 0:ac9d425a8b57 | 131 | } |
jdawkins | 0:ac9d425a8b57 | 132 | else if(pwm<-1.0){ |
jdawkins | 0:ac9d425a8b57 | 133 | pwm = -1.0; |
jdawkins | 0:ac9d425a8b57 | 134 | } |
jdawkins | 0:ac9d425a8b57 | 135 | else{} |
jdawkins | 0:ac9d425a8b57 | 136 | |
jdawkins | 0:ac9d425a8b57 | 137 | //Set the new output. |
jdawkins | 0:ac9d425a8b57 | 138 | |
jdawkins | 0:ac9d425a8b57 | 139 | mot_control(1, pwm); |
jdawkins | 0:ac9d425a8b57 | 140 | |
jdawkins | 0:ac9d425a8b57 | 141 | float enc2 = -(float)LS7366_read_counter(2); |
jdawkins | 0:ac9d425a8b57 | 142 | float adc0 = read_max1270_volts(0, 1, 1); //chan, range, bipol |
jdawkins | 0:ac9d425a8b57 | 143 | |
jdawkins | 0:ac9d425a8b57 | 144 | // if( (t.read()- write_timer) >= 0.01){ |
jdawkins | 0:ac9d425a8b57 | 145 | pc.printf("%.1f, %.1f, %.1f, %.3f, %.3f\r\n", enc_cmd, enc1, enc2, pwm, adc0); |
jdawkins | 0:ac9d425a8b57 | 146 | |
jdawkins | 0:ac9d425a8b57 | 147 | // } |
jdawkins | 0:ac9d425a8b57 | 148 | led3=!led3; |
jdawkins | 0:ac9d425a8b57 | 149 | err_old = err; |
jdawkins | 0:ac9d425a8b57 | 150 | |
jdawkins | 0:ac9d425a8b57 | 151 | wait(dt); |
jdawkins | 0:ac9d425a8b57 | 152 | } // while t < tstop |
jdawkins | 0:ac9d425a8b57 | 153 | run_flag = false; |
jdawkins | 0:ac9d425a8b57 | 154 | is_square = false; |
jdawkins | 0:ac9d425a8b57 | 155 | } //if run flag |
jdawkins | 0:ac9d425a8b57 | 156 | } // if pc.readable |
jdawkins | 0:ac9d425a8b57 | 157 | }//while |
jdawkins | 0:ac9d425a8b57 | 158 | }//main |