main.cpp

00001 #include "mbed.h"
00002 #include "QEI.h"
00003 
00004 #include "servoController.h"
00005 #include "emg.h"
00006 
00007 #define pi 3.14159265359;
00008 
00009 PwmOut Motor1_pwm(D5);
00010 DigitalOut SlideMotor_Direction(D4);
00011 PwmOut Motor2_pwm(D6);
00012 DigitalOut LiftMotor_Direction(D7);
00013 AnalogIn Potmeter(A0);
00014 AnalogIn Potmeter2(A1);
00015 QEI Slide_Encoder(D12,D13,NC,64);
00016 QEI Lift_Encoder(D10,D11,NC,64);
00017 
00018 BiQuadChain Hz_1;
00019 BiQuad bq0(0.05852855368, 0.11705710736, 0.05852855368,-1.05207469728, 0.28586907478);
00020 BiQuad bq1(0.06463239794, 0.12926479589, 0.06463239794,-1.16338171052, 0.42191097989);
00021 BiQuad bq2(0.07902502847, 0.15805005694, 0.07902502847,-1.42439823874, 0.7409311811);
00022 
00023 //Serial pc(USBTX,USBRX);
00024 Ticker Controller;
00025 
00026 bool Controller_Flag=0;
00027 float Frequency = 30;
00028 float Frequency_PWM = 10000;
00029 
00030 float Slide_Radius = 12.5;
00031 float Slide_Multiplier = 1;
00032 float k1 = 1;
00033 float k2 = 0.1f;
00034 float k3 = 0.1f;
00035 float Start_slow = 40;
00036 float Start_lock = 0;
00037 float End_slow = 340;
00038 float End_lock = 380;
00039 float Slide_Counts;
00040 float Slide_Revolutions;
00041 float Slide_Angle;
00042 float Slide_Position;
00043 
00044 float Slide_Input_force = 0;
00045 float Slide_Curr_speed = 0;
00046 float Slide_Desired_speed;
00047 float Slide_Delta_speed;
00048 float Slide_Int_delta_speed;
00049 float Slide_Deriv_delta_speed = 0;
00050 float Slide_Prev_delta_speed = 0;
00051 float Slide_PI;
00052 
00053 float Lift_Radius = 10;
00054 float Lift_Multiplier = 1;
00055 float Lift_k1 = 0.2;
00056 float Lift_k2 = 0.05;
00057 float Lift_k3 = 0.01;
00058 float Lift_Start = 0;
00059 float Lift_End = 50;
00060 float Lift_Counts;
00061 float Lift_Revolutions;
00062 float Lift_Angle;
00063 float Lift_Position;
00064 
00065 float Lift_Input_force = 0;
00066 float Lift_Desired_position;
00067 float Lift_Delta_position;
00068 float Lift_Int_delta_position;
00069 float Lift_Deriv_delta_position = 0;
00070 float Lift_Prev_delta_position = 0;
00071 float Lift_PI;
00072 bool Lift_Switch = false;
00073 Timeout Lift_Timeout;
00074 bool Lift_Timeout_Switch = true;
00075 
00076 void Slide_Controller();
00077 void Lift_Controller();
00078 void Ticker_Flag();
00079 
00080 int main()
00081 {
00082     Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
00083     Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f
00084     Controller.attach(&Ticker_Flag,1/Frequency);
00085     pc.baud(9600);
00086     
00087     led.write(1);
00088     Lift_Input_force =  Potmeter.read();
00089     Slide_Input_force = Potmeter2.read();
00090     
00091     notch_50_0.add( &bq03 ).add( &bq04 ).add( &bq05 );
00092     notch_50_1.add( &bq13 ).add( &bq14 ).add( &bq15 );
00093     notch_50_2.add( &bq23 ).add( &bq24 ).add( &bq25 );
00094     high_pass_0.add( &bq06 ).add( &bq07 );
00095     high_pass_1.add( &bq16 ).add( &bq17 );
00096     high_pass_2.add( &bq26 ).add( &bq27 );
00097     low_pass_0.add( &bq9 ).add( &bq10 ).add( &bq11 );
00098     low_pass_1.add( &bq19 ).add( &bq110 ).add( &bq111 );
00099     low_pass_2.add( &bq29 ).add( &bq210 ).add( &bq211 );
00100     
00101     change_state.attach( &calibrate,1);
00102     change_state2.attach( &run,11);
00103     emgSampleTicker.attach( &emgSample, 0.005); //200Hz
00104     
00105 //    treshold = (cali_max-cali_min)*treshold_multiplier;
00106 //    servoTick.attach(&control_servo, 1/Frequency);
00107     ServoPWMpin.period(0.01f);                          // 0.01 second period
00108     
00109     while (true) {
00110  //       pc.printf("\n\r%f", Norm_EMG_0);
00111          
00112         if (go_emgSample == true){
00113                 EMG_filter();
00114         }
00115         
00116         if (Controller_Flag == true){
00117             Slide_Controller();
00118             Lift_Controller();
00119             control_servo(Norm_EMG_0);
00120             
00121             Controller_Flag = false;
00122         }
00123 
00124     }
00125     return 0;
00126 }
00127 
00128 void Ticker_Flag(){
00129     Controller_Flag = true;
00130 }
00131 
00132 void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
00133 
00134     Slide_Counts =      Slide_Encoder.getPulses();
00135     Slide_Revolutions = Slide_Counts /(32*131);
00136     Slide_Angle =       Slide_Revolutions*2*pi;
00137     Slide_Position =    Slide_Angle*Slide_Radius + 135;
00138             
00139     Slide_Desired_speed= (-Norm_EMG_1+Norm_EMG_0)*Slide_Multiplier;
00140     
00141     
00142     if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
00143         Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
00144         
00145     }
00146     if (Slide_Position > End_slow && Slide_Desired_speed < 0){
00147        Slide_Desired_speed *= (End_lock-Slide_Position)/(End_lock-End_slow);
00148     }
00149     
00150     Slide_Prev_delta_speed = Slide_Delta_speed;
00151     Slide_Delta_speed = Slide_Desired_speed-Slide_Curr_speed;                   // P
00152     Slide_Int_delta_speed += Slide_Delta_speed/Frequency;                       // I
00153     if (Slide_Int_delta_speed > 1){Slide_Int_delta_speed = 1;}
00154     if (Slide_Int_delta_speed < -1){Slide_Int_delta_speed = -1;}
00155     Slide_Int_delta_speed *= 1/1.3;
00156     pc.printf("%f - %f - %f \r\n",Norm_EMG_0, Norm_EMG_1, Slide_Delta_speed);
00157     Slide_PI = k1*Slide_Delta_speed + k2*Slide_Int_delta_speed;
00158     if (Slide_PI<0){
00159         SlideMotor_Direction = 0;
00160     }else{
00161         SlideMotor_Direction = 1;
00162     }   
00163     
00164     Motor1_pwm.write(abs(Slide_PI));
00165     //return k1*Delta_speed + k2*Int_delta_speed;
00166 }
00167 
00168 void Lift_Timeout_Return(){
00169         Lift_Timeout_Switch = true;
00170     }
00171 
00172 void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
00173     
00174     Lift_Counts =       Lift_Encoder.getPulses();
00175     Lift_Revolutions =  Lift_Counts /(32*131);
00176     Lift_Angle =        Lift_Revolutions*2*pi;
00177     Lift_Position =     Lift_Angle*Lift_Radius;
00178     
00179     if(Norm_EMG_0 > 0.6 && Lift_Timeout_Switch == true){
00180         //Lift_Switch = !Lift_Switch;
00181         Lift_Timeout_Switch = false;
00182         Lift_Timeout.attach(Lift_Timeout_Return, 1);
00183     }
00184     
00185     Lift_Desired_position = Lift_Switch*150;
00186     //pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
00187     Lift_Prev_delta_position = Lift_Delta_position;                             
00188     Lift_Delta_position = Lift_Desired_position-Lift_Position;             // P
00189     Lift_Int_delta_position += Lift_Delta_position/Frequency;                   // I
00190     
00191     Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency;   //D
00192     if (Lift_Int_delta_position > 1){Lift_Int_delta_position = 1;}
00193     if (Lift_Int_delta_position < -1){Lift_Int_delta_position = -1;}
00194     
00195     Lift_PI = Lift_k1*Lift_Delta_position + 0*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
00196     if (Lift_PI<0){
00197         LiftMotor_Direction = 1;
00198     }else{
00199         LiftMotor_Direction = 0;
00200     }   
00201     
00202     Motor2_pwm.write(abs(Lift_PI));
00203     //return k1*Delta_speed + k2*Int_delta_speed;
00204 }