1/*
2 * Author: Jon Trulson <jtrulson@ics.com>
3 * Copyright (c) 2015 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 <iostream>
26#include <string>
27#include <stdexcept>
28
29#include "l298.h"
30
31using namespace upm;
32using namespace std;
33
34// constructor for the DC motor(s) mode
35L298::L298(int pwmA, int dir1, int dir2)
36{
37  // No stepper in this mode
38  m_stepper = false;
39
40  // disable until complete
41  m_motor = false;
42
43  if ( !(m_pwm = mraa_pwm_init(pwmA)) )
44    {
45      throw std::invalid_argument(std::string(__FUNCTION__) +
46                                  ": mraa_pwm_init() failed, invalid pin?");
47      return;
48    }
49
50  if ( !(m_dir1 = mraa_gpio_init(dir1)) )
51    {
52      throw std::invalid_argument(std::string(__FUNCTION__) +
53                                  ": mraa_gpio_init(dir1) failed, invalid pin?");
54      mraa_pwm_close(m_pwm);
55      return;
56    }
57  mraa_gpio_dir(m_dir1, MRAA_GPIO_OUT);
58
59  if ( !(m_dir2 = mraa_gpio_init(dir2)) )
60    {
61      throw std::invalid_argument(std::string(__FUNCTION__) +
62                                  ": mraa_gpio_init(dir2) failed, invalid pin?");
63      mraa_pwm_close(m_pwm);
64      mraa_gpio_close(m_dir1);
65      return;
66    }
67  mraa_gpio_dir(m_dir2, MRAA_GPIO_OUT);
68
69  setPeriodMS(L298_DEFAULT_PWM_PERIOD);
70  setDirection(DIR_NONE);
71  setSpeed(0);
72  m_motor = true;
73}
74
75// constructor for the stepper mode
76L298::L298(int stepsPerRev, int en, int i1, int i2, int i3, int i4)
77{
78  // no DC motors in this mode
79  m_motor = false;
80
81  // disable until complete
82  m_stepper = false;
83
84  m_stepsPerRev = stepsPerRev;
85  m_currentStep = 0;
86  m_stepDelay = 0;
87  m_stepDirection = 1;          // default is forward
88
89  // init the gpio's we will need
90  if ( !(m_stepEnable = mraa_gpio_init(en)) )
91    {
92      throw std::invalid_argument(std::string(__FUNCTION__) +
93                                  ": mraa_gpio_init(en) failed, invalid pin?");
94      return;
95    }
96  mraa_gpio_dir(m_stepEnable, MRAA_GPIO_OUT);
97
98  if ( !(m_stepI1 = mraa_gpio_init(i1)) )
99    {
100      throw std::invalid_argument(std::string(__FUNCTION__) +
101                                  ": mraa_gpio_init(i1) failed, invalid pin?");
102      return;
103    }
104  mraa_gpio_dir(m_stepI1, MRAA_GPIO_OUT);
105
106  if ( !(m_stepI2 = mraa_gpio_init(i2)) )
107    {
108      throw std::invalid_argument(std::string(__FUNCTION__) +
109                                  ": mraa_gpio_init(i2) failed, invalid pin?");
110      mraa_gpio_close(m_stepI1);
111      return;
112    }
113  mraa_gpio_dir(m_stepI2, MRAA_GPIO_OUT);
114
115  if ( !(m_stepI3 = mraa_gpio_init(i3)) )
116    {
117      throw std::invalid_argument(std::string(__FUNCTION__) +
118                                  ": mraa_gpio_init(i3) failed, invalid pin?");
119      mraa_gpio_close(m_stepI1);
120      mraa_gpio_close(m_stepI2);
121      return;
122    }
123  mraa_gpio_dir(m_stepI3, MRAA_GPIO_OUT);
124
125  if ( !(m_stepI4 = mraa_gpio_init(i4)) )
126    {
127      throw std::invalid_argument(std::string(__FUNCTION__) +
128                                  ": mraa_gpio_init(i4) failed, invalid pin?");
129      mraa_gpio_close(m_stepI1);
130      mraa_gpio_close(m_stepI2);
131      mraa_gpio_close(m_stepI3);
132
133      return;
134    }
135  mraa_gpio_dir(m_stepI4, MRAA_GPIO_OUT);
136
137  m_stepper = true;
138}
139
140
141void L298::initClock()
142{
143  gettimeofday(&m_startTime, NULL);
144}
145
146uint32_t L298::getMillis()
147{
148  struct timeval elapsed, now;
149  uint32_t elapse;
150
151  // get current time
152  gettimeofday(&now, NULL);
153
154  // compute the delta since m_startTime
155  if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 )
156    {
157      elapsed.tv_usec += 1000000;
158      elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1;
159    }
160  else
161    {
162      elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec;
163    }
164
165  elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000));
166
167  // never return 0
168  if (elapse == 0)
169    elapse = 1;
170
171  return elapse;
172}
173
174
175L298::~L298()
176{
177  if (m_stepper)
178    {
179      enable(false);
180      mraa_gpio_close(m_stepEnable);
181      mraa_gpio_close(m_stepI1);
182      mraa_gpio_close(m_stepI2);
183      mraa_gpio_close(m_stepI3);
184      mraa_gpio_close(m_stepI4);
185    }
186
187  if (m_motor)
188    {
189      setDirection(DIR_NONE);
190      setSpeed(0);
191      enable(false);
192      mraa_pwm_close(m_pwm);
193      mraa_gpio_close(m_dir1);
194      mraa_gpio_close(m_dir2);
195    }
196}
197
198void L298::setPeriodMS(int ms)
199{
200  if (m_motor)
201    {
202      if (mraa_pwm_period_ms(m_pwm, ms) != MRAA_SUCCESS)
203        throw std::invalid_argument(std::string(__FUNCTION__) +
204                                    ": mraa_pwm_period_ms() failed");
205    }
206}
207
208void L298::enable(bool enable)
209{
210  if (m_motor)
211    {
212      mraa_pwm_enable(m_pwm, ((enable) ? 1 : 0));
213    }
214
215  if (m_stepper)
216    {
217      mraa_gpio_write(m_stepEnable, ((enable) ? 1 : 0));
218    }
219}
220
221void L298::setSpeed(int speed)
222{
223  if (m_motor)
224    {
225      if (speed < 0)
226        speed = 0;
227
228      if (speed > 100)
229        speed = 100;
230
231      float percent = float(speed) / 100.0;
232
233      if (m_motor)
234        {
235          mraa_pwm_write(m_pwm, percent);
236        }
237    }
238
239  if (m_stepper)
240    {
241      m_stepDelay = 60 * 1000 / m_stepsPerRev / speed;
242    }
243}
244
245void L298::setDirection(L298_DIRECTION_T dir)
246{
247  if (m_motor)
248    {
249      if (dir & 0x01)
250        mraa_gpio_write(m_dir1, 1);
251      else
252        mraa_gpio_write(m_dir1, 0);
253
254      if (dir & 0x02)
255        mraa_gpio_write(m_dir2, 1);
256      else
257        mraa_gpio_write(m_dir2, 0);
258    }
259
260  if (m_stepper)
261    {
262      switch (dir)
263        {
264        case DIR_CW:
265          m_stepDirection = 1;
266          break;
267        case DIR_CCW:
268          m_stepDirection = -1;
269          break;
270        default:                // default to 1 if DIR_NONE specified
271          m_stepDirection = 1;
272          break;
273        }
274    }
275
276
277}
278
279void L298::stepperStep()
280{
281  int step = m_currentStep % 4;
282
283  //   Step I0 I1 I2 I3
284  //     1  1  0  1  0
285  //     2  0  1  1  0
286  //     3  0  1  0  1
287  //     4  1  0  0  1
288
289  switch (step)
290    {
291    case 0:    // 1010
292      mraa_gpio_write(m_stepI1, 1);
293      mraa_gpio_write(m_stepI2, 0);
294      mraa_gpio_write(m_stepI3, 1);
295      mraa_gpio_write(m_stepI4, 0);
296      break;
297    case 1:    // 0110
298      mraa_gpio_write(m_stepI1, 0);
299      mraa_gpio_write(m_stepI2, 1);
300      mraa_gpio_write(m_stepI3, 1);
301      mraa_gpio_write(m_stepI4, 0);
302      break;
303    case 2:    //0101
304      mraa_gpio_write(m_stepI1, 0);
305      mraa_gpio_write(m_stepI2, 1);
306      mraa_gpio_write(m_stepI3, 0);
307      mraa_gpio_write(m_stepI4, 1);
308      break;
309    case 3:    //1001
310      mraa_gpio_write(m_stepI1, 1);
311      mraa_gpio_write(m_stepI2, 0);
312      mraa_gpio_write(m_stepI3, 0);
313      mraa_gpio_write(m_stepI4, 1);
314      break;
315    }
316}
317
318void L298::stepperSteps(unsigned int steps)
319{
320  while (steps > 0)
321    {
322      if (getMillis() >= m_stepDelay)
323        {
324          // reset the clock
325          initClock();
326
327          m_currentStep += m_stepDirection;
328
329          if (m_stepDirection == 1)
330            {
331              if (m_currentStep >= m_stepsPerRev)
332                m_currentStep = 0;
333            }
334          else
335            {
336              if (m_currentStep <= 0)
337                m_currentStep = m_stepsPerRev;
338            }
339
340          steps--;
341          stepperStep();
342          // cerr << "STEPNUM: " << m_currentStep << endl;
343        }
344    }
345}
346