USNA ES305
Code used for Active suspension experiment in ES301 Fall 2014
Dependencies: PID mbed mbedWSEsbc
Revision 0:ac9d425a8b57, committed 2015-10-20
- Comitter:
- jdawkins
- Date:
- Tue Oct 20 19:35:43 2015 +0000
- Commit message:
- Initial Commit of Active Suspension Demo code
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/PID.lib Tue Oct 20 19:35:43 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Oct 20 19:35:43 2015 +0000
@@ -0,0 +1,158 @@
+#include "mbed.h"
+#include "mbedWSEsbc.h"
+
+#define PI 3.14159
+#define LOG_FREQ 100;
+#define CTRL_FREQ 1000;
+
+
+DigitalOut myled(LED1);
+
+int main() {
+
+ float Kp = 0.0028;//2*7.92; Ku = 25;Tuned with ziger-nichols
+ float Ki = 0.002;//1.5*8;
+ float Kd = 0.000;//0*0.115;
+ float dt = 0.001;
+ float err,err_i,err_d,err_old=0;
+ float pwm = 0;
+ float write_timer = 0;
+ bool is_square = false;
+ bool run_flag = false;
+
+ mbedWSEsbcInit(921600); //Initialize the mbed WSE Project SBC
+ wait(.2); //delay at beginning for voltage settle purposes
+ t.start(); // Set up timer
+
+
+ pc.printf("Quanser Active Suspension Demo\r\n");
+
+
+ t.reset(); // zero timer
+ float sampT = t.read();
+ float tstop = 10;
+
+ float sine_amp = 1000.0;
+ float freq = 1;
+
+ while(1) {
+
+ if(pc.readable())
+ {
+ char c = pc.getc();
+
+ if(c == 'A' || c == 'a')
+ {
+ pc.scanf("%f",&sine_amp);
+ }
+ if(c == 'F' || c == 'f')
+ {
+ pc.scanf("%f",&freq);
+ }
+ if(c == 'Q' || c == 'q')
+ {
+ is_square = true;
+ }
+ if(c == 'R' || c == 'r')
+ {
+ run_flag = true;
+ }
+ if(c == 'S' || c == 's')
+ {
+ is_square = false;
+ }
+ if(c == 'T' || c == 't')
+ {
+ pc.scanf("%f",&tstop);
+ }
+ if(c == 'u' || c == 'U')
+ {
+ pc.printf("Enter value for Amplitude (0 - 5000 max). Current Amp is %.1f\r\n", sine_amp);
+ pc.scanf("%f",&sine_amp);
+ pc.printf("Enter value for Frequency in Hertz. Current Freq is %.1f Hz\r\n", freq);
+ pc.scanf("%f",&freq);
+
+ pc.printf("Enter value for Run Duration in Seconds\r\n", freq);
+ pc.scanf("%f",&tstop);
+
+ }
+
+ if(c == ' ')
+ {
+ mot_control(1, 0.0);
+ wait(2);
+ pc.printf("Stopped and Gains Zeroed\r\n");
+ pc.printf("Press any key to continue...\r\n");
+ while(!pc.readable());
+ }
+
+ if(abs(sine_amp) >5000)
+ sine_amp = 5000;
+
+ if(run_flag){
+ err_i = 0;
+ err = 0;
+ err_d = 0;
+ err_old = 0;
+ 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);
+ t.reset();
+ while(t.read() < tstop){
+ // update the set point
+ float enc_cmd = sine_amp * sin(2*PI*freq*t.read());
+
+ if(is_square){
+ if(enc_cmd > 0){
+ enc_cmd = sine_amp;
+ }else if (enc_cmd < 0){
+ enc_cmd = -sine_amp;
+ }else{
+ enc_cmd = 0;
+ }
+ }
+
+ // if(t.read() > tstop){
+ // enc_cmd = 0;
+ //}
+ //Update the process variable.
+ float enc1 = (float)LS7366_read_counter(1);
+
+ err = enc_cmd - enc1;
+ err_i += err*dt;
+ err_d = (err-err_old)/dt;
+
+ pwm = -(Kp*err + Kd*err_d +Ki*err_i);
+
+ if(pwm>1.0){
+ pwm =1.0;
+
+ }
+ else if(pwm<-1.0){
+ pwm = -1.0;
+ }
+ else{}
+
+ //Set the new output.
+
+ mot_control(1, pwm);
+
+ float enc2 = -(float)LS7366_read_counter(2);
+ float adc0 = read_max1270_volts(0, 1, 1); //chan, range, bipol
+
+ // if( (t.read()- write_timer) >= 0.01){
+ pc.printf("%.1f, %.1f, %.1f, %.3f, %.3f\r\n", enc_cmd, enc1, enc2, pwm, adc0);
+
+ // }
+ led3=!led3;
+ err_old = err;
+
+ wait(dt);
+ } // while t < tstop
+ run_flag = false;
+ is_square = false;
+ } //if run flag
+ } // if pc.readable
+ }//while
+}//main
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 20 19:35:43 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/9327015d4013 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbedWSEsbc.lib Tue Oct 20 19:35:43 2015 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/jebradshaw/code/mbedWSEsbc/#1aa4a75f6885