File content as of revision 0:ac9d425a8b57:

#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