Home | History | Annotate | Download | only in grovemd 
      1 /*
      2  * Author: Jon Trulson <jtrulson (at) ics.com>
      3  * Copyright (c) 2014 Intel Corporation.
      4  *
      5  * Permission is hereby granted, free of charge, to any person obtaining
      6  * a copy of this software and associated documentation files (the
      7  * "Software"), to deal in the Software without restriction, including
      8  * without limitation the rights to use, copy, modify, merge, publish,
      9  * distribute, sublicense, and/or sell copies of the Software, and to
     10  * permit persons to whom the Software is furnished to do so, subject to
     11  * the following conditions:
     12  *
     13  * The above copyright notice and this permission notice shall be
     14  * included in all copies or substantial portions of the Software.
     15  *
     16  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
     17  * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
     18  * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
     19  * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
     20  * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
     21  * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
     22  * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
     23  */
     24 
     25 #include <unistd.h>
     26 #include <iostream>
     27 #include <string>
     28 #include <stdexcept>
     29 
     30 #include "grovemd.h"
     31 
     32 using namespace upm;
     33 using namespace std;
     34 
     35 
     36 GroveMD::GroveMD(int bus, uint8_t address) :
     37   m_i2c(bus)
     38 {
     39   m_addr = address;
     40 
     41   // this board *requires* 100Khz i2c bus only
     42   mraa::Result rv;
     43   if ( (rv = m_i2c.frequency(mraa::I2C_STD)) != mraa::SUCCESS )
     44     {
     45       throw std::invalid_argument(std::string(__FUNCTION__) +
     46                                   ": I2c.frequency(I2C_STD) failed");
     47       return;
     48     }
     49 
     50   if (m_i2c.address(m_addr))
     51     {
     52       throw std::runtime_error(std::string(__FUNCTION__) +
     53                                ": I2c.address() failed");
     54       return;
     55     }
     56 
     57   initClock();
     58   // default to mode1 stepper operation, 200 steps per rev.
     59   configStepper(200, STEP_MODE1);
     60 }
     61 
     62 GroveMD::~GroveMD()
     63 {
     64   setMotorSpeeds(0, 0);
     65   writePacket(SET_DIRECTION, 0, GROVEMD_NOOP);
     66 }
     67 
     68 bool GroveMD::writePacket(REG_T reg, uint8_t data1, uint8_t data2)
     69 {
     70   uint8_t buf[3];
     71 
     72   buf[0] = reg;
     73   buf[1] = data1;
     74   buf[2] = data2;
     75 
     76   if ( m_i2c.write(buf, 3) != mraa::SUCCESS )
     77     {
     78       throw std::runtime_error(std::string(__FUNCTION__) +
     79                                ": I2c.write() failed");
     80       return false;
     81     }
     82 
     83   // This sleep appears to be required.  Without it, writes randomly
     84   // fail (no ACK received).  This happens most often on the SET_SPEED
     85   // packet.  I am guessing that there is a timing problem and/or bug
     86   // in the motor driver's firmware.
     87 
     88   usleep(100);
     89 
     90   return true;
     91 }
     92 
     93 bool GroveMD::setMotorSpeeds(uint8_t speedA, uint8_t speedB)
     94 {
     95   return writePacket(SET_SPEED, speedA, speedB);
     96 }
     97 
     98 bool GroveMD::setPWMFrequencyPrescale(uint8_t freq)
     99 {
    100   return writePacket(SET_PWM_FREQ, freq, GROVEMD_NOOP);
    101 }
    102 
    103 bool GroveMD::setMotorDirections(DC_DIRECTION_T dirA, DC_DIRECTION_T dirB)
    104 {
    105   uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03);
    106   return writePacket(SET_DIRECTION, dir, GROVEMD_NOOP);
    107 }
    108 
    109 bool GroveMD::enableStepper(STEP_DIRECTION_T dir, uint8_t speed)
    110 {
    111   // If mode 2, send the command and return immediately
    112   if (m_stepMode == STEP_MODE2)
    113     return writePacket(STEPPER_ENABLE, dir, speed);
    114 
    115   // otherwise, mode 1, setup the basics and start stepping.
    116 
    117   m_stepDelay = 60 * 1000 / m_stepsPerRev / speed;
    118   m_stepDirection = ((dir == STEP_DIR_CW) ? 1 : -1);
    119 
    120   // seeed says speed should always be 255,255 for stepper operation
    121   setMotorSpeeds(255, 255);
    122 
    123   while (m_totalSteps > 0)
    124     {
    125       if (getMillis() >= m_stepDelay)
    126         {
    127           // reset the clock
    128           initClock();
    129 
    130           m_currentStep += m_stepDirection;
    131 
    132           if (m_stepDirection == 1)
    133             {
    134               if (m_currentStep >= m_stepsPerRev)
    135                 m_currentStep = 0;
    136             }
    137           else
    138             {
    139               if (m_currentStep <= 0)
    140                 m_currentStep = m_stepsPerRev;
    141             }
    142 
    143           m_totalSteps--;
    144           stepperStep();
    145         }
    146     }
    147 
    148   // and... we're done
    149   return true;
    150 }
    151 
    152 bool GroveMD::disableStepper()
    153 {
    154   if (m_stepMode == STEP_MODE2)
    155     return writePacket(STEPPER_DISABLE, GROVEMD_NOOP, GROVEMD_NOOP);
    156 
    157   // else, mode 1
    158   writePacket(SET_DIRECTION, 0, GROVEMD_NOOP);
    159   return setMotorSpeeds(0, 0);
    160 }
    161 
    162 bool GroveMD::setStepperSteps(unsigned int steps)
    163 {
    164   if (m_stepMode == STEP_MODE2)
    165     {
    166       if (steps == 0)
    167         {
    168           // invalid
    169           throw std::out_of_range(std::string(__FUNCTION__) +
    170                                   ": invalid number of steps.  " +
    171                                   "Valid values are between 1 and 255.");
    172           return false;
    173         }
    174       return writePacket(STEPPER_NUM_STEPS, steps, GROVEMD_NOOP);
    175     }
    176 
    177   // for mode one, just store it for future use by enableStepper()
    178   m_totalSteps = steps;
    179   return true;
    180 }
    181 
    182 void GroveMD::initClock()
    183 {
    184   gettimeofday(&m_startTime, NULL);
    185 }
    186 
    187 uint32_t GroveMD::getMillis()
    188 {
    189   struct timeval elapsed, now;
    190   uint32_t elapse;
    191 
    192   // get current time
    193   gettimeofday(&now, NULL);
    194 
    195   // compute the delta since m_startTime
    196   if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 )
    197     {
    198       elapsed.tv_usec += 1000000;
    199       elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1;
    200     }
    201   else
    202     {
    203       elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec;
    204     }
    205 
    206   elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000));
    207 
    208   // never return 0
    209   if (elapse == 0)
    210     elapse = 1;
    211 
    212   return elapse;
    213 }
    214 
    215 void GroveMD::configStepper(unsigned int stepsPerRev, STEP_MODE_T mode)
    216 {
    217   m_stepsPerRev = stepsPerRev;
    218   m_stepMode = mode;
    219   m_currentStep = 0;
    220   m_stepDelay = 0;
    221   m_stepDirection = 1;
    222   m_totalSteps = 0;
    223 }
    224 
    225 void GroveMD::stepperStep()
    226 {
    227   int step = m_currentStep % 4;
    228 
    229   switch (step)
    230     {
    231     case 0:
    232       writePacket(SET_DIRECTION, 0b0101, GROVEMD_NOOP);
    233       break;
    234     case 1:
    235       writePacket(SET_DIRECTION, 0b0110, GROVEMD_NOOP);
    236       break;
    237     case 2:
    238       writePacket(SET_DIRECTION, 0b1010, GROVEMD_NOOP);
    239       break;
    240     case 3:
    241       writePacket(SET_DIRECTION, 0b1001, GROVEMD_NOOP);
    242       break;
    243     }
    244 }
    245