1/*
2 * Author: Jon Trulson <jtrulson@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
32using namespace upm;
33using namespace std;
34
35
36GroveMD::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
62GroveMD::~GroveMD()
63{
64  setMotorSpeeds(0, 0);
65  writePacket(SET_DIRECTION, 0, GROVEMD_NOOP);
66}
67
68bool 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
93bool GroveMD::setMotorSpeeds(uint8_t speedA, uint8_t speedB)
94{
95  return writePacket(SET_SPEED, speedA, speedB);
96}
97
98bool GroveMD::setPWMFrequencyPrescale(uint8_t freq)
99{
100  return writePacket(SET_PWM_FREQ, freq, GROVEMD_NOOP);
101}
102
103bool 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
109bool 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
152bool 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
162bool 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
182void GroveMD::initClock()
183{
184  gettimeofday(&m_startTime, NULL);
185}
186
187uint32_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
215void 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
225void 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