Diff: motorControl.cpp

Revision:

0:9156e6b6bf46

diff -r 000000000000 -r 9156e6b6bf46 motorControl.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motorControl.cpp	Mon Oct 06 11:57:45 2014 +0000
@@ -0,0 +1,509 @@
+/*
+############################################
+##           motorControl v0.1 Library    ##
+##          created by Peter Ilsoee       ##
+############################################
+        ---- piniels@gmail.com -----
+This library was made for 4-Phase Stepper Motors
+I don't take any resposability for the damage caused to your equipment.
+
+*/
+
+#include "motorControl.h"
+
+#include "mbed.h"
+#include "globalt.h"
+
+
+SPI spi(PTE1, PTE3, PTE2); // mosi, miso, sclk
+DigitalOut cs(PTE4);
+//struct for holding run register information
+
+//GPIO
+DigitalOut valve(PTD0);
+DigitalIn centerInput(PTD4);
+
+//Ticker for running at a specified speed
+Ticker runTickMotorControl;
+
+
+
+
+
+/**
+ * ! @brief Construct including the pins for "manual(not SPI cmd)" control of A4980
+ * @param step Step logic input. Motor advances on rising edge. Filtered input with hysteresis.
+ * @param dir Direction logic input. Direction changes on the next STEP rising edge. When high, the Phase Angle Number is increased
+ *            on the rising edge of STEP. Has no effect when using the serial
+ *            interface. Filtered input with hysteresis.
+ * @param ms1 Microstep resolution select input.
+ * @param ms0 Microstep resolution select input.
+ * @param enable Controls activity of bridge outputs. When held low, deactivates the outputs, that is, turns off all output bridge FETs.
+ *               Internal logic continues to follow input commands.
+ * @param reset Resets faults when pulsed low. Forces low-power shutdown(sleep) when held low for more than the Reset Shutdown
+ *              Width, tRSD . Can be pulled to VBB with 30 kΩ resistor.
+ * @param diag  Diagnostic output. Function selected via the serial interface,
+ *              setting Configuration Register 1. Default is Fault output.
+ */
+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){
+
+    this->_RESET = 0;
+    this->_STEP = 0; // duty cycle on 50 %
+    this->_DIR = 0;
+    this->_MS1 = 0;
+    this->_MS0 = 0;
+    this->_ENABLE = 0;
+    //Setting default values to run regi
+    this->_REGI_RUN.B.sc = 0x1; //1 steps positive value
+    this->_REGI_RUN.B.dcy = 0x1; // bit7-6 Decay mode selection 00=Slow, *01=Mixed-PFD fixed, 10=Mixed-PFD auto, 11=Fast
+    this->_REGI_RUN.B.brk = 0x0; // bit8 Brake enable *0=Normal operation 1=Brake active
+    this->_REGI_RUN.B.slew = 0x1; // bit9 Slew rate control 0=Disable *1=Enable
+    this->_REGI_RUN.B.hlr = 0x0; // bit10. Selects slow decay and brake recirculation path *0=High side, 1=Low side
+    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%
+    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
+    this->_REGI_RUN.B.addr = 0x2; // bit15-14 Address run = 0b10
+    //Setting default value to control regi. 0
+    this->_REGI_0.B.pwm = 0x0;
+    this->_REGI_0.B.tofFrq = 0x6;
+    this->_REGI_0.B.tbk = 0x1;
+    this->_REGI_0.B.pfd = 0x4;
+    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%
+    this->_REGI_0.B.ms = 0x0;
+    this->_REGI_0.B.syr = 0x1;
+    this->_REGI_0.B.addr = 0x0;
+    //Setting default value to control regi. 1
+    this->_REGI_1.B.diag = 0x0;
+    this->_REGI_1.B.cd = 0x8;
+    this->_REGI_1.B.notInUse = 0x0;
+    this->_REGI_1.B.tsc = 0x2;
+    this->_REGI_1.B.osc = 0x0;
+    this->_REGI_1.B.addr = 0x1;
+    wait(0.1);
+    this->_RESET = 1;
+    this->_Step_Counter = 0;
+    
+    
+
+}
+
+
+void motorControl::initSpi() { // rotate the motor 1 step anticlockwise
+cs = 1;
+spi.format(8,3);
+spi.frequency(1000000);
+
+}
+
+/**
+ * ! @brief set the step counter
+ * @param value an int counting the steps
+ */
+void motorControl::setStepCount(int value)
+{
+    this->_Step_Counter = value;
+    
+}
+
+/**
+ * ! @brief get the step counter
+ * @return an int counting the steps
+ */
+int motorControl::getStepCount()
+{
+    return this->_Step_Counter;
+    
+}
+
+/**
+ * ! @brief set configuration and control register 0
+ * @param t_config_control_regi_0 an union with represents some seetings (see datasheet A4980 data sheet)
+ */
+int motorControl::setConfigRegi0(t_config_control_regi_0 value)
+{
+    this->_REGI_0 = value;
+    cs = 0;
+    int response = (spi.write(value.I >> 8)) << 8;
+    response |= spi.write(value.I & 0xff);
+    cs = 1;
+    return response;
+}
+
+/**
+ * ! @brief get configuration and control register 0
+ * @return t_config_control_regi_0 an union with represents some seetings (see datasheet A4980 data sheet)
+ */
+t_config_control_regi_0 motorControl::getConfigRegi0()
+{
+    return this->_REGI_0;
+}
+
+/**
+ * ! @brief open or close the GPIO controlling the valve
+ * @parm openTrue Boolean true for open
+ */
+void motorControl::openCloseValve(bool openTrue)
+{
+    if(openTrue)
+    {
+        valve = 0;
+    }
+    else
+    {
+        valve = 1;
+    }
+}
+
+/**
+ * ! @brief set configuration and control register 1
+ * @param t_config_control_regi_1 an union with represents some seetings (see datasheet A4980 data sheet)
+ */
+int motorControl::setConfigRegi1(t_config_control_regi_1 value)
+{
+    this->_REGI_1 = value;
+    cs = 0;
+    int response = (spi.write(value.I >> 8)) << 8;
+    response |= spi.write(value.I & 0xff);
+    cs = 1;
+    return response;
+}
+
+/**
+ * ! @brief set micro step bits
+ * @param int from 0 to 3
+ */
+void motorControl::setMicroSteps(int value)
+{
+    printf("Value for microsteps: %i",value);
+    printf("\r\n");
+    this->_MS1 = (value & 0x2) >> 1;
+    this->_MS0 = value & 0x1;
+    
+}
+
+/**
+ * ! @brief get micro step bits
+ * @param int from 0 to 3
+ */
+int motorControl::getMicroSteps()
+{
+    return this->_MS1 << 1 | this->_MS0;
+    
+}
+
+
+/**
+ * ! @brief get configuration and control register 1
+ * @return t_config_control_regi_1 an union with represents some seetings (see datasheet A4980 data sheet)
+ */
+t_config_control_regi_1 motorControl::getConfigRegi1()
+{
+    return this->_REGI_1;
+}
+
+/**
+ * ! @brief set configuration and run register
+ * @param t_run_regi an union with represents some seetings (see datasheet A4980 data sheet)
+ */
+int motorControl::setRunRegi(t_run_regi value)
+{
+    this->_REGI_RUN = value;
+    cs = 0;
+    int response = (spi.write(value.I >> 8)) << 8;
+    response |= spi.write(value.I & 0xff);
+    cs = 1;
+    return response;
+}
+
+/**
+ * ! @brief get run control register
+ * @return t_run_regi an union with represents some seetings (see datasheet A4980 data sheet)
+ */
+t_run_regi motorControl::getRunRegi()
+{
+    return this->_REGI_RUN;
+}
+
+/**
+ * ! @brief Run at a specified speed, none blocking
+ * @param startStop If true start moter if false stop motor
+ * @param whatSpeed speed in RPM
+ * @param direction if 1 then postive phase, if 0 negative phase
+ */
+// 
+void motorControl::runStepperAtSpeed(bool startStop, int whatSpeed, int direction)
+{
+    this->_DIR = direction;
+    this->step_delay = calculateStepDelay(whatSpeed);
+    
+    if(this->step_delay > MAX_STEP_DELAY)
+    {
+        this->step_delay = MAX_STEP_DELAY;
+    }
+    else if(this->step_delay < MIN_STEP_DELAY)
+    {
+        this->step_delay = MIN_STEP_DELAY;
+    }
+    
+    
+    
+    if(startStop)
+    {
+        this->_ENABLE = 1;
+        _STEP = 0.5; //duty cycle 50%
+        _STEP.period_us((int) this->step_delay);
+        
+    }
+    else
+    {
+        _STEP = 0; //Duty-cycle 0
+        _ENABLE = 0;
+        _STEP = 0;
+        
+    }
+}
+
+void motorControl::doRamp(int numberOfRampsteps,float endDelay)
+{
+    //Test do ramping
+    
+    int delay_us_start = 10000;
+    int diff_delay_us = delay_us_start - endDelay;
+    int ramping_step_us = diff_delay_us/numberOfRampsteps;
+    int delay_ramp = 0;
+     
+    
+    for(int i = 0;i < numberOfRampsteps; i++)
+    {
+           delay_ramp = delay_us_start - (i*ramping_step_us);
+           printf("Ramping Delay is in  us: %i", delay_ramp);
+           printf("\r\n");
+           _STEP.period_us(delay_ramp);
+           wait_us(delay_ramp);
+    }
+}
+
+float motorControl::calculateStepDelay(int whatSpeed)
+{
+   if(whatSpeed != 0)
+    {
+        this->step_delay = 60L * 1000000L / ((float)this->_Number_OF_STEPS * (float)whatSpeed);
+        int microStep = getMicroSteps();
+        if(microStep > 0)
+        {
+           if(microStep == 1)
+           {
+            this->step_delay = this->step_delay/2;  
+            }
+            else if(microStep == 2)
+            {
+                this->step_delay = this->step_delay/4;  
+            }
+            else
+            {
+                this->step_delay = this->step_delay/16;  
+            }
+           
+        }
+        
+    }
+    else
+    {
+        this->step_delay = 60L * 1000L / (float) this->_Number_OF_STEPS / (float) 1;
+    }
+    
+    return this->step_delay;
+}
+
+// tick event handler for running the motor at a specified speed
+void motorControl::runMotor(void)
+{
+    setRunRegi(this->_REGI_RUN); //1 steps positive value
+    if(this->_Bool_Direction)
+    {
+        this->_Step_Counter++;
+    }
+    else
+    {
+        this->_Step_Counter--;
+    }
+    
+    
+    
+}
+
+ void motorControl::goToZeroPosition(int whatSpeed)
+ {
+    float step_delay = (60 / (float)this->_Number_OF_STEPS / (float)whatSpeed);
+    bool runMotorBool = true;
+    bool runMotorBack = false;
+    openCloseValve(true);
+    
+    while(runMotorBool)//this->_Step_Counter != 0 || centerInput )
+    {
+        this->_REGI_RUN.B.en = 1;
+        if(whatSpeed > 0)
+        {
+            this->_Bool_Direction = true;
+            this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
+            runMotor();
+            wait(step_delay);
+            if(centerInput)
+            {
+                runMotorBool = false;
+                this->_Step_Counter = 0;
+                
+            }
+        }
+        else
+        {
+            this->_Bool_Direction = false;
+            this->_REGI_RUN.B.sc = 0x30; // 1 steps positive value
+            runMotor();
+            wait(-1*step_delay);
+            if(centerInput)
+            {
+                runMotorBack = true;
+            }
+            else if(runMotorBack && !centerInput)
+            {
+                runMotorBool = false;
+                this->_Step_Counter = 0;
+            }
+            
+        }
+       
+        
+    } 
+    this->_REGI_RUN.B.en = 0;    
+
+}
+
+// Run at a specified speed, no blocking
+void motorControl::runStepperAtSpeedWithSPI(bool startStop, int whatSpeed, int direction)
+{
+    
+    
+    if(whatSpeed != 0)
+    {
+        this->step_delay = (60L * 1000000L / this->_Number_OF_STEPS / whatSpeed);
+        
+    }
+    else
+    {
+        this->step_delay = 60L * 1000000L / this->_Number_OF_STEPS / 1;
+        
+    }
+    //pc.printf("Step dealy: %d",this->step_delay);
+    //pc.printf("\r\n");
+    if(direction)
+    {
+        this->_Bool_Direction = true;
+        if(whatSpeed > 50)//HACK: for testing micro step
+        {
+            this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
+        }
+        else
+        {
+            this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
+            //this->_REGI_RUN.B.sc = 0x4; // 1/8 steps positive value
+            //this->step_delay = this->step_delay/4;
+            printf("Step dealy micro step: %d",this->step_delay);
+            printf("\r\n");
+        }
+        
+    }
+    else
+    {
+        this->_Bool_Direction = false;;
+        
+        if(whatSpeed > 50) //HACK: for testing micro step
+        {
+            this->_REGI_RUN.B.sc = 0x30; // 1 steps negative value
+        }
+        else
+        {
+            this->_REGI_RUN.B.sc = 0x30; // 1 steps negative value
+            //this->_REGI_RUN.B.sc = 0x3C; // 1/16 steps negative value
+            //this->step_delay = this->step_delay/4;
+            printf("Step dealy micro step: %d",this->step_delay);
+            printf("\r\n");
+        }
+        //
+        
+    }
+
+    if(startStop)
+    {
+        this->_REGI_RUN.B.en = 1;
+        runTickMotorControl.attach_us(this, &motorControl::runMotor, (this->step_delay)); // the address of the function to be attached
+    }
+    else
+    {
+        this->_REGI_RUN.B.en = 0;
+        setRunRegi(this->_REGI_RUN);
+        runTickMotorControl.detach();
+        
+    }
+}
+
+
+
+void motorControl::step(int num_steps, int delay, bool direction) {// steper function: number of steps, direction (0- right, 1- left), 
+
+    this->_REGI_RUN.B.en = 1;
+
+    
+    
+    if(direction)
+    {
+        if(getMicroSteps() == 0)
+        {
+            this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
+        }
+        else if(getMicroSteps() == 1)
+        {
+            this->_REGI_RUN.B.sc = 0x8; // 1 steps positive value
+        }
+        else if(getMicroSteps() == 2)
+        {
+            this->_REGI_RUN.B.sc = 0x4; // 1 steps positive value
+        }
+        else if(getMicroSteps() == 3)
+        {
+            this->_REGI_RUN.B.sc = 0x2; // 1 steps positive value
+        }
+        
+        
+    }
+    else
+    {
+        if(getMicroSteps() == 0)
+        {
+            this->_REGI_RUN.B.sc = 0x30; // 1 steps positive value
+        }
+        else if(getMicroSteps() == 1)
+        {
+            this->_REGI_RUN.B.sc = 0x38; // 1 steps positive value
+        }
+        else if(getMicroSteps() == 2)
+        {
+            this->_REGI_RUN.B.sc = 0x3C; // 1 steps positive value
+        }
+        else if(getMicroSteps() == 3)
+        {
+            this->_REGI_RUN.B.sc = 0x3E; // 1 steps positive value
+        }
+    }
+
+ 
+    
+    for(int i = 0;i < num_steps;i++)
+    {
+        setRunRegi(this->_REGI_RUN); //1 steps positive value
+         wait_ms(delay);
+    }
+    
+    this->_REGI_RUN.B.en = 0;
+    setRunRegi(this->_REGI_RUN); //1 steps positive value
+
+}