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