Diff: main.cpp

Revision:

0:ac9d425a8b57

--- /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