motorControl.cpp

00001 /*
00002 ############################################
00003 ##           motorControl v0.1 Library    ##
00004 ##          created by Peter Ilsoee       ##
00005 ############################################
00006         ---- piniels@gmail.com -----
00007 This library was made for 4-Phase Stepper Motors
00008 I don't take any resposability for the damage caused to your equipment.
00009 
00010 */
00011 
00012 #include "motorControl.h"
00013 
00014 #include "mbed.h"
00015 #include "globalt.h"
00016 
00017 
00018 SPI spi(PTE1, PTE3, PTE2); // mosi, miso, sclk
00019 DigitalOut cs(PTE4);
00020 //struct for holding run register information
00021 
00022 //GPIO
00023 DigitalOut valve(PTD0);
00024 DigitalIn centerInput(PTD4);
00025 
00026 //Ticker for running at a specified speed
00027 Ticker runTickMotorControl;
00028 
00029 
00030 
00031 
00032 
00033 /**
00034  * ! @brief Construct including the pins for "manual(not SPI cmd)" control of A4980
00035  * @param step Step logic input. Motor advances on rising edge. Filtered input with hysteresis.
00036  * @param dir Direction logic input. Direction changes on the next STEP rising edge. When high, the Phase Angle Number is increased
00037  *            on the rising edge of STEP. Has no effect when using the serial
00038  *            interface. Filtered input with hysteresis.
00039  * @param ms1 Microstep resolution select input.
00040  * @param ms0 Microstep resolution select input.
00041  * @param enable Controls activity of bridge outputs. When held low, deactivates the outputs, that is, turns off all output bridge FETs.
00042  *               Internal logic continues to follow input commands.
00043  * @param reset Resets faults when pulsed low. Forces low-power shutdown(sleep) when held low for more than the Reset Shutdown
00044  *              Width, tRSD . Can be pulled to VBB with 30 kΩ resistor.
00045  * @param diag  Diagnostic output. Function selected via the serial interface,
00046  *              setting Configuration Register 1. Default is Fault output.
00047  */
00048 motorControl::motorControl(int numberOfSteps, PinName step, PinName dir, PinName ms1, PinName ms0, PinName enable, PinName reset, PinName diag): _Number_OF_STEPS(numberOfSteps), _STEP(step), _DIR(dir), _MS1(ms1), _MS0(ms0), _ENABLE(enable), _RESET(reset), _DIAG(diag){
00049 
00050     this->_RESET = 0;
00051     this->_STEP = 0; // duty cycle on 50 %
00052     this->_DIR = 0;
00053     this->_MS1 = 0;
00054     this->_MS0 = 0;
00055     this->_ENABLE = 0;
00056     //Setting default values to run regi
00057     this->_REGI_RUN.B.sc = 0x1; //1 steps positive value
00058     this->_REGI_RUN.B.dcy = 0x1; // bit7-6 Decay mode selection 00=Slow, *01=Mixed-PFD fixed, 10=Mixed-PFD auto, 11=Fast
00059     this->_REGI_RUN.B.brk = 0x0; // bit8 Brake enable *0=Normal operation 1=Brake active
00060     this->_REGI_RUN.B.slew = 0x1; // bit9 Slew rate control 0=Disable *1=Enable
00061     this->_REGI_RUN.B.hlr = 0x0; // bit10. Selects slow decay and brake recirculation path *0=High side, 1=Low side
00062     this->_REGI_RUN.B.ol = 0x1; // bit12-11. Open load current threshold as a percentage of maximum current defined by ISMAX and MXI[1..0] 00=20%, *01=30%, 10=40%, 11=50%
00063     this->_REGI_RUN.B.en = 1; // bit13. Phase current enable OR with ENABLE pin *0=Output bridges disabled if ENABLE pin = 0, 1=Output bridges enabled
00064     this->_REGI_RUN.B.addr = 0x2; // bit15-14 Address run = 0b10
00065     //Setting default value to control regi. 0
00066     this->_REGI_0.B.pwm = 0x0;
00067     this->_REGI_0.B.tofFrq = 0x6;
00068     this->_REGI_0.B.tbk = 0x1;
00069     this->_REGI_0.B.pfd = 0x4;
00070     this->_REGI_0.B.mx = 0x03; // bit10-9. Max phase current as a percentage of I(SMAX) 00=25% 01=50% 10=75% *11=100%
00071     this->_REGI_0.B.ms = 0x0;
00072     this->_REGI_0.B.syr = 0x1;
00073     this->_REGI_0.B.addr = 0x0;
00074     //Setting default value to control regi. 1
00075     this->_REGI_1.B.diag = 0x0;
00076     this->_REGI_1.B.cd = 0x8;
00077     this->_REGI_1.B.notInUse = 0x0;
00078     this->_REGI_1.B.tsc = 0x2;
00079     this->_REGI_1.B.osc = 0x0;
00080     this->_REGI_1.B.addr = 0x1;
00081     wait(0.1);
00082     this->_RESET = 1;
00083     this->_Step_Counter = 0;
00084     
00085     
00086 
00087 }
00088 
00089 
00090 void motorControl::initSpi() { // rotate the motor 1 step anticlockwise
00091 cs = 1;
00092 spi.format(8,3);
00093 spi.frequency(1000000);
00094 
00095 }
00096 
00097 /**
00098  * ! @brief set the step counter
00099  * @param value an int counting the steps
00100  */
00101 void motorControl::setStepCount(int value)
00102 {
00103     this->_Step_Counter = value;
00104     
00105 }
00106 
00107 /**
00108  * ! @brief get the step counter
00109  * @return an int counting the steps
00110  */
00111 int motorControl::getStepCount()
00112 {
00113     return this->_Step_Counter;
00114     
00115 }
00116 
00117 /**
00118  * ! @brief set configuration and control register 0
00119  * @param t_config_control_regi_0 an union with represents some seetings (see datasheet A4980 data sheet)
00120  */
00121 int motorControl::setConfigRegi0(t_config_control_regi_0 value)
00122 {
00123     this->_REGI_0 = value;
00124     cs = 0;
00125     int response = (spi.write(value.I >> 8)) << 8;
00126     response |= spi.write(value.I & 0xff);
00127     cs = 1;
00128     return response;
00129 }
00130 
00131 /**
00132  * ! @brief get configuration and control register 0
00133  * @return t_config_control_regi_0 an union with represents some seetings (see datasheet A4980 data sheet)
00134  */
00135 t_config_control_regi_0 motorControl::getConfigRegi0()
00136 {
00137     return this->_REGI_0;
00138 }
00139 
00140 /**
00141  * ! @brief open or close the GPIO controlling the valve
00142  * @parm openTrue Boolean true for open
00143  */
00144 void motorControl::openCloseValve(bool openTrue)
00145 {
00146     if(openTrue)
00147     {
00148         valve = 0;
00149     }
00150     else
00151     {
00152         valve = 1;
00153     }
00154 }
00155 
00156 /**
00157  * ! @brief set configuration and control register 1
00158  * @param t_config_control_regi_1 an union with represents some seetings (see datasheet A4980 data sheet)
00159  */
00160 int motorControl::setConfigRegi1(t_config_control_regi_1 value)
00161 {
00162     this->_REGI_1 = value;
00163     cs = 0;
00164     int response = (spi.write(value.I >> 8)) << 8;
00165     response |= spi.write(value.I & 0xff);
00166     cs = 1;
00167     return response;
00168 }
00169 
00170 /**
00171  * ! @brief set micro step bits
00172  * @param int from 0 to 3
00173  */
00174 void motorControl::setMicroSteps(int value)
00175 {
00176     printf("Value for microsteps: %i",value);
00177     printf("\r\n");
00178     this->_MS1 = (value & 0x2) >> 1;
00179     this->_MS0 = value & 0x1;
00180     
00181 }
00182 
00183 /**
00184  * ! @brief get micro step bits
00185  * @param int from 0 to 3
00186  */
00187 int motorControl::getMicroSteps()
00188 {
00189     return this->_MS1 << 1 | this->_MS0;
00190     
00191 }
00192 
00193 
00194 /**
00195  * ! @brief get configuration and control register 1
00196  * @return t_config_control_regi_1 an union with represents some seetings (see datasheet A4980 data sheet)
00197  */
00198 t_config_control_regi_1 motorControl::getConfigRegi1()
00199 {
00200     return this->_REGI_1;
00201 }
00202 
00203 /**
00204  * ! @brief set configuration and run register
00205  * @param t_run_regi an union with represents some seetings (see datasheet A4980 data sheet)
00206  */
00207 int motorControl::setRunRegi(t_run_regi value)
00208 {
00209     this->_REGI_RUN = value;
00210     cs = 0;
00211     int response = (spi.write(value.I >> 8)) << 8;
00212     response |= spi.write(value.I & 0xff);
00213     cs = 1;
00214     return response;
00215 }
00216 
00217 /**
00218  * ! @brief get run control register
00219  * @return t_run_regi an union with represents some seetings (see datasheet A4980 data sheet)
00220  */
00221 t_run_regi motorControl::getRunRegi()
00222 {
00223     return this->_REGI_RUN;
00224 }
00225 
00226 /**
00227  * ! @brief Run at a specified speed, none blocking
00228  * @param startStop If true start moter if false stop motor
00229  * @param whatSpeed speed in RPM
00230  * @param direction if 1 then postive phase, if 0 negative phase
00231  */
00232 // 
00233 void motorControl::runStepperAtSpeed(bool startStop, int whatSpeed, int direction)
00234 {
00235     this->_DIR = direction;
00236     this->step_delay = calculateStepDelay(whatSpeed);
00237     
00238     if(this->step_delay > MAX_STEP_DELAY)
00239     {
00240         this->step_delay = MAX_STEP_DELAY;
00241     }
00242     else if(this->step_delay < MIN_STEP_DELAY)
00243     {
00244         this->step_delay = MIN_STEP_DELAY;
00245     }
00246     
00247     
00248     
00249     if(startStop)
00250     {
00251         this->_ENABLE = 1;
00252         _STEP = 0.5; //duty cycle 50%
00253         _STEP.period_us((int) this->step_delay);
00254         
00255     }
00256     else
00257     {
00258         _STEP = 0; //Duty-cycle 0
00259         _ENABLE = 0;
00260         _STEP = 0;
00261         
00262     }
00263 }
00264 
00265 void motorControl::doRamp(int numberOfRampsteps,float endDelay)
00266 {
00267     //Test do ramping
00268     
00269     int delay_us_start = 10000;
00270     int diff_delay_us = delay_us_start - endDelay;
00271     int ramping_step_us = diff_delay_us/numberOfRampsteps;
00272     int delay_ramp = 0;
00273      
00274     
00275     for(int i = 0;i < numberOfRampsteps; i++)
00276     {
00277            delay_ramp = delay_us_start - (i*ramping_step_us);
00278            printf("Ramping Delay is in  us: %i", delay_ramp);
00279            printf("\r\n");
00280            _STEP.period_us(delay_ramp);
00281            wait_us(delay_ramp);
00282     }
00283 }
00284 
00285 float motorControl::calculateStepDelay(int whatSpeed)
00286 {
00287    if(whatSpeed != 0)
00288     {
00289         this->step_delay = 60L * 1000000L / ((float)this->_Number_OF_STEPS * (float)whatSpeed);
00290         int microStep = getMicroSteps();
00291         if(microStep > 0)
00292         {
00293            if(microStep == 1)
00294            {
00295             this->step_delay = this->step_delay/2;  
00296             }
00297             else if(microStep == 2)
00298             {
00299                 this->step_delay = this->step_delay/4;  
00300             }
00301             else
00302             {
00303                 this->step_delay = this->step_delay/16;  
00304             }
00305            
00306         }
00307         
00308     }
00309     else
00310     {
00311         this->step_delay = 60L * 1000L / (float) this->_Number_OF_STEPS / (float) 1;
00312     }
00313     
00314     return this->step_delay;
00315 }
00316 
00317 // tick event handler for running the motor at a specified speed
00318 void motorControl::runMotor(void)
00319 {
00320     setRunRegi(this->_REGI_RUN); //1 steps positive value
00321     if(this->_Bool_Direction)
00322     {
00323         this->_Step_Counter++;
00324     }
00325     else
00326     {
00327         this->_Step_Counter--;
00328     }
00329     
00330     
00331     
00332 }
00333 
00334  void motorControl::goToZeroPosition(int whatSpeed)
00335  {
00336     float step_delay = (60 / (float)this->_Number_OF_STEPS / (float)whatSpeed);
00337     bool runMotorBool = true;
00338     bool runMotorBack = false;
00339     openCloseValve(true);
00340     
00341     while(runMotorBool)//this->_Step_Counter != 0 || centerInput )
00342     {
00343         this->_REGI_RUN.B.en = 1;
00344         if(whatSpeed > 0)
00345         {
00346             this->_Bool_Direction = true;
00347             this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
00348             runMotor();
00349             wait(step_delay);
00350             if(centerInput)
00351             {
00352                 runMotorBool = false;
00353                 this->_Step_Counter = 0;
00354                 
00355             }
00356         }
00357         else
00358         {
00359             this->_Bool_Direction = false;
00360             this->_REGI_RUN.B.sc = 0x30; // 1 steps positive value
00361             runMotor();
00362             wait(-1*step_delay);
00363             if(centerInput)
00364             {
00365                 runMotorBack = true;
00366             }
00367             else if(runMotorBack && !centerInput)
00368             {
00369                 runMotorBool = false;
00370                 this->_Step_Counter = 0;
00371             }
00372             
00373         }
00374        
00375         
00376     } 
00377     this->_REGI_RUN.B.en = 0;    
00378 
00379 }
00380 
00381 // Run at a specified speed, no blocking
00382 void motorControl::runStepperAtSpeedWithSPI(bool startStop, int whatSpeed, int direction)
00383 {
00384     
00385     
00386     if(whatSpeed != 0)
00387     {
00388         this->step_delay = (60L * 1000000L / this->_Number_OF_STEPS / whatSpeed);
00389         
00390     }
00391     else
00392     {
00393         this->step_delay = 60L * 1000000L / this->_Number_OF_STEPS / 1;
00394         
00395     }
00396     //pc.printf("Step dealy: %d",this->step_delay);
00397     //pc.printf("\r\n");
00398     if(direction)
00399     {
00400         this->_Bool_Direction = true;
00401         if(whatSpeed > 50)//HACK: for testing micro step
00402         {
00403             this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
00404         }
00405         else
00406         {
00407             this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
00408             //this->_REGI_RUN.B.sc = 0x4; // 1/8 steps positive value
00409             //this->step_delay = this->step_delay/4;
00410             printf("Step dealy micro step: %d",this->step_delay);
00411             printf("\r\n");
00412         }
00413         
00414     }
00415     else
00416     {
00417         this->_Bool_Direction = false;;
00418         
00419         if(whatSpeed > 50) //HACK: for testing micro step
00420         {
00421             this->_REGI_RUN.B.sc = 0x30; // 1 steps negative value
00422         }
00423         else
00424         {
00425             this->_REGI_RUN.B.sc = 0x30; // 1 steps negative value
00426             //this->_REGI_RUN.B.sc = 0x3C; // 1/16 steps negative value
00427             //this->step_delay = this->step_delay/4;
00428             printf("Step dealy micro step: %d",this->step_delay);
00429             printf("\r\n");
00430         }
00431         //
00432         
00433     }
00434 
00435     if(startStop)
00436     {
00437         this->_REGI_RUN.B.en = 1;
00438         runTickMotorControl.attach_us(this, &motorControl::runMotor, (this->step_delay)); // the address of the function to be attached
00439     }
00440     else
00441     {
00442         this->_REGI_RUN.B.en = 0;
00443         setRunRegi(this->_REGI_RUN);
00444         runTickMotorControl.detach();
00445         
00446     }
00447 }
00448 
00449 
00450 
00451 void motorControl::step(int num_steps, int delay, bool direction) {// steper function: number of steps, direction (0- right, 1- left), 
00452 
00453     this->_REGI_RUN.B.en = 1;
00454 
00455     
00456     
00457     if(direction)
00458     {
00459         if(getMicroSteps() == 0)
00460         {
00461             this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
00462         }
00463         else if(getMicroSteps() == 1)
00464         {
00465             this->_REGI_RUN.B.sc = 0x8; // 1 steps positive value
00466         }
00467         else if(getMicroSteps() == 2)
00468         {
00469             this->_REGI_RUN.B.sc = 0x4; // 1 steps positive value
00470         }
00471         else if(getMicroSteps() == 3)
00472         {
00473             this->_REGI_RUN.B.sc = 0x2; // 1 steps positive value
00474         }
00475         
00476         
00477     }
00478     else
00479     {
00480         if(getMicroSteps() == 0)
00481         {
00482             this->_REGI_RUN.B.sc = 0x30; // 1 steps positive value
00483         }
00484         else if(getMicroSteps() == 1)
00485         {
00486             this->_REGI_RUN.B.sc = 0x38; // 1 steps positive value
00487         }
00488         else if(getMicroSteps() == 2)
00489         {
00490             this->_REGI_RUN.B.sc = 0x3C; // 1 steps positive value
00491         }
00492         else if(getMicroSteps() == 3)
00493         {
00494             this->_REGI_RUN.B.sc = 0x3E; // 1 steps positive value
00495         }
00496     }
00497 
00498  
00499     
00500     for(int i = 0;i < num_steps;i++)
00501     {
00502         setRunRegi(this->_REGI_RUN); //1 steps positive value
00503          wait_ms(delay);
00504     }
00505     
00506     this->_REGI_RUN.B.en = 0;
00507     setRunRegi(this->_REGI_RUN); //1 steps positive value
00508 
00509 }