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 extra;
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 const float Frequency = 30;
00028 const float Frequency_PWM = 10000;
00029 
00030 float Slide_Radius = 12.5;
00031 float Slide_Multiplier = 1;
00032 float k1 = 1;
00033 float k2 = 0.01f;
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 
00073 float NomNom;
00074 
00075 void Slide_Controller();
00076 void Lift_Controller();
00077 void Ticker_Flag();
00078 
00079 int main()
00080 {
00081     Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
00082     Motor2_pwm.period(1.0/Frequency_PWM);//T=1/f
00083     Controller.attach(&Ticker_Flag,1/Frequency);
00084     pc.baud(9600);
00085     
00086     led.write(1);
00087     Lift_Input_force =  Potmeter.read();
00088     Slide_Input_force = Potmeter2.read();
00089     
00090     extra.add( &bq0 ).add( &bq1 ).add( &bq2 );
00091     notch_50.add( &bq3 ).add( &bq4 ).add( &bq5 );
00092     high_pass.add( &bq6 ).add( &bq7 );
00093     low_pass.add( &bq9 ).add( &bq10 ).add( &bq11 );
00094     
00095     change_state.attach( &calibrate,1);
00096     change_state2.attach( &run,11);
00097     emgSampleTicker.attach( &emgSample, 0.005); //200Hz
00098     
00099 //    treshold = (cali_max-cali_min)*treshold_multiplier;
00100 //    servoTick.attach(&control_servo, 1/Frequency);
00101     ServoPWMpin.period(0.01f);                          // 0.01 second period
00102     
00103     while (true) {
00104  //       pc.printf("\n\r%f", Norm_EMG_0);
00105          
00106         if (go_emgSample == true){
00107                 EMG_filter();
00108         }
00109         
00110         if (Controller_Flag == true){
00111             Slide_Controller();
00112             Lift_Controller();
00113             control_servo(Norm_EMG_0);
00114             
00115             Controller_Flag = false;
00116         }
00117 
00118     }
00119     return 0;
00120 }
00121 
00122 void Ticker_Flag(){
00123     Controller_Flag = true;
00124 }
00125 
00126 void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
00127 
00128     Slide_Counts =      Slide_Encoder.getPulses();
00129     Slide_Revolutions = Slide_Counts /(32*131);
00130     Slide_Angle =       Slide_Revolutions*2*pi;
00131     Slide_Position =    Slide_Angle*Slide_Radius + 135;
00132             
00133     Slide_Desired_speed= (-0)*Slide_Multiplier;
00134     
00135     
00136     if (Slide_Position < Start_slow && Slide_Desired_speed > 0){
00137         Slide_Desired_speed *= (Slide_Position-Start_lock)/(Start_slow-Start_lock);
00138         
00139     }
00140     if (Slide_Position > End_slow && Slide_Desired_speed < 0){
00141        Slide_Desired_speed *= (End_lock-Slide_Position)/(End_lock-End_slow);
00142     }
00143     
00144     Slide_Prev_delta_speed = Slide_Delta_speed;
00145     Slide_Delta_speed = Slide_Desired_speed-Slide_Curr_speed;                   // P
00146     Slide_Int_delta_speed += Slide_Delta_speed/Frequency;                       // I
00147     if (Slide_Int_delta_speed > 1){Slide_Int_delta_speed = 1;}
00148     if (Slide_Int_delta_speed < -1){Slide_Int_delta_speed = -1;}
00149     Slide_Int_delta_speed *= 1/1.3;
00150     //pc.printf("\r\n%f - %f", Slide_Int_delta_speed, Slide_Delta_speed);
00151     Slide_PI = k1*Slide_Delta_speed + 0*Slide_Int_delta_speed;
00152     if (Slide_PI<0){
00153         SlideMotor_Direction = 0;
00154     }else{
00155         SlideMotor_Direction = 1;
00156     }   
00157     
00158     Motor1_pwm.write(abs(Slide_PI));
00159     //return k1*Delta_speed + k2*Int_delta_speed;
00160 }
00161 
00162 void Lift_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
00163     
00164     Lift_Counts =       Lift_Encoder.getPulses();
00165     Lift_Revolutions =  Lift_Counts /(32*131);
00166     Lift_Angle =        Lift_Revolutions*2*pi;
00167     Lift_Position =     Lift_Angle*Lift_Radius;
00168     
00169     NomNom = extra.step(Norm_EMG_0);
00170     
00171     Lift_Desired_position = (-NomNom)*40*Lift_Multiplier;
00172     //pc.printf("\n\r%f - %f", Lift_Desired_position, Lift_Position);
00173     Lift_Prev_delta_position = Lift_Delta_position;                             
00174     Lift_Delta_position = Lift_Desired_position-Lift_Position;             // P
00175     Lift_Int_delta_position += Lift_Delta_position/Frequency;                   // I
00176     
00177     Lift_Deriv_delta_position = (Lift_Delta_position-Lift_Prev_delta_position)*Frequency;   //D
00178     if (Lift_Int_delta_position > 1){Lift_Int_delta_position = 1;}
00179     if (Lift_Int_delta_position < -1){Lift_Int_delta_position = -1;}
00180     
00181     //fixed tha print
00182     pc.printf("%d, %d\r\n",Norm_EMG_0, NomNom);
00183     Lift_PI = Lift_k1*Lift_Delta_position + 0*Lift_Int_delta_position + Lift_k3*Lift_Deriv_delta_position;
00184     if (Lift_PI<0){
00185         LiftMotor_Direction = 1;
00186     }else{
00187         LiftMotor_Direction = 0;
00188     }   
00189     
00190     Motor2_pwm.write(abs(Lift_PI));
00191     //return k1*Delta_speed + k2*Int_delta_speed;
00192 }