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
31 using namespace upm;
32 using namespace std;
33
34 // constructor for the DC motor(s) mode
L298(int pwmA,int dir1,int dir2)35 L298::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
L298(int stepsPerRev,int en,int i1,int i2,int i3,int i4)76 L298::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
initClock()141 void L298::initClock()
142 {
143 gettimeofday(&m_startTime, NULL);
144 }
145
getMillis()146 uint32_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
~L298()175 L298::~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
setPeriodMS(int ms)198 void 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
enable(bool enable)208 void 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
setSpeed(int speed)221 void 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
setDirection(L298_DIRECTION_T dir)245 void 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
stepperStep()279 void 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
stepperSteps(unsigned int steps)318 void 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