Revision 0:9156e6b6bf46, committed 2014-10-06

Comitter:

piniels

Date:

Mon Oct 06 11:57:45 2014 +0000

Commit message:

jkhasd

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/A4980ControlRegi.h	Mon Oct 06 11:57:45 2014 +0000
@@ -0,0 +1,99 @@
+/*-----------------------------------------------------------
+*                   COPYRIGHT (C) 2010.
+*          GN Otometrics. All rights reserved.
+*-----------------------------------------------------------
+*            P R O P R I E T A R Y   R I G H T S
+* This document must not be used, nor copied in any form or
+* handed over to third party, without prior written
+* permission from GN Otometrics.
+*-----------------------------------------------------------
+*
+* This source file is part of the 1082 Turtle project.
+*
+* P4 Info: (P4-filetype must be "ktext" for these to work!)
+* Last edit by: $Author:$
+* Date:         $DateTime:$
+* Filename:     $File:$
+* Revision:     $Revision:$
+*
+* File Info:
+* Original author: peilni
+* Creation date:   Nov 19, 2013
+*
+* Doxygen Info: *//*!
+* @file
+* @brief Blah blah blah.
+*
+* @htmlonly
+* @endhtmlonly
+*
+* Blah blah
+*/
+#ifndef A4980CONTROLREGI_H_
+#define A4980CONTROLREGI_H_
+#include <stdint.h>
+
+/*
+ * (*) are default
+ */
+typedef union
+{
+    uint16_t I;
+
+    struct
+    {
+        uint16_t pwm              : 1;    // bit0 PWM configuration *0=Fixed off-time, 1=Fixed frequency
+        uint16_t tofFrq           : 3;    // bit3-1 Off time (only valid when PWM bit = 0).Replaces FRQ bits(Assumes 4-MHz clock) 000=20us, 001=24us, 010=28us, 011=32us, 100=36us, 101=40us, *110=44us, 111=48us//        Frequency )(only valid when PWM bit = 1)Replace TOF bits(Assumes 4-MHz clock) 000=24us/41.7kHz, 001=32us/31.3kHz, 010=40us/25kHz, 011=46us/21.7kHz, 100=52us/19.2kHz 101=56us/17.9kHz, *110=60us/16.7kHz, 111=64us/15.6kHz
+        uint16_t tbk              : 2;    // bit5-4 Blank Time (Assumes 4-MHz clock) 00=1us, *01=1.5us, 10=2.5us, 11=3.5us
+        uint16_t pfd              : 3;    // bit8-6 Fast decay time for mixed decay. Assumes 4-MHz clock 000=2us, 001=3us, 010=4us, 011=6us, 100=8us, 101=10us, 110=14us, 111=20us
+        uint16_t mx               : 2;    // bit10-9. Max phase current as a percentage of I(SMAX) 00=25% 01=50% 10=75% *11=100%
+        uint16_t ms               : 2;    // bit12-11. Microstep mode for external STEP input control *00=Full Step 01=Half Step 10=Quarter Step 11=Sixteenth Step
+        uint16_t syr              : 1;    // bit13. Synchronous rectification 0=Diode recirculation *1=Synchronous
+        uint16_t addr             : 2;    // bit15-14 Address config0 = 0b00
+
+    } B;
+} t_config_control_regi_0;
+
+/*
+ * (*) are default
+ */
+typedef union
+{
+    uint16_t I;
+
+    struct
+    {
+        uint16_t diag             : 2;    // bit1-0 Selects signal routed to DIAG output *00=Fault-low true, 01=ST-low true, 10=PWN-on Phase A, 11=Temperature
+        uint16_t cd               : 4;    // bit5-2 PWM count difference for ST detection default to 8
+        uint16_t notInUse         : 5;    // bit10-6 Not in use value = 0
+        uint16_t tsc              : 2;    // bit12-11 Overcurrent fault delay(Assumes 4-MHz clock) 00=0.5us, 01=1us, *10=2us, 11=3us
+        uint16_t osc              : 1;    // bit13 Selects clock source *0=internal, 1=external
+        uint16_t addr             : 2;    // bit15-14 Address config0 = 0b01
+
+    } B;
+}t_config_control_regi_1;
+
+/*
+ * (*) are default
+ */
+typedef union
+{
+    uint16_t I;
+
+    struct
+    {
+        uint16_t sc               : 6;    // bit5-0 Step change number 2’s complement format. Positive value increases Step Angle Number. Negative value decreases Step Angle Number
+        uint16_t dcy              : 2;    // bit7-6 Decay mode selection 00=Slow, *01=Mixed-PFD fixed, 10=Mixed-PFD auto, 11=Fast
+        uint16_t brk              : 1;    // bit8 Brake enable *0=Normal operation 1=Brake active
+        uint16_t slew             : 1;    // bit9 Slew rate control 0=Disable *1=Enable
+        uint16_t hlr              : 1;    // bit10. Selects slow decay and brake recirculation path *0=High side, 1=Low side
+        uint16_t ol               : 2;    // 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%
+        uint16_t en               : 1;    // bit13. Phase current enable OR with ENABLE pin *0=Output bridges disabled if ENABLE pin = 0, 1=Output bridges enabled
+        uint16_t addr             : 2;    // bit15-14 Address run = 0b10
+
+    } B;
+}t_run_regi;
+
+
+
+#endif /* A4980CONTROLREGI_H_ */

--- /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
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motorControl.h	Mon Oct 06 11:57:45 2014 +0000
@@ -0,0 +1,67 @@
+/*
+############################################
+##           Motor Control v0.1 Library   ##
+##          created by Peter Ilsoee       ##
+############################################
+        ---- piniels@gmail.com -----
+This library was made for 4-Phase Stepper Motors for A4980 SPI device from Allegro
+I don't take any resposability for the damage caused to your equipment.
+
+*/
+#ifndef MOTOR_CONTROL_H
+#define MOTOR_CONTROL_H
+
+#include "mbed.h"
+#include "A4980ControlRegi.h"
+
+class motorControl {
+public:
+
+    motorControl(int numberOfSteps, PinName step, PinName dir, PinName ms1, PinName ms0, PinName enable, PinName reset, PinName diag); //motor constructor
+    void step(int num_steps, int direction, int speed);
+
+    void initSpi();
+    int setConfigRegi0(t_config_control_regi_0 value);
+    int setConfigRegi1(t_config_control_regi_1 value);
+    int setRunRegi(t_run_regi value);
+    int getStepCount();
+    void setStepCount(int value);
+    void setMicroSteps(int value);
+    int getMicroSteps();
+    t_config_control_regi_0 getConfigRegi0();
+    t_config_control_regi_1 getConfigRegi1();
+    t_run_regi getRunRegi();
+    void runStepperAtSpeed(bool startStop, int whatSpeed, int direction);
+    void runStepperAtSpeedWithSPI(bool startStop, int whatSpeed, int direction);
+    void goToZeroPosition(int whatSpeed);
+    void openCloseValve(bool openTrue);
+    /** Call back member for timer tick
+     *
+     */
+    void runMotor(void);
+    void doRamp(int numberOfRampsteps,float endDelay);
+    float calculateStepDelay(int whatSpeed);
+    void step(int num_steps, int delay, bool direction);
+    
+private:
+    int _Number_OF_STEPS;
+    int _Step_Counter;
+    bool _Bool_Direction;
+    PwmOut _STEP;
+    DigitalOut _DIR;
+    DigitalOut _MS1;
+    DigitalOut _MS0;
+    DigitalOut _ENABLE;
+    DigitalOut _RESET;
+    DigitalIn  _DIAG;
+    float step_delay;
+    t_config_control_regi_0 _REGI_0;
+    t_config_control_regi_1 _REGI_1;
+    t_run_regi              _REGI_RUN;
+    
+
+
+
+};
+
+#endif