1765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang/* 2765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Author: Jon Trulson <jtrulson@ics.com> 3765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Copyright (c) 2014 Intel Corporation. 4765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 5765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Permission is hereby granted, free of charge, to any person obtaining 6765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * a copy of this software and associated documentation files (the 7765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * "Software"), to deal in the Software without restriction, including 8765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * without limitation the rights to use, copy, modify, merge, publish, 9765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * distribute, sublicense, and/or sell copies of the Software, and to 10765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * permit persons to whom the Software is furnished to do so, subject to 11765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * the following conditions: 12765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 13765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * The above copyright notice and this permission notice shall be 14765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * included in all copies or substantial portions of the Software. 15765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 16765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang */ 24765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 25765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <unistd.h> 26765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <iostream> 27765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <string> 28765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <stdexcept> 29765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 30765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include "grovemd.h" 31765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 32765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace upm; 33765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace std; 34765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 35765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 36765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangGroveMD::GroveMD(int bus, uint8_t address) : 37765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_i2c(bus) 38765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 39765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_addr = address; 40765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 41765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // this board *requires* 100Khz i2c bus only 42765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::Result rv; 43765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( (rv = m_i2c.frequency(mraa::I2C_STD)) != mraa::SUCCESS ) 44765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 45765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 46765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": I2c.frequency(I2C_STD) failed"); 47765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 48765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 49765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 50765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_i2c.address(m_addr)) 51765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 52765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 53765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": I2c.address() failed"); 54765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 55765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 56765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 57765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang initClock(); 58765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // default to mode1 stepper operation, 200 steps per rev. 59765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang configStepper(200, STEP_MODE1); 60765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 61765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 62765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangGroveMD::~GroveMD() 63765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 64765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setMotorSpeeds(0, 0); 65765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang writePacket(SET_DIRECTION, 0, GROVEMD_NOOP); 66765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 67765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 68765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool GroveMD::writePacket(REG_T reg, uint8_t data1, uint8_t data2) 69765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 70765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t buf[3]; 71765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 72765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang buf[0] = reg; 73765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang buf[1] = data1; 74765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang buf[2] = data2; 75765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 76765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( m_i2c.write(buf, 3) != mraa::SUCCESS ) 77765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 78765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::runtime_error(std::string(__FUNCTION__) + 79765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": I2c.write() failed"); 80765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return false; 81765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 82765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 83765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // This sleep appears to be required. Without it, writes randomly 84765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // fail (no ACK received). This happens most often on the SET_SPEED 85765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // packet. I am guessing that there is a timing problem and/or bug 86765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // in the motor driver's firmware. 87765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 88765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang usleep(100); 89765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 90765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return true; 91765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 92765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 93765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool GroveMD::setMotorSpeeds(uint8_t speedA, uint8_t speedB) 94765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 95765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writePacket(SET_SPEED, speedA, speedB); 96765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 97765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 98765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool GroveMD::setPWMFrequencyPrescale(uint8_t freq) 99765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 100765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writePacket(SET_PWM_FREQ, freq, GROVEMD_NOOP); 101765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 102765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 103765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool GroveMD::setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB) 104765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 105765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03); 106765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writePacket(SET_DIRECTION, dir, GROVEMD_NOOP); 107765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 108765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 109765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool GroveMD::enableStepper(STEP_DIRECTION_T dir, uint8_t speed) 110765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 111765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // If mode 2, send the command and return immediately 112765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepMode == STEP_MODE2) 113765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writePacket(STEPPER_ENABLE, dir, speed); 114765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 115765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // otherwise, mode 1, setup the basics and start stepping. 116765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 117765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDelay = 60 * 1000 / m_stepsPerRev / speed; 118765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDirection = ((dir == STEP_DIR_CW) ? 1 : -1); 119765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 120765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // seeed says speed should always be 255,255 for stepper operation 121765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setMotorSpeeds(255, 255); 122765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 123765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang while (m_totalSteps > 0) 124765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 125765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (getMillis() >= m_stepDelay) 126765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 127765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // reset the clock 128765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang initClock(); 129765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 130765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_currentStep += m_stepDirection; 131765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 132765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepDirection == 1) 133765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 134765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_currentStep >= m_stepsPerRev) 135765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_currentStep = 0; 136765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 137765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 138765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 139765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_currentStep <= 0) 140765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_currentStep = m_stepsPerRev; 141765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 142765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 143765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_totalSteps--; 144765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang stepperStep(); 145765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 146765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 147765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 148765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // and... we're done 149765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return true; 150765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 151765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 152765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool GroveMD::disableStepper() 153765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 154765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepMode == STEP_MODE2) 155765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writePacket(STEPPER_DISABLE, GROVEMD_NOOP, GROVEMD_NOOP); 156765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 157765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // else, mode 1 158765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang writePacket(SET_DIRECTION, 0, GROVEMD_NOOP); 159765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return setMotorSpeeds(0, 0); 160765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 161765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 162765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangbool GroveMD::setStepperSteps(unsigned int steps) 163765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 164765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepMode == STEP_MODE2) 165765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 166765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (steps == 0) 167765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 168765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // invalid 169765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::out_of_range(std::string(__FUNCTION__) + 170765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": invalid number of steps. " + 171765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang "Valid values are between 1 and 255."); 172765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return false; 173765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 174765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return writePacket(STEPPER_NUM_STEPS, steps, GROVEMD_NOOP); 175765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 176765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 177765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // for mode one, just store it for future use by enableStepper() 178765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_totalSteps = steps; 179765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return true; 180765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 181765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 182765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid GroveMD::initClock() 183765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 184765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gettimeofday(&m_startTime, NULL); 185765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 186765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 187765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhanguint32_t GroveMD::getMillis() 188765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 189765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang struct timeval elapsed, now; 190765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint32_t elapse; 191765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 192765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // get current time 193765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gettimeofday(&now, NULL); 194765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 195765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // compute the delta since m_startTime 196765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 ) 197765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 198765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_usec += 1000000; 199765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1; 200765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 201765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 202765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 203765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec; 204765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 205765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 206765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000)); 207765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 208765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // never return 0 209765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (elapse == 0) 210765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapse = 1; 211765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 212765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return elapse; 213765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 214765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 215765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid GroveMD::configStepper(unsigned int stepsPerRev, STEP_MODE_T mode) 216765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 217765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepsPerRev = stepsPerRev; 218765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepMode = mode; 219765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_currentStep = 0; 220765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDelay = 0; 221765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDirection = 1; 222765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_totalSteps = 0; 223765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 224765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 225765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid GroveMD::stepperStep() 226765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 227765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang int step = m_currentStep % 4; 228765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 229765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang switch (step) 230765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 231765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 0: 232765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang writePacket(SET_DIRECTION, 0b0101, GROVEMD_NOOP); 233765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 234765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 1: 235765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang writePacket(SET_DIRECTION, 0b0110, GROVEMD_NOOP); 236765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 237765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 2: 238765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang writePacket(SET_DIRECTION, 0b1010, GROVEMD_NOOP); 239765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 240765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 3: 241765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang writePacket(SET_DIRECTION, 0b1001, GROVEMD_NOOP); 242765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 243765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 244765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 245