main.cpp

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 #include "QEI.h"
00004 #include "math.h"
00005 #include "HIDScope.h"
00006 #include "BiQuad.h"
00007  
00008 
00009 ///////////////////////////
00010 ////////Constantes/////////
00011 ///////////////////////////
00012 
00013 double const pi = 3.14159265359;
00014 
00015 ///////////////////////////
00016 // Initialise hardware/////
00017 ///////////////////////////
00018 
00019 MODSERIAL pc(USBTX, USBRX);
00020 //-----------------------------------------------------------
00021 //--------------------All lights-----------------------------
00022 //-----------------------------------------------------------
00023 DigitalOut green(LED_GREEN);
00024 DigitalOut red(LED_RED);
00025 //------------------------------------------------------------
00026 //--------------------All sensors-----------------------------
00027 //------------------------------------------------------------
00028 
00029 //--------------------Encoders--------------------------------
00030 
00031 //Variables
00032 
00033 volatile double Signal = 0.0;
00034 const int SampleFrequency = 400;
00035 const double AnglePerPulse = 2*pi/4200; //Soms doet dit een beetje vaag, dus dit moet wel af en toe uitgetest worden
00036 volatile double Position_L = 0.0; //The position of the left motor in radiants
00037 volatile double Position_R = 0.0; //The position of the right motor in radiants
00038 volatile double Previous_Position_L =0.0; //The previous position of the left motor in radiants
00039 volatile double Previous_Position_R = 0.0; //The previous position of the right motor in radiants
00040 volatile double Speed_L = 0.0; //The speed of the left motor
00041 volatile double Speed_R = 0.0; //The speed of the right motor
00042 
00043 //Defining Pins
00044 QEI Encoder_L (D11, D10,NC, 64);  //D11 is poort A D10 is poort b
00045 QEI Encoder_R (D13, D12,NC, 64); //D 13 is poort A 12 is poort b
00046 
00047 //--------------------AnalogInput---------------------------------
00048 AnalogIn Left(A0); //Motorspeed control Left
00049 AnalogIn Right(A1); //MotorSpeed control Right
00050 AnalogIn Change(A2);//EMG signal for other controls
00051 double EMG_L;
00052 double EMG_R;
00053 double EMG_Change; 
00054 
00055 
00056 //--------------------------------------------------------------
00057 //--------------------All Motors--------------------------------
00058 //--------------------------------------------------------------
00059 volatile int movement = 1,direction_L =1, direction_R =-1; //determining the direction of the motors
00060 
00061 DigitalOut M_L_D(D4); //Richting motor links-> 
00062 //while M_L_D is zero the motor's direction is counterclockwise and the pulses are positive
00063 PwmOut     M_L_S(D5); //Speed motor links
00064 DigitalOut M_R_D(D7); //Richting motor Rechts
00065 PwmOut     M_R_S(D6); //Speed motor Rechts
00066 const int Motor_Frequency = 1000;
00067 
00068 DigitalOut Grap(D9); //Graple active or not;
00069 
00070 //----------------------Lights ---------------------------------4
00071 DigitalOut translation(D0);
00072 DigitalOut grip1(D1);
00073 DigitalOut rotation(D2);
00074 DigitalOut grip2(D3);
00075 
00076 //--------------------------------------------------------------
00077 //-----------------------Functions------------------------------
00078 //--------------------------------------------------------------
00079 
00080 //-----------------------on of sitch of the sytem-----------------------------
00081 
00082 
00083 
00084 AnalogIn Active(A5);
00085 
00086     
00087 
00088 //--------------------------------Sampling------------------------------------------   
00089 
00090 Ticker Tick_Sample;// ticker for data sampling
00091 
00092 bool Sample_Flag = false;
00093 
00094 void Set_Sample_Flag(){
00095     Sample_Flag = true;
00096     }
00097 
00098 void Sample(){
00099     
00100 //This function reads out the position and the speed of the motors in radiants 
00101 //Aand stores them in the values Speed and Position
00102 
00103 //Position in Radials:
00104     Previous_Position_L = Position_L;
00105     Previous_Position_R = Position_R;
00106     Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
00107     Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;
00108 //Speed in RadPers second
00109     Speed_L = (Position_L-Previous_Position_L)*SampleFrequency;
00110     Speed_R = (Position_R-Previous_Position_R)*SampleFrequency;
00111 
00112                Encoder_L.reset();
00113                Encoder_R.reset();
00114                
00115     }
00116     
00117     bool Controller_Flag = false;
00118     
00119     Ticker Tick_Controller;
00120     
00121     void Set_controller_Flag(){
00122         Controller_Flag = true;
00123         }
00124 
00125 
00126     
00127 //-------------------------------PI ---------------------------------
00128     
00129 double PI(double e, const double Kp, const double Ki, double Sf,
00130  double &e_int){
00131     //This function is a PID controller that returns the errorsignal for the plant
00132 
00133 
00134     e_int = e_int + Sf * e;
00135     // PID
00136     return Kp*e + Ki*e_int ;
00137     }
00138  
00139  //---------------------------Controller---------------------------------------  
00140  
00141  
00142 
00143  // Controller gains (motor1−Kp,−Ki,...)
00144  const double Kpm= 0.1, Kim = 0.005;
00145  double e_L_int = 0;
00146  
00147     double Controller_L(int direction, double signal, double reference ){
00148         //This funcition returns a value that will be sent to drive the motor
00149         // - Motor_Direction_Input is the Digital in of the motor
00150         
00151         // 1 for dirrection means that a clockwise rotation wil be regarded as positive
00152         // and -1 for direction means the opposite
00153         
00154         //PID controller
00155        double Speed_Set = PI( reference-signal*direction,Kpm, Kim, SampleFrequency, e_L_int );
00156     //Determining rotational direction of the motor
00157            if ((reference-signal*direction >= 0)){
00158                  M_L_D = 1;
00159             } else {
00160                  M_L_D = 0;
00161              }
00162             if (fabs(Speed_Set)< 1){
00163                 if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
00164                     return  0;
00165                     } else {
00166             return fabs(Speed_Set);
00167             }
00168             } else {
00169                 return 1;            
00170             }
00171             }
00172       double e_R_int = 0;   
00173 double Controller_R(int direction, double signal, double reference ){
00174         //This funcition returns a value that will be sent to drive the motor
00175         // - Motor_Direction_Input is the Digital in of the motor
00176         
00177         // 1 for dirrection means that a clockwise rotation wil be regarded as positive
00178         // and -1 for direction means the opposite
00179         
00180         //PI controller
00181        double Speed_Set = PI( reference-signal*direction,Kpm, Kim,  SampleFrequency, e_R_int);
00182     //Determining rotational direction of the motor
00183             if ((reference-signal*direction >= 0)){
00184                  M_R_D = 0;
00185             } else {
00186                  M_R_D = 1;
00187              }
00188             if (fabs(Speed_Set)< 1){
00189                 if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
00190                     return  0;
00191                     } else {
00192             return fabs(Speed_Set);
00193             }
00194             } else {
00195                 return 1;
00196             
00197             }
00198         }
00199 //-----------------------------Change_Movement----------------------------------
00200 // This function changes the movement type of the robot as a consequence of the input by EMG
00201 // Or when the EMG signal is hold for 1.5S The grapler wil activate
00202 Ticker Tick_Movement;
00203 int check = 0;
00204 
00205 bool Movement_Flag = false;
00206 void Set_movement_Flag(){
00207     Movement_Flag = true;
00208 }
00209 
00210 void Change_Movement(){
00211    if (Movement_Flag == true){
00212        if (EMG_Change > 0.5f ){
00213             check += 1;
00214                if (check <= 6 and check > 1 ){
00215                     grip1 = 1;
00216                 }
00217                if (check > 6){
00218                      grip2 =1;
00219                 }
00220             } else {
00221                 //Hold the signal shorter than 1.5 seconds
00222                 if (check <= 6 and check > 1 ){
00223                     movement = !movement;
00224                     grip1 = 1;
00225                     }
00226                 // Hold the signal longer than 1.5 seconds
00227                 if (check > 6){
00228                      grip2 =1;
00229                      Grap = !Grap;
00230                      
00231                     }
00232                    
00233                 grip2 = 0;
00234                 grip1 =0;
00235                 check = 0;
00236                     
00237         }
00238         Movement_Flag = false;
00239         }
00240     }
00241     //-----------------------------EMG Signal determinator----------------------
00242 BiQuadChain bqc_L;
00243 BiQuadChain bqc_R;
00244 BiQuadChain bqc_Change;
00245 BiQuad bq1_R( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
00246 BiQuad bq2_R( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
00247 BiQuad bq3_R( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
00248 
00249 BiQuad bq1_L( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
00250 BiQuad bq2_L( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
00251 BiQuad bq3_L( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
00252 
00253 
00254 BiQuad bq1_Change( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
00255 BiQuad bq2_Change( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
00256 BiQuad bq3_Change( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
00257 //HIDScope scope(5); // Sending two sets of data
00258 HIDScope scope(3);
00259 void getEMG(){
00260      
00261  
00262     EMG_Change = bqc_Change.step(Change.read());
00263     EMG_Change = fabs(EMG_Change);
00264     EMG_Change = bq3_Change.step(EMG_Change)*30;
00265          scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope     
00266         //scope.send();           //        scope.set(4,Change.read());     
00267      if (EMG_Change < 0.4){
00268         EMG_Change=0;
00269     } else {
00270         EMG_Change=1.0;
00271     }
00272            
00273             
00274     EMG_L =   bqc_L.step(Left.read());
00275     EMG_L= fabs(EMG_L);
00276     EMG_L= bq3_L.step( EMG_L)*30;
00277                     scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope  
00278       
00279     if (EMG_L < 0.5){
00280         EMG_L=0;
00281     } else {
00282         //EMG_L=1.0;
00283     }
00284     
00285                
00286   
00287     EMG_R =  bqc_R.step(Right.read());
00288     EMG_R= fabs(EMG_R);
00289     EMG_R= bq3_R.step( EMG_R)*30;        
00290          scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope                  
00291     if (EMG_R < 0.5){
00292         EMG_R=0;
00293     } else {
00294         //EMG_R= 1.0;
00295     }
00296     scope.send();
00297 }
00298 //------------------------------------------------------Postition reset----------------------------------
00299 //After the robot has been set in it's equlibrium state, this data is used to prevent the robot of destroying itself.
00300 
00301 void Position_reset(){
00302     Position_R =0;
00303     Position_L =0;
00304     }
00305 
00306 //-------------------------------------------------------Limit-------------------------------------
00307 //calculates the angele of the robot with for its limit, and checks of its limit has been reached
00308     int config_count = 0;
00309     double max_angle = 0;
00310     double min_angle = 0.5;
00311 int Limit(){
00312     double angle=Position_R-Position_L; //R moves the left arm and right moves the right arm
00313     int limit = 0;
00314     if (angle < min_angle){ //limit angle of -13 degrees translated to the motor -13*2*pi*136/(33*360)
00315             limit = 1;
00316         } 
00317         if (angle > max_angle){ //limit angle of 15 degrees tranlated to the motor 2*15*2*pi*136/(33*360) 
00318             limit = 2;
00319         }
00320       return limit;
00321 
00322         
00323     }
00324 
00325 
00326 
00327 bool Configure_Flag = true;
00328 
00329     
00330 /////////////-----------------------Booting---------------------------------------=
00331 void Boot(){
00332   
00333   //Fucnction that initializes variables that have to be initalized within the main of the script
00334   //And does the same thing for functions like encoder reset
00335     pc.baud(115200);
00336     pc.printf("\r\n**BOARD RESET**\r\n");
00337     
00338     M_L_S.period(1.0/Motor_Frequency);
00339     M_L_S = 0.0;
00340     M_R_S.period(1.0/Motor_Frequency);
00341     M_R_S= 0.0;
00342                        rotation = 0;
00343                    translation = 1;
00344     Encoder_L.reset();
00345     Encoder_R.reset();
00346     
00347     Grap = 0;
00348     grip1 = 0;
00349     
00350     bqc_L.add( &bq1_L ).add( &bq2_L );
00351     bqc_R.add(&bq1_R).add(&bq2_R);
00352     bqc_Change.add(&bq1_Change).add(&bq2_Change);
00353     Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency);   
00354     Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
00355     Tick_Movement.attach(Set_movement_Flag, 0.25);
00356     }
00357 
00358     
00359     
00360 int main()
00361 {
00362 //---------------------Setting constants and system booting---------------------
00363    
00364     Boot();
00365 
00366     //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
00367     //\/\/\/\/\/\/\/\/\/\/\/\/\/\Main Loop for program\/\/\/\/\/\/\/\/\/\/\/\/\/
00368     //\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/  
00369     while (true) { 
00370        if (Sample_Flag == true){
00371 
00372             Sample();
00373             getEMG();
00374               //  EMG_Change = getEMG(  bqc.step(Change.read()));
00375             Signal = pi*(EMG_R-EMG_L);
00376 
00377             
00378             //Signal = pi*(Right.read()-Left.read());
00379             //EMG_Change = Change.read();
00380                // scope.set(3,Signal);//------------------------------------------------------------scope
00381 
00382                   //       scope.send();
00383             Sample_Flag = false;
00384             }   
00385             //-------------------------------------------- Configureren-------------------------------------         
00386             if (Configure_Flag == true){
00387                 
00388                  if (config_count < 20){   
00389                 Signal = pi;
00390                 } else {
00391                     Signal = -pi;
00392                     }
00393                  if (config_count == 19){ 
00394                   Position_reset();
00395                   
00396                  }
00397                  if (config_count > 40){
00398                      Configure_Flag = false;
00399                      max_angle = Position_R-Position_L-0.5;
00400                      }
00401                  
00402                 
00403                 M_L_S = 0.02*Controller_L(direction_L,Signal,Speed_L);
00404                 M_R_S = 0.02*Controller_R(direction_R,Signal,Speed_R);
00405 
00406                
00407                 //Signal = -pi;
00408                // M_L_S = 0.03*Controller_L(direction_L,Signal,Speed_L);
00409                // M_R_S = 0.03*Controller_R(direction_R,Signal,Speed_R);
00410 
00411 
00412                 Grap = 0;
00413                 if (Movement_Flag == true){
00414                 rotation = !rotation;
00415                 translation =!translation;
00416 
00417                 grip1 = !grip1;
00418                 grip2 = !grip2;
00419                 config_count ++;
00420                 Movement_Flag= false;
00421                 }
00422             } else {
00423     //Main statement that states if the system is active or not  
00424         if (Active.read() > 0.5){
00425             green = 0;
00426             red = 1;
00427             Change_Movement();
00428 
00429             if (Controller_Flag == true){
00430                switch (movement) {
00431                    case 0:
00432                    //Clockwise rotation of the robot
00433                    direction_L = 1;
00434                    direction_R = 1;
00435                    rotation = 1;
00436                    translation = 0;
00437                    Signal = Signal*2;
00438                     M_L_S = Controller_L(direction_L,Signal,Speed_L);
00439                     M_R_S = Controller_R(direction_R,Signal,Speed_R);
00440                    break;
00441                    
00442                    case 1:
00443                    //Radial movement outwards for a positive value
00444                    direction_L = 1;
00445                    direction_R = -1;
00446                    rotation = 0;
00447                    translation = 1;
00448                    
00449                    switch (Limit()){ //making sure the limits are not passed
00450                        case 0: //case the robot is in its working space
00451                              M_L_S = Controller_L(direction_L,Signal,Speed_L);
00452                              M_R_S = Controller_R(direction_R,Signal,Speed_R);
00453                        break;
00454                        case 1: //case the robot has reached it's lowest limit
00455                             if (Signal < 0){
00456                                 M_L_S = Controller_L(direction_L,Signal,Speed_L);
00457                                 M_R_S = Controller_R(direction_R,Signal,Speed_R);
00458                             } else {
00459                                 M_L_S = 0;
00460                                 M_R_S = 0;
00461                                 }
00462                        break;
00463                        case 2: //case the robot has reachted its highest limit
00464                             if (Signal > 0){
00465                                 M_L_S = Controller_L(direction_L,Signal,Speed_L);
00466                                 M_R_S = Controller_R(direction_R,Signal,Speed_R);
00467                             } else {
00468                                 M_L_S = 0;
00469                                 M_R_S = 0;
00470                                 }
00471                        break; 
00472                        }
00473                    break;
00474                    }
00475 
00476                Controller_Flag = false;
00477             }
00478         } else {
00479             if (Active.read() < 0.5){
00480             green = 1;
00481             red = 0;
00482             M_R_S = 0;
00483             M_L_S = 0;
00484             Grap=0;
00485             }
00486         }       
00487     }
00488 }
00489 }
00490