MotorMove.cpp

00001 #include    "mbed.h"
00002 #include    "DRV8830.h"
00003 #include    "MotorMove.h"
00004 #define SPEED_RATIO  1.0
00005 
00006 //float vol_decel_ini[DECEL_SIZE] = {1,0.75,0.4,0.15,0.05,0};
00007 float vol_decel_ini[DECEL_SIZE] = {-0.5,0,0,0,0,0};
00008 float vol_accel_ini[ACCEL_SIZE] = {1,1,1,1,1,1};
00009 //float vol_accel_ini[ACCEL_SIZE] = {0.2,0.4,0.6,0.8,.9,1.0};
00010 
00011 void MotorMove::up_motor_set(int time_counter, float speed)
00012     {
00013         bflag_up   =1;
00014         bflag_down =0;
00015         start_time_count=time_counter;
00016         accel_count = 0;
00017         decel_count = 0;
00018 
00019         for (int i=0;i<ACCEL_SIZE;i++)
00020         {
00021              vol_accel[i] = -vol_accel_ini[i]*speed/SPEED_RATIO;
00022         }                
00023 
00024         for (int i=0;i<DECEL_SIZE;i++)
00025         {
00026              vol_decel[i] = -vol_decel_ini[i]*speed/SPEED_RATIO;
00027         }
00028     };
00029 
00030 void MotorMove::down_motor_set(int time_counter, float speed)
00031     {
00032         bflag_up   =0;
00033         bflag_down =1;
00034         start_time_count=time_counter;
00035         accel_count = 0;
00036         decel_count = 0;
00037 
00038         for (int i=0;i<ACCEL_SIZE;i++)
00039         {
00040              vol_accel[i] = vol_accel_ini[i]*speed/SPEED_RATIO;
00041         }  
00042         
00043         for (int i=0;i<DECEL_SIZE;i++)
00044         {
00045              vol_decel[i] = vol_decel_ini[i]*speed/SPEED_RATIO;
00046         }
00047     };
00048     
00049     
00050 float MotorMove::ReturnMotorVol(int time_counter,int bflag_downed, int bflag_upped)
00051     {
00052         float speed;
00053         int time;
00054         
00055         static int flag_up_stopping=0;
00056         static int flag_down_stopping=0;
00057         
00058         //Check flag for normal mode or brake mode
00059         //accel_count MODE
00060 
00061         
00062         if (bflag_up==1 && bflag_upped ==1)
00063         {
00064             flag_up_stopping=1;   
00065             //bflag_up==0;
00066         }
00067         else if (bflag_down==1 && bflag_downed ==1)
00068         {
00069            flag_down_stopping=1;
00070            //bflag_down=0;
00071         }
00072 
00073 
00074         if (bflag_up==1 && bflag_upped==0)
00075           {
00076                 if (accel_count<(ACCEL_SIZE-1))
00077                 {
00078                     speed = vol_accel[accel_count];
00079                     accel_count++;
00080                 }
00081                 else
00082                 {
00083                     speed = vol_accel[ACCEL_SIZE-1];    
00084                 }
00085           }
00086         else if(bflag_up==1 && bflag_upped==1)
00087            {
00088                     speed=vol_decel[0];
00089                     bflag_up=0;
00090            }
00091         else if (bflag_down==1 && bflag_downed==0)
00092           {
00093                 if (accel_count<(ACCEL_SIZE-1))
00094                 {
00095                     speed = vol_accel[accel_count];
00096                     accel_count++;
00097                 }
00098                 else
00099                 {
00100                     speed = vol_accel[ACCEL_SIZE-1];    
00101                 }
00102           }
00103         else if(bflag_down==1 && bflag_downed==1)
00104            {
00105                     speed=vol_decel[0];
00106                     bflag_down=0;
00107            }
00108         
00109         
00110         
00111          else
00112          {
00113              speed=0;
00114         }
00115         
00116         /*              
00117         if (bflag_up==1)
00118           {
00119             if  (flag_up_stopping==0)
00120             {
00121                 if (accel_count<(ACCEL_SIZE-1))
00122                 {
00123                     speed = vol_accel[accel_count];
00124                     accel_count++;
00125                 }
00126                 else
00127                 {
00128                     speed = vol_accel[ACCEL_SIZE-1];    
00129                 }
00130             }
00131             else if(flag_up_stopping==1)
00132             {
00133                 if  (decel_count<=(DECEL_SIZE-1))
00134                 {
00135                     speed = vol_decel[decel_count];
00136                     decel_count++;
00137                 }
00138                 else 
00139                 {
00140                 bflag_up=0;
00141                 speed=0;
00142                 flag_up_stopping=0;
00143                 }
00144             }
00145          }
00146         else if (bflag_down==1)
00147           {
00148             if  (flag_down_stopping==0)
00149             {
00150                 if (accel_count<(ACCEL_SIZE-1))
00151                 {
00152                     speed = vol_accel[accel_count];
00153                     accel_count++;
00154                 }
00155                 else
00156                 {
00157                     speed = vol_accel[ACCEL_SIZE-1];    
00158                 }
00159             }
00160             else if(flag_down_stopping==1)
00161             {
00162                 if  (decel_count<=(DECEL_SIZE-1))
00163                 {
00164                     speed = vol_decel[decel_count];
00165                     decel_count++;
00166                 }
00167                 else 
00168                 {
00169                 bflag_down=0;
00170                 speed=0;
00171                 flag_down_stopping=0;
00172                 }
00173             }
00174          }
00175          else
00176          {
00177              speed=0;
00178         }
00179         */
00180                          
00181 
00182         //move motor before push stopping switch 
00183         return speed;      
00184         
00185          
00186     };
00187     
00188   
00189