SMC.cpp

00001 /**
00002  * @file    SMC.cpp
00003  * @brief   Stepper motor control module uses 1/8 microstepping
00004  *
00005  * @author  Jeroen Lodder
00006  * @date        Oktober 2013
00007  *
00008  * @{
00009  */
00010 #include "mbed.h"
00011 #include "smc.h"
00012 #include "CT32B0_PWM.h"
00013 
00014 #define PWM_PERIOD              513     /**< @brief PWM period integer */
00015 #define MIN_STEPTIME_US     100   /**< @brief Minimum time required for one microstep, derived from Hi-side driver latency (in uS) */
00016 
00017 /* Motor Control IO */
00018 DigitalOut HIFET_1(P0_16);  /**< @brief Hi Side FET 1 of bridge 1 */
00019 DigitalOut HIFET_2(P0_17);  /**< @brief Hi Side FET 2 of bridge 1 */
00020 DigitalOut HIFET_3(P0_18);  /**< @brief Hi Side FET 3 of bridge 2 */
00021 DigitalOut HIFET_4(P0_19);  /**< @brief Hi Side FET 4 of bridge 2 */
00022 
00023 /* Motor Control stepcontrol */
00024 Ticker smc;
00025 volatile static int smc_walker      =   0;  /**< @brief Motor control LUT index */
00026 volatile static int smc_dir             = 1;    /**< @brief Director in lookup table (unused) */
00027 volatile static int smc_steps           = -1; /**< @brief Remaining steps */
00028 volatile static int smc_free            = 1;  /**< @brief Argument to free motor when done */
00029 volatile static int smc_abort           = 0;  /**< @brief Set when motor control should quit by itself */
00030 volatile static int smc_steptime    = 0;  /**< @brief Time used to make 1 step */
00031 volatile static int smc_isPaused    = 0;  /**< @brief Boolean for paused state of control */
00032 
00033 /**
00034  * @brief   Initializes Stepper Motor Control
00035  */
00036 void SMC_init(void){
00037     // Hi fet low
00038   HIFET_1 = 0;
00039     HIFET_2 = 0;
00040     HIFET_3 = 0;
00041     HIFET_4 = 0;
00042     // Wait minimum
00043     
00044     // Lo fet low
00045   CT32B0_initpwm(PWM_PERIOD,0);
00046     // Start pwm
00047     CT32B0_start();
00048 }
00049 
00050 /**
00051  * @brief   De-Initializes Stepper Motor Control
00052  */
00053 void SMC_deinit(void){
00054     smc.detach();
00055     CT32B0_deinit(0);
00056     
00057   HIFET_1 = 0;
00058     HIFET_2 = 0;
00059     HIFET_3 = 0;
00060     HIFET_4 = 0;
00061 
00062     smc_walker  =   0;
00063     smc_dir         = 1;
00064     smc_steps   = -1;
00065     smc_free        = 1;
00066   smc_abort = 0;
00067     smc_isPaused = 0;
00068 }
00069 
00070 /**
00071  * @brief   Routine called by interrupt to modify H-Bridge states
00072  */
00073 void SMC_routine(void){
00074     #define i   smc_walker
00075     CT32B0_wait_refresh();
00076     __disable_irq();
00077     // Phase 1 A    
00078     // If sin +, H1->L2
00079     // If sin -, H2->L1
00080     // If direction -1, swap motor A channels, this changes direction.
00081     // Reversing lookup table is not effective
00082     
00083     if(smc_dir > 0){
00084         HIFET_1 = LUT_H1[i];
00085         HIFET_2 = LUT_H2[i];
00086         if(LUT_L1[i] == 0)
00087             CT32B0_set(0, (PWM_PERIOD+1) );
00088         else
00089             CT32B0_set(0, PWM_PERIOD-LUT_L1[i] );
00090         if(LUT_L2[i] == 0)
00091             CT32B0_set(1, (PWM_PERIOD+1) );
00092         else
00093             CT32B0_set(1, PWM_PERIOD-LUT_L2[i] );
00094     }else{
00095         // Reversed for dir -1
00096         HIFET_1 = LUT_H2[i];
00097         HIFET_2 = LUT_H1[i];
00098         if(LUT_L2[i] == 0)
00099             CT32B0_set(0, (PWM_PERIOD+1) );
00100         else
00101             CT32B0_set(0, PWM_PERIOD-LUT_L2[i] );
00102         if(LUT_L1[i] == 0)
00103             CT32B0_set(1, (PWM_PERIOD+1) );
00104         else
00105             CT32B0_set(1, PWM_PERIOD-LUT_L1[i] );
00106     }
00107 
00108     // Phase 1 A    
00109     // If sin +, H1->L2
00110     // If sin -, H2->L1
00111     HIFET_3 = LUT_H3[i]; 
00112     HIFET_4 = LUT_H4[i]; 
00113     if(LUT_L3[i] == 0)
00114         CT32B0_set(2, (PWM_PERIOD+1) );
00115     else
00116         CT32B0_set(2, PWM_PERIOD-LUT_L3[i] );
00117     if(LUT_L4[i] == 0)
00118         CT32B0_set(3, (PWM_PERIOD+1) );
00119     else
00120         CT32B0_set(3, PWM_PERIOD-LUT_L4[i] );
00121     if(i==9)
00122         CT32B0_stage(1);
00123     if(i==24)
00124         CT32B0_stage(0);
00125     CT32B0_reload_mat();    
00126     
00127     /*
00128     volatile static uint8_t hifet[4] = {0,0,0,0};
00129     hifet[0] = HIFET_1;
00130     hifet[1] = HIFET_2;
00131     hifet[2] = HIFET_3;
00132     hifet[3] = HIFET_4;
00133     volatile static uint8_t lofet[4] = {0,0,0,0};
00134     if(LUT_L1[i])   lofet[0] = 1; else lofet[0] = 0;
00135     if(LUT_L2[i])   lofet[1] = 1; else lofet[1] = 0;
00136     if(LUT_L3[i])   lofet[2] = 1; else lofet[2] = 0;
00137     if(LUT_L4[i])   lofet[3] = 1; else lofet[3] = 0;
00138     volatile static uint8_t errfet[4] = {0,0,0,0};
00139     errfet[0] = hifet[0] & lofet[0];
00140     errfet[1] = hifet[1] & lofet[1];
00141     errfet[2] = hifet[2] & lofet[2];
00142     errfet[3] = hifet[3] & lofet[3];
00143     
00144     if( errfet[0] | errfet[1] | errfet[2] | errfet[3] ){
00145         // ILLEGAL MODE
00146     //  smc_abort = 1;
00147          __NOP();
00148     } */
00149     
00150     #undef i
00151     
00152     /* Walk */
00153     smc_walker += 1; //abs(smc_dir);
00154     if(smc_walker > 31)
00155     smc_walker = 0;
00156     //if(smc_walker < 0)
00157     //smc_walker = 31;
00158     /* Coutdown */
00159     if(smc_steps != -1){
00160         if(smc_steps == 0 || smc_abort == 1){
00161             if(smc_free || smc_abort == 1){
00162                 // motor free
00163                 HIFET_1 = 0;
00164                 HIFET_2 = 0;
00165                 HIFET_3 = 0;
00166                 HIFET_4 = 0;
00167                 CT32B0_deinit(0);
00168             }else{
00169                 // motor locked
00170             }
00171             smc.detach();
00172             smc_abort = 0;
00173             smc_isPaused = 0;
00174         }
00175     smc_steps--;
00176     }
00177     __enable_irq();
00178 }
00179 
00180 /**
00181  * @brief   Stepper motor control main command
00182  *
00183  * @param[in]   steps           Number of steps to take
00184  * @param[in]   dir           Direction to step in, 1 or 0
00185  * @param[in]   time_ms   Time to take for these steps
00186  * @param[in]   free        Free or lock motor when done 1 or 0 respectively
00187  * @return          -1 when illegal command or mode  
00188  */
00189 int SMC_step(int steps, uint8_t dir, uint32_t time_ms, uint8_t free){
00190     // steps   = number of microsteps (8 microsteps per full step)
00191     // dir     = -1 or 1
00192     // time_us = completion time in s
00193     uint32_t steptime = (time_ms*1000)/steps;   
00194     // Parameter check
00195     if(smc_steps != -1) return -1;  // Only if motor idle
00196     if(steps < 1) return -1;        // At least one step
00197     if(dir) dir = 1;                    // Round dir to bool
00198     if(free) free = 1;                  // Round free to bool
00199     if(steptime < MIN_STEPTIME_US)  // Verify steptime
00200             return -1;
00201     if(dir == 0) 
00202         smc_dir = -1; 
00203     else 
00204         smc_dir = 1;
00205     // Configure stepper
00206     smc_steps = steps;
00207     smc_free    = free;
00208     smc_steptime = steptime;
00209     smc_isPaused = 0;
00210     // Initiate
00211     SMC_init();             
00212     smc.attach_us(&SMC_routine, smc_steptime);
00213     return 0;   
00214 }
00215 
00216 /**
00217  * @brief   Return 1 of stepper motor control is idle
00218  */
00219 uint32_t SMC_idle(void){
00220     if(smc_steps == -1)
00221         return 1;
00222     else
00223         return 0;
00224 }
00225 
00226 /**
00227  * @brief   Puts motor in brake mode, enable all low-side mosfets
00228  */
00229 void SMC_brake(void){
00230     // Do not brake when active control
00231     if(smc_walker > 0)
00232         return;
00233     // Regular deinit
00234     SMC_deinit();
00235     // Re-enable low side
00236     CT32B0_deinit(1);
00237 }
00238 
00239 /**
00240  * @brief   Puts motor in free mode, disables all mosfets
00241  */
00242 void SMC_free(void){
00243     // Do not free when active control
00244     if(smc_walker > 0)
00245         return;
00246     // Regular deinit
00247     SMC_deinit();
00248     // Re-enable low side
00249     CT32B0_deinit(0);
00250 }
00251 
00252 /**
00253  * @brief   Pause current stepping command
00254  */
00255 void SMC_pause(void){
00256     if( !SMC_idle() ){
00257         smc.detach();
00258         smc_isPaused = 1;
00259     }
00260 }
00261 
00262 /**
00263  * @brief   Continue from pause
00264  */
00265 void SMC_continue(void){
00266     if( !SMC_idle() && smc_isPaused==1 ){
00267         smc.attach_us(&SMC_routine, smc_steptime);
00268         smc_isPaused = 0;
00269     }
00270 }
00271 
00272 /**
00273  * @brief   Return remaining steps, negative if paused
00274  */
00275 int SMC_getState(void){
00276     if( smc_steps < 0 ){
00277         return 0;
00278     }else{
00279       if(smc_isPaused)
00280             return (-1*smc_steps);
00281         else
00282           return (1*smc_steps);
00283     }
00284 }
00285 
00286 /**
00287 * @brief   Egg, open it to find out
00288 */
00289 void SMC_egg(void){
00290 const   uint16_t rr[] = {
00291         627  ,      1045 ,
00292         122  ,      785  ,
00293         887  ,      187  ,
00294         233  ,      166  ,
00295         788  ,      752  ,
00296         1157
00297     };
00298     int rri=0;
00299     // Egg
00300     for(int i=0;i<2; i++){
00301         SMC_step(rr[rri++]  , 1, 685, 1);       // 83 - 987
00302         while( !SMC_idle()) ;       //    -
00303         wait_ms(1);
00304         SMC_step(rr[rri++]  , 1, 965, 1);   // 85 - 1108
00305         while( !SMC_idle()) ;       //    -
00306         wait_ms(1);
00307         SMC_step(rr[rri++]  , 1, 245, 1);   // 78 - 739
00308         while( !SMC_idle()) ;       //    -
00309         wait_ms(1);
00310         SMC_step(rr[rri++]  , 1, 725, 1);   // 85 - 1108
00311         while( !SMC_idle()) ;       //    -
00312         wait_ms(1);
00313         SMC_step(rr[rri++]  , 1, 710, 1);   // 87 - 1244
00314         while( !SMC_idle()) ;       //    -
00315         wait_ms(1);
00316         SMC_step(rr[rri++]  , 1, 125, 1);   // 90 - 1479
00317         while( !SMC_idle()) ;       //    -
00318         wait_ms(1);
00319         SMC_step(rr[rri++]  , 1, 175, 1);   // 88 - 1318
00320         while( !SMC_idle()) ;       //    -
00321         wait_ms(1);
00322         SMC_step(rr[rri++]  , 1, 133, 1);   // 87 - 1244
00323         while( !SMC_idle()) ;       //    -
00324         wait_ms(1);
00325         SMC_step(rr[rri++]  , 1, 860, 1);   // 83 - 987
00326         while( !SMC_idle()) ;       //    -
00327         wait_ms(1);
00328         SMC_step(rr[rri++]  , 1, 695, 1);   // 85 - 1108
00329         while( !SMC_idle()) ;       //    -
00330         wait_ms(1);
00331         SMC_step(rr[rri++]  , 1, 2315, 1); // 78 -  739
00332         while( !SMC_idle()) ;       //    -
00333         wait_ms(1);
00334         rri=0;
00335     }
00336 }
00337 /**
00338  *@}
00339  */
00340 
00341