Ilsoe Peter / motorControl

for controlling stepper motor with A4980

Dependents:   HIDTympDeviceWithPIDController

motorControl.cpp@0:9156e6b6bf46, 2014-10-06 (annotated)

Committer:
piniels
Date:
Mon Oct 06 11:57:45 2014 +0000
Revision:
0:9156e6b6bf46
jkhasd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
piniels 0:9156e6b6bf46 1 /*
piniels 0:9156e6b6bf46 2 ############################################
piniels 0:9156e6b6bf46 3 ## motorControl v0.1 Library ##
piniels 0:9156e6b6bf46 4 ## created by Peter Ilsoee ##
piniels 0:9156e6b6bf46 5 ############################################
piniels 0:9156e6b6bf46 6 ---- piniels@gmail.com -----
piniels 0:9156e6b6bf46 7 This library was made for 4-Phase Stepper Motors
piniels 0:9156e6b6bf46 8 I don't take any resposability for the damage caused to your equipment.
piniels 0:9156e6b6bf46 9
piniels 0:9156e6b6bf46 10 */
piniels 0:9156e6b6bf46 11
piniels 0:9156e6b6bf46 12 #include "motorControl.h"
piniels 0:9156e6b6bf46 13
piniels 0:9156e6b6bf46 14 #include "mbed.h"
piniels 0:9156e6b6bf46 15 #include "globalt.h"
piniels 0:9156e6b6bf46 16
piniels 0:9156e6b6bf46 17
piniels 0:9156e6b6bf46 18 SPI spi(PTE1, PTE3, PTE2); // mosi, miso, sclk
piniels 0:9156e6b6bf46 19 DigitalOut cs(PTE4);
piniels 0:9156e6b6bf46 20 //struct for holding run register information
piniels 0:9156e6b6bf46 21
piniels 0:9156e6b6bf46 22 //GPIO
piniels 0:9156e6b6bf46 23 DigitalOut valve(PTD0);
piniels 0:9156e6b6bf46 24 DigitalIn centerInput(PTD4);
piniels 0:9156e6b6bf46 25
piniels 0:9156e6b6bf46 26 //Ticker for running at a specified speed
piniels 0:9156e6b6bf46 27 Ticker runTickMotorControl;
piniels 0:9156e6b6bf46 28
piniels 0:9156e6b6bf46 29
piniels 0:9156e6b6bf46 30
piniels 0:9156e6b6bf46 31
piniels 0:9156e6b6bf46 32
piniels 0:9156e6b6bf46 33 /**
piniels 0:9156e6b6bf46 34 * ! @brief Construct including the pins for "manual(not SPI cmd)" control of A4980
piniels 0:9156e6b6bf46 35 * @param step Step logic input. Motor advances on rising edge. Filtered input with hysteresis.
piniels 0:9156e6b6bf46 36 * @param dir Direction logic input. Direction changes on the next STEP rising edge. When high, the Phase Angle Number is increased
piniels 0:9156e6b6bf46 37 * on the rising edge of STEP. Has no effect when using the serial
piniels 0:9156e6b6bf46 38 * interface. Filtered input with hysteresis.
piniels 0:9156e6b6bf46 39 * @param ms1 Microstep resolution select input.
piniels 0:9156e6b6bf46 40 * @param ms0 Microstep resolution select input.
piniels 0:9156e6b6bf46 41 * @param enable Controls activity of bridge outputs. When held low, deactivates the outputs, that is, turns off all output bridge FETs.
piniels 0:9156e6b6bf46 42 * Internal logic continues to follow input commands.
piniels 0:9156e6b6bf46 43 * @param reset Resets faults when pulsed low. Forces low-power shutdown(sleep) when held low for more than the Reset Shutdown
piniels 0:9156e6b6bf46 44 * Width, tRSD . Can be pulled to VBB with 30 kΩ resistor.
piniels 0:9156e6b6bf46 45 * @param diag Diagnostic output. Function selected via the serial interface,
piniels 0:9156e6b6bf46 46 * setting Configuration Register 1. Default is Fault output.
piniels 0:9156e6b6bf46 47 */
piniels 0:9156e6b6bf46 48 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){
piniels 0:9156e6b6bf46 49
piniels 0:9156e6b6bf46 50 this->_RESET = 0;
piniels 0:9156e6b6bf46 51 this->_STEP = 0; // duty cycle on 50 %
piniels 0:9156e6b6bf46 52 this->_DIR = 0;
piniels 0:9156e6b6bf46 53 this->_MS1 = 0;
piniels 0:9156e6b6bf46 54 this->_MS0 = 0;
piniels 0:9156e6b6bf46 55 this->_ENABLE = 0;
piniels 0:9156e6b6bf46 56 //Setting default values to run regi
piniels 0:9156e6b6bf46 57 this->_REGI_RUN.B.sc = 0x1; //1 steps positive value
piniels 0:9156e6b6bf46 58 this->_REGI_RUN.B.dcy = 0x1; // bit7-6 Decay mode selection 00=Slow, *01=Mixed-PFD fixed, 10=Mixed-PFD auto, 11=Fast
piniels 0:9156e6b6bf46 59 this->_REGI_RUN.B.brk = 0x0; // bit8 Brake enable *0=Normal operation 1=Brake active
piniels 0:9156e6b6bf46 60 this->_REGI_RUN.B.slew = 0x1; // bit9 Slew rate control 0=Disable *1=Enable
piniels 0:9156e6b6bf46 61 this->_REGI_RUN.B.hlr = 0x0; // bit10. Selects slow decay and brake recirculation path *0=High side, 1=Low side
piniels 0:9156e6b6bf46 62 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%
piniels 0:9156e6b6bf46 63 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
piniels 0:9156e6b6bf46 64 this->_REGI_RUN.B.addr = 0x2; // bit15-14 Address run = 0b10
piniels 0:9156e6b6bf46 65 //Setting default value to control regi. 0
piniels 0:9156e6b6bf46 66 this->_REGI_0.B.pwm = 0x0;
piniels 0:9156e6b6bf46 67 this->_REGI_0.B.tofFrq = 0x6;
piniels 0:9156e6b6bf46 68 this->_REGI_0.B.tbk = 0x1;
piniels 0:9156e6b6bf46 69 this->_REGI_0.B.pfd = 0x4;
piniels 0:9156e6b6bf46 70 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%
piniels 0:9156e6b6bf46 71 this->_REGI_0.B.ms = 0x0;
piniels 0:9156e6b6bf46 72 this->_REGI_0.B.syr = 0x1;
piniels 0:9156e6b6bf46 73 this->_REGI_0.B.addr = 0x0;
piniels 0:9156e6b6bf46 74 //Setting default value to control regi. 1
piniels 0:9156e6b6bf46 75 this->_REGI_1.B.diag = 0x0;
piniels 0:9156e6b6bf46 76 this->_REGI_1.B.cd = 0x8;
piniels 0:9156e6b6bf46 77 this->_REGI_1.B.notInUse = 0x0;
piniels 0:9156e6b6bf46 78 this->_REGI_1.B.tsc = 0x2;
piniels 0:9156e6b6bf46 79 this->_REGI_1.B.osc = 0x0;
piniels 0:9156e6b6bf46 80 this->_REGI_1.B.addr = 0x1;
piniels 0:9156e6b6bf46 81 wait(0.1);
piniels 0:9156e6b6bf46 82 this->_RESET = 1;
piniels 0:9156e6b6bf46 83 this->_Step_Counter = 0;
piniels 0:9156e6b6bf46 84
piniels 0:9156e6b6bf46 85
piniels 0:9156e6b6bf46 86
piniels 0:9156e6b6bf46 87 }
piniels 0:9156e6b6bf46 88
piniels 0:9156e6b6bf46 89
piniels 0:9156e6b6bf46 90 void motorControl::initSpi() { // rotate the motor 1 step anticlockwise
piniels 0:9156e6b6bf46 91 cs = 1;
piniels 0:9156e6b6bf46 92 spi.format(8,3);
piniels 0:9156e6b6bf46 93 spi.frequency(1000000);
piniels 0:9156e6b6bf46 94
piniels 0:9156e6b6bf46 95 }
piniels 0:9156e6b6bf46 96
piniels 0:9156e6b6bf46 97 /**
piniels 0:9156e6b6bf46 98 * ! @brief set the step counter
piniels 0:9156e6b6bf46 99 * @param value an int counting the steps
piniels 0:9156e6b6bf46 100 */
piniels 0:9156e6b6bf46 101 void motorControl::setStepCount(int value)
piniels 0:9156e6b6bf46 102 {
piniels 0:9156e6b6bf46 103 this->_Step_Counter = value;
piniels 0:9156e6b6bf46 104
piniels 0:9156e6b6bf46 105 }
piniels 0:9156e6b6bf46 106
piniels 0:9156e6b6bf46 107 /**
piniels 0:9156e6b6bf46 108 * ! @brief get the step counter
piniels 0:9156e6b6bf46 109 * @return an int counting the steps
piniels 0:9156e6b6bf46 110 */
piniels 0:9156e6b6bf46 111 int motorControl::getStepCount()
piniels 0:9156e6b6bf46 112 {
piniels 0:9156e6b6bf46 113 return this->_Step_Counter;
piniels 0:9156e6b6bf46 114
piniels 0:9156e6b6bf46 115 }
piniels 0:9156e6b6bf46 116
piniels 0:9156e6b6bf46 117 /**
piniels 0:9156e6b6bf46 118 * ! @brief set configuration and control register 0
piniels 0:9156e6b6bf46 119 * @param t_config_control_regi_0 an union with represents some seetings (see datasheet A4980 data sheet)
piniels 0:9156e6b6bf46 120 */
piniels 0:9156e6b6bf46 121 int motorControl::setConfigRegi0(t_config_control_regi_0 value)
piniels 0:9156e6b6bf46 122 {
piniels 0:9156e6b6bf46 123 this->_REGI_0 = value;
piniels 0:9156e6b6bf46 124 cs = 0;
piniels 0:9156e6b6bf46 125 int response = (spi.write(value.I >> 8)) << 8;
piniels 0:9156e6b6bf46 126 response |= spi.write(value.I & 0xff);
piniels 0:9156e6b6bf46 127 cs = 1;
piniels 0:9156e6b6bf46 128 return response;
piniels 0:9156e6b6bf46 129 }
piniels 0:9156e6b6bf46 130
piniels 0:9156e6b6bf46 131 /**
piniels 0:9156e6b6bf46 132 * ! @brief get configuration and control register 0
piniels 0:9156e6b6bf46 133 * @return t_config_control_regi_0 an union with represents some seetings (see datasheet A4980 data sheet)
piniels 0:9156e6b6bf46 134 */
piniels 0:9156e6b6bf46 135 t_config_control_regi_0 motorControl::getConfigRegi0()
piniels 0:9156e6b6bf46 136 {
piniels 0:9156e6b6bf46 137 return this->_REGI_0;
piniels 0:9156e6b6bf46 138 }
piniels 0:9156e6b6bf46 139
piniels 0:9156e6b6bf46 140 /**
piniels 0:9156e6b6bf46 141 * ! @brief open or close the GPIO controlling the valve
piniels 0:9156e6b6bf46 142 * @parm openTrue Boolean true for open
piniels 0:9156e6b6bf46 143 */
piniels 0:9156e6b6bf46 144 void motorControl::openCloseValve(bool openTrue)
piniels 0:9156e6b6bf46 145 {
piniels 0:9156e6b6bf46 146 if(openTrue)
piniels 0:9156e6b6bf46 147 {
piniels 0:9156e6b6bf46 148 valve = 0;
piniels 0:9156e6b6bf46 149 }
piniels 0:9156e6b6bf46 150 else
piniels 0:9156e6b6bf46 151 {
piniels 0:9156e6b6bf46 152 valve = 1;
piniels 0:9156e6b6bf46 153 }
piniels 0:9156e6b6bf46 154 }
piniels 0:9156e6b6bf46 155
piniels 0:9156e6b6bf46 156 /**
piniels 0:9156e6b6bf46 157 * ! @brief set configuration and control register 1
piniels 0:9156e6b6bf46 158 * @param t_config_control_regi_1 an union with represents some seetings (see datasheet A4980 data sheet)
piniels 0:9156e6b6bf46 159 */
piniels 0:9156e6b6bf46 160 int motorControl::setConfigRegi1(t_config_control_regi_1 value)
piniels 0:9156e6b6bf46 161 {
piniels 0:9156e6b6bf46 162 this->_REGI_1 = value;
piniels 0:9156e6b6bf46 163 cs = 0;
piniels 0:9156e6b6bf46 164 int response = (spi.write(value.I >> 8)) << 8;
piniels 0:9156e6b6bf46 165 response |= spi.write(value.I & 0xff);
piniels 0:9156e6b6bf46 166 cs = 1;
piniels 0:9156e6b6bf46 167 return response;
piniels 0:9156e6b6bf46 168 }
piniels 0:9156e6b6bf46 169
piniels 0:9156e6b6bf46 170 /**
piniels 0:9156e6b6bf46 171 * ! @brief set micro step bits
piniels 0:9156e6b6bf46 172 * @param int from 0 to 3
piniels 0:9156e6b6bf46 173 */
piniels 0:9156e6b6bf46 174 void motorControl::setMicroSteps(int value)
piniels 0:9156e6b6bf46 175 {
piniels 0:9156e6b6bf46 176 printf("Value for microsteps: %i",value);
piniels 0:9156e6b6bf46 177 printf("\r\n");
piniels 0:9156e6b6bf46 178 this->_MS1 = (value & 0x2) >> 1;
piniels 0:9156e6b6bf46 179 this->_MS0 = value & 0x1;
piniels 0:9156e6b6bf46 180
piniels 0:9156e6b6bf46 181 }
piniels 0:9156e6b6bf46 182
piniels 0:9156e6b6bf46 183 /**
piniels 0:9156e6b6bf46 184 * ! @brief get micro step bits
piniels 0:9156e6b6bf46 185 * @param int from 0 to 3
piniels 0:9156e6b6bf46 186 */
piniels 0:9156e6b6bf46 187 int motorControl::getMicroSteps()
piniels 0:9156e6b6bf46 188 {
piniels 0:9156e6b6bf46 189 return this->_MS1 << 1 | this->_MS0;
piniels 0:9156e6b6bf46 190
piniels 0:9156e6b6bf46 191 }
piniels 0:9156e6b6bf46 192
piniels 0:9156e6b6bf46 193
piniels 0:9156e6b6bf46 194 /**
piniels 0:9156e6b6bf46 195 * ! @brief get configuration and control register 1
piniels 0:9156e6b6bf46 196 * @return t_config_control_regi_1 an union with represents some seetings (see datasheet A4980 data sheet)
piniels 0:9156e6b6bf46 197 */
piniels 0:9156e6b6bf46 198 t_config_control_regi_1 motorControl::getConfigRegi1()
piniels 0:9156e6b6bf46 199 {
piniels 0:9156e6b6bf46 200 return this->_REGI_1;
piniels 0:9156e6b6bf46 201 }
piniels 0:9156e6b6bf46 202
piniels 0:9156e6b6bf46 203 /**
piniels 0:9156e6b6bf46 204 * ! @brief set configuration and run register
piniels 0:9156e6b6bf46 205 * @param t_run_regi an union with represents some seetings (see datasheet A4980 data sheet)
piniels 0:9156e6b6bf46 206 */
piniels 0:9156e6b6bf46 207 int motorControl::setRunRegi(t_run_regi value)
piniels 0:9156e6b6bf46 208 {
piniels 0:9156e6b6bf46 209 this->_REGI_RUN = value;
piniels 0:9156e6b6bf46 210 cs = 0;
piniels 0:9156e6b6bf46 211 int response = (spi.write(value.I >> 8)) << 8;
piniels 0:9156e6b6bf46 212 response |= spi.write(value.I & 0xff);
piniels 0:9156e6b6bf46 213 cs = 1;
piniels 0:9156e6b6bf46 214 return response;
piniels 0:9156e6b6bf46 215 }
piniels 0:9156e6b6bf46 216
piniels 0:9156e6b6bf46 217 /**
piniels 0:9156e6b6bf46 218 * ! @brief get run control register
piniels 0:9156e6b6bf46 219 * @return t_run_regi an union with represents some seetings (see datasheet A4980 data sheet)
piniels 0:9156e6b6bf46 220 */
piniels 0:9156e6b6bf46 221 t_run_regi motorControl::getRunRegi()
piniels 0:9156e6b6bf46 222 {
piniels 0:9156e6b6bf46 223 return this->_REGI_RUN;
piniels 0:9156e6b6bf46 224 }
piniels 0:9156e6b6bf46 225
piniels 0:9156e6b6bf46 226 /**
piniels 0:9156e6b6bf46 227 * ! @brief Run at a specified speed, none blocking
piniels 0:9156e6b6bf46 228 * @param startStop If true start moter if false stop motor
piniels 0:9156e6b6bf46 229 * @param whatSpeed speed in RPM
piniels 0:9156e6b6bf46 230 * @param direction if 1 then postive phase, if 0 negative phase
piniels 0:9156e6b6bf46 231 */
piniels 0:9156e6b6bf46 232 //
piniels 0:9156e6b6bf46 233 void motorControl::runStepperAtSpeed(bool startStop, int whatSpeed, int direction)
piniels 0:9156e6b6bf46 234 {
piniels 0:9156e6b6bf46 235 this->_DIR = direction;
piniels 0:9156e6b6bf46 236 this->step_delay = calculateStepDelay(whatSpeed);
piniels 0:9156e6b6bf46 237
piniels 0:9156e6b6bf46 238 if(this->step_delay > MAX_STEP_DELAY)
piniels 0:9156e6b6bf46 239 {
piniels 0:9156e6b6bf46 240 this->step_delay = MAX_STEP_DELAY;
piniels 0:9156e6b6bf46 241 }
piniels 0:9156e6b6bf46 242 else if(this->step_delay < MIN_STEP_DELAY)
piniels 0:9156e6b6bf46 243 {
piniels 0:9156e6b6bf46 244 this->step_delay = MIN_STEP_DELAY;
piniels 0:9156e6b6bf46 245 }
piniels 0:9156e6b6bf46 246
piniels 0:9156e6b6bf46 247
piniels 0:9156e6b6bf46 248
piniels 0:9156e6b6bf46 249 if(startStop)
piniels 0:9156e6b6bf46 250 {
piniels 0:9156e6b6bf46 251 this->_ENABLE = 1;
piniels 0:9156e6b6bf46 252 _STEP = 0.5; //duty cycle 50%
piniels 0:9156e6b6bf46 253 _STEP.period_us((int) this->step_delay);
piniels 0:9156e6b6bf46 254
piniels 0:9156e6b6bf46 255 }
piniels 0:9156e6b6bf46 256 else
piniels 0:9156e6b6bf46 257 {
piniels 0:9156e6b6bf46 258 _STEP = 0; //Duty-cycle 0
piniels 0:9156e6b6bf46 259 _ENABLE = 0;
piniels 0:9156e6b6bf46 260 _STEP = 0;
piniels 0:9156e6b6bf46 261
piniels 0:9156e6b6bf46 262 }
piniels 0:9156e6b6bf46 263 }
piniels 0:9156e6b6bf46 264
piniels 0:9156e6b6bf46 265 void motorControl::doRamp(int numberOfRampsteps,float endDelay)
piniels 0:9156e6b6bf46 266 {
piniels 0:9156e6b6bf46 267 //Test do ramping
piniels 0:9156e6b6bf46 268
piniels 0:9156e6b6bf46 269 int delay_us_start = 10000;
piniels 0:9156e6b6bf46 270 int diff_delay_us = delay_us_start - endDelay;
piniels 0:9156e6b6bf46 271 int ramping_step_us = diff_delay_us/numberOfRampsteps;
piniels 0:9156e6b6bf46 272 int delay_ramp = 0;
piniels 0:9156e6b6bf46 273
piniels 0:9156e6b6bf46 274
piniels 0:9156e6b6bf46 275 for(int i = 0;i < numberOfRampsteps; i++)
piniels 0:9156e6b6bf46 276 {
piniels 0:9156e6b6bf46 277 delay_ramp = delay_us_start - (i*ramping_step_us);
piniels 0:9156e6b6bf46 278 printf("Ramping Delay is in us: %i", delay_ramp);
piniels 0:9156e6b6bf46 279 printf("\r\n");
piniels 0:9156e6b6bf46 280 _STEP.period_us(delay_ramp);
piniels 0:9156e6b6bf46 281 wait_us(delay_ramp);
piniels 0:9156e6b6bf46 282 }
piniels 0:9156e6b6bf46 283 }
piniels 0:9156e6b6bf46 284
piniels 0:9156e6b6bf46 285 float motorControl::calculateStepDelay(int whatSpeed)
piniels 0:9156e6b6bf46 286 {
piniels 0:9156e6b6bf46 287 if(whatSpeed != 0)
piniels 0:9156e6b6bf46 288 {
piniels 0:9156e6b6bf46 289 this->step_delay = 60L * 1000000L / ((float)this->_Number_OF_STEPS * (float)whatSpeed);
piniels 0:9156e6b6bf46 290 int microStep = getMicroSteps();
piniels 0:9156e6b6bf46 291 if(microStep > 0)
piniels 0:9156e6b6bf46 292 {
piniels 0:9156e6b6bf46 293 if(microStep == 1)
piniels 0:9156e6b6bf46 294 {
piniels 0:9156e6b6bf46 295 this->step_delay = this->step_delay/2;
piniels 0:9156e6b6bf46 296 }
piniels 0:9156e6b6bf46 297 else if(microStep == 2)
piniels 0:9156e6b6bf46 298 {
piniels 0:9156e6b6bf46 299 this->step_delay = this->step_delay/4;
piniels 0:9156e6b6bf46 300 }
piniels 0:9156e6b6bf46 301 else
piniels 0:9156e6b6bf46 302 {
piniels 0:9156e6b6bf46 303 this->step_delay = this->step_delay/16;
piniels 0:9156e6b6bf46 304 }
piniels 0:9156e6b6bf46 305
piniels 0:9156e6b6bf46 306 }
piniels 0:9156e6b6bf46 307
piniels 0:9156e6b6bf46 308 }
piniels 0:9156e6b6bf46 309 else
piniels 0:9156e6b6bf46 310 {
piniels 0:9156e6b6bf46 311 this->step_delay = 60L * 1000L / (float) this->_Number_OF_STEPS / (float) 1;
piniels 0:9156e6b6bf46 312 }
piniels 0:9156e6b6bf46 313
piniels 0:9156e6b6bf46 314 return this->step_delay;
piniels 0:9156e6b6bf46 315 }
piniels 0:9156e6b6bf46 316
piniels 0:9156e6b6bf46 317 // tick event handler for running the motor at a specified speed
piniels 0:9156e6b6bf46 318 void motorControl::runMotor(void)
piniels 0:9156e6b6bf46 319 {
piniels 0:9156e6b6bf46 320 setRunRegi(this->_REGI_RUN); //1 steps positive value
piniels 0:9156e6b6bf46 321 if(this->_Bool_Direction)
piniels 0:9156e6b6bf46 322 {
piniels 0:9156e6b6bf46 323 this->_Step_Counter++;
piniels 0:9156e6b6bf46 324 }
piniels 0:9156e6b6bf46 325 else
piniels 0:9156e6b6bf46 326 {
piniels 0:9156e6b6bf46 327 this->_Step_Counter--;
piniels 0:9156e6b6bf46 328 }
piniels 0:9156e6b6bf46 329
piniels 0:9156e6b6bf46 330
piniels 0:9156e6b6bf46 331
piniels 0:9156e6b6bf46 332 }
piniels 0:9156e6b6bf46 333
piniels 0:9156e6b6bf46 334 void motorControl::goToZeroPosition(int whatSpeed)
piniels 0:9156e6b6bf46 335 {
piniels 0:9156e6b6bf46 336 float step_delay = (60 / (float)this->_Number_OF_STEPS / (float)whatSpeed);
piniels 0:9156e6b6bf46 337 bool runMotorBool = true;
piniels 0:9156e6b6bf46 338 bool runMotorBack = false;
piniels 0:9156e6b6bf46 339 openCloseValve(true);
piniels 0:9156e6b6bf46 340
piniels 0:9156e6b6bf46 341 while(runMotorBool)//this->_Step_Counter != 0 || centerInput )
piniels 0:9156e6b6bf46 342 {
piniels 0:9156e6b6bf46 343 this->_REGI_RUN.B.en = 1;
piniels 0:9156e6b6bf46 344 if(whatSpeed > 0)
piniels 0:9156e6b6bf46 345 {
piniels 0:9156e6b6bf46 346 this->_Bool_Direction = true;
piniels 0:9156e6b6bf46 347 this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
piniels 0:9156e6b6bf46 348 runMotor();
piniels 0:9156e6b6bf46 349 wait(step_delay);
piniels 0:9156e6b6bf46 350 if(centerInput)
piniels 0:9156e6b6bf46 351 {
piniels 0:9156e6b6bf46 352 runMotorBool = false;
piniels 0:9156e6b6bf46 353 this->_Step_Counter = 0;
piniels 0:9156e6b6bf46 354
piniels 0:9156e6b6bf46 355 }
piniels 0:9156e6b6bf46 356 }
piniels 0:9156e6b6bf46 357 else
piniels 0:9156e6b6bf46 358 {
piniels 0:9156e6b6bf46 359 this->_Bool_Direction = false;
piniels 0:9156e6b6bf46 360 this->_REGI_RUN.B.sc = 0x30; // 1 steps positive value
piniels 0:9156e6b6bf46 361 runMotor();
piniels 0:9156e6b6bf46 362 wait(-1*step_delay);
piniels 0:9156e6b6bf46 363 if(centerInput)
piniels 0:9156e6b6bf46 364 {
piniels 0:9156e6b6bf46 365 runMotorBack = true;
piniels 0:9156e6b6bf46 366 }
piniels 0:9156e6b6bf46 367 else if(runMotorBack && !centerInput)
piniels 0:9156e6b6bf46 368 {
piniels 0:9156e6b6bf46 369 runMotorBool = false;
piniels 0:9156e6b6bf46 370 this->_Step_Counter = 0;
piniels 0:9156e6b6bf46 371 }
piniels 0:9156e6b6bf46 372
piniels 0:9156e6b6bf46 373 }
piniels 0:9156e6b6bf46 374
piniels 0:9156e6b6bf46 375
piniels 0:9156e6b6bf46 376 }
piniels 0:9156e6b6bf46 377 this->_REGI_RUN.B.en = 0;
piniels 0:9156e6b6bf46 378
piniels 0:9156e6b6bf46 379 }
piniels 0:9156e6b6bf46 380
piniels 0:9156e6b6bf46 381 // Run at a specified speed, no blocking
piniels 0:9156e6b6bf46 382 void motorControl::runStepperAtSpeedWithSPI(bool startStop, int whatSpeed, int direction)
piniels 0:9156e6b6bf46 383 {
piniels 0:9156e6b6bf46 384
piniels 0:9156e6b6bf46 385
piniels 0:9156e6b6bf46 386 if(whatSpeed != 0)
piniels 0:9156e6b6bf46 387 {
piniels 0:9156e6b6bf46 388 this->step_delay = (60L * 1000000L / this->_Number_OF_STEPS / whatSpeed);
piniels 0:9156e6b6bf46 389
piniels 0:9156e6b6bf46 390 }
piniels 0:9156e6b6bf46 391 else
piniels 0:9156e6b6bf46 392 {
piniels 0:9156e6b6bf46 393 this->step_delay = 60L * 1000000L / this->_Number_OF_STEPS / 1;
piniels 0:9156e6b6bf46 394
piniels 0:9156e6b6bf46 395 }
piniels 0:9156e6b6bf46 396 //pc.printf("Step dealy: %d",this->step_delay);
piniels 0:9156e6b6bf46 397 //pc.printf("\r\n");
piniels 0:9156e6b6bf46 398 if(direction)
piniels 0:9156e6b6bf46 399 {
piniels 0:9156e6b6bf46 400 this->_Bool_Direction = true;
piniels 0:9156e6b6bf46 401 if(whatSpeed > 50)//HACK: for testing micro step
piniels 0:9156e6b6bf46 402 {
piniels 0:9156e6b6bf46 403 this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
piniels 0:9156e6b6bf46 404 }
piniels 0:9156e6b6bf46 405 else
piniels 0:9156e6b6bf46 406 {
piniels 0:9156e6b6bf46 407 this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
piniels 0:9156e6b6bf46 408 //this->_REGI_RUN.B.sc = 0x4; // 1/8 steps positive value
piniels 0:9156e6b6bf46 409 //this->step_delay = this->step_delay/4;
piniels 0:9156e6b6bf46 410 printf("Step dealy micro step: %d",this->step_delay);
piniels 0:9156e6b6bf46 411 printf("\r\n");
piniels 0:9156e6b6bf46 412 }
piniels 0:9156e6b6bf46 413
piniels 0:9156e6b6bf46 414 }
piniels 0:9156e6b6bf46 415 else
piniels 0:9156e6b6bf46 416 {
piniels 0:9156e6b6bf46 417 this->_Bool_Direction = false;;
piniels 0:9156e6b6bf46 418
piniels 0:9156e6b6bf46 419 if(whatSpeed > 50) //HACK: for testing micro step
piniels 0:9156e6b6bf46 420 {
piniels 0:9156e6b6bf46 421 this->_REGI_RUN.B.sc = 0x30; // 1 steps negative value
piniels 0:9156e6b6bf46 422 }
piniels 0:9156e6b6bf46 423 else
piniels 0:9156e6b6bf46 424 {
piniels 0:9156e6b6bf46 425 this->_REGI_RUN.B.sc = 0x30; // 1 steps negative value
piniels 0:9156e6b6bf46 426 //this->_REGI_RUN.B.sc = 0x3C; // 1/16 steps negative value
piniels 0:9156e6b6bf46 427 //this->step_delay = this->step_delay/4;
piniels 0:9156e6b6bf46 428 printf("Step dealy micro step: %d",this->step_delay);
piniels 0:9156e6b6bf46 429 printf("\r\n");
piniels 0:9156e6b6bf46 430 }
piniels 0:9156e6b6bf46 431 //
piniels 0:9156e6b6bf46 432
piniels 0:9156e6b6bf46 433 }
piniels 0:9156e6b6bf46 434
piniels 0:9156e6b6bf46 435 if(startStop)
piniels 0:9156e6b6bf46 436 {
piniels 0:9156e6b6bf46 437 this->_REGI_RUN.B.en = 1;
piniels 0:9156e6b6bf46 438 runTickMotorControl.attach_us(this, &motorControl::runMotor, (this->step_delay)); // the address of the function to be attached
piniels 0:9156e6b6bf46 439 }
piniels 0:9156e6b6bf46 440 else
piniels 0:9156e6b6bf46 441 {
piniels 0:9156e6b6bf46 442 this->_REGI_RUN.B.en = 0;
piniels 0:9156e6b6bf46 443 setRunRegi(this->_REGI_RUN);
piniels 0:9156e6b6bf46 444 runTickMotorControl.detach();
piniels 0:9156e6b6bf46 445
piniels 0:9156e6b6bf46 446 }
piniels 0:9156e6b6bf46 447 }
piniels 0:9156e6b6bf46 448
piniels 0:9156e6b6bf46 449
piniels 0:9156e6b6bf46 450
piniels 0:9156e6b6bf46 451 void motorControl::step(int num_steps, int delay, bool direction) {// steper function: number of steps, direction (0- right, 1- left),
piniels 0:9156e6b6bf46 452
piniels 0:9156e6b6bf46 453 this->_REGI_RUN.B.en = 1;
piniels 0:9156e6b6bf46 454
piniels 0:9156e6b6bf46 455
piniels 0:9156e6b6bf46 456
piniels 0:9156e6b6bf46 457 if(direction)
piniels 0:9156e6b6bf46 458 {
piniels 0:9156e6b6bf46 459 if(getMicroSteps() == 0)
piniels 0:9156e6b6bf46 460 {
piniels 0:9156e6b6bf46 461 this->_REGI_RUN.B.sc = 0x10; // 1 steps positive value
piniels 0:9156e6b6bf46 462 }
piniels 0:9156e6b6bf46 463 else if(getMicroSteps() == 1)
piniels 0:9156e6b6bf46 464 {
piniels 0:9156e6b6bf46 465 this->_REGI_RUN.B.sc = 0x8; // 1 steps positive value
piniels 0:9156e6b6bf46 466 }
piniels 0:9156e6b6bf46 467 else if(getMicroSteps() == 2)
piniels 0:9156e6b6bf46 468 {
piniels 0:9156e6b6bf46 469 this->_REGI_RUN.B.sc = 0x4; // 1 steps positive value
piniels 0:9156e6b6bf46 470 }
piniels 0:9156e6b6bf46 471 else if(getMicroSteps() == 3)
piniels 0:9156e6b6bf46 472 {
piniels 0:9156e6b6bf46 473 this->_REGI_RUN.B.sc = 0x2; // 1 steps positive value
piniels 0:9156e6b6bf46 474 }
piniels 0:9156e6b6bf46 475
piniels 0:9156e6b6bf46 476
piniels 0:9156e6b6bf46 477 }
piniels 0:9156e6b6bf46 478 else
piniels 0:9156e6b6bf46 479 {
piniels 0:9156e6b6bf46 480 if(getMicroSteps() == 0)
piniels 0:9156e6b6bf46 481 {
piniels 0:9156e6b6bf46 482 this->_REGI_RUN.B.sc = 0x30; // 1 steps positive value
piniels 0:9156e6b6bf46 483 }
piniels 0:9156e6b6bf46 484 else if(getMicroSteps() == 1)
piniels 0:9156e6b6bf46 485 {
piniels 0:9156e6b6bf46 486 this->_REGI_RUN.B.sc = 0x38; // 1 steps positive value
piniels 0:9156e6b6bf46 487 }
piniels 0:9156e6b6bf46 488 else if(getMicroSteps() == 2)
piniels 0:9156e6b6bf46 489 {
piniels 0:9156e6b6bf46 490 this->_REGI_RUN.B.sc = 0x3C; // 1 steps positive value
piniels 0:9156e6b6bf46 491 }
piniels 0:9156e6b6bf46 492 else if(getMicroSteps() == 3)
piniels 0:9156e6b6bf46 493 {
piniels 0:9156e6b6bf46 494 this->_REGI_RUN.B.sc = 0x3E; // 1 steps positive value
piniels 0:9156e6b6bf46 495 }
piniels 0:9156e6b6bf46 496 }
piniels 0:9156e6b6bf46 497
piniels 0:9156e6b6bf46 498
piniels 0:9156e6b6bf46 499
piniels 0:9156e6b6bf46 500 for(int i = 0;i < num_steps;i++)
piniels 0:9156e6b6bf46 501 {
piniels 0:9156e6b6bf46 502 setRunRegi(this->_REGI_RUN); //1 steps positive value
piniels 0:9156e6b6bf46 503 wait_ms(delay);
piniels 0:9156e6b6bf46 504 }
piniels 0:9156e6b6bf46 505
piniels 0:9156e6b6bf46 506 this->_REGI_RUN.B.en = 0;
piniels 0:9156e6b6bf46 507 setRunRegi(this->_REGI_RUN); //1 steps positive value
piniels 0:9156e6b6bf46 508
piniels 0:9156e6b6bf46 509 }