USNA ES305 / Mbed 2 deprecated active_suspension_demo

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?

UserRevisionLine numberNew 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