1765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang/* 2765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Author: Jon Trulson <jtrulson@ics.com> 3765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Copyright (c) 2015 Intel Corporation. 4765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 5765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Permission is hereby granted, free of charge, to any person obtaining 6765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * a copy of this software and associated documentation files (the 7765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * "Software"), to deal in the Software without restriction, including 8765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * without limitation the rights to use, copy, modify, merge, publish, 9765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * distribute, sublicense, and/or sell copies of the Software, and to 10765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * permit persons to whom the Software is furnished to do so, subject to 11765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * the following conditions: 12765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 13765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * The above copyright notice and this permission notice shall be 14765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * included in all copies or substantial portions of the Software. 15765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * 16765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, 17765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF 18765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND 19765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 20765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 21765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION 22765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 23765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang */ 24765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 25765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <iostream> 26765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <string> 27765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <stdexcept> 28765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 29765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include "l298.h" 30765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 31765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace upm; 32765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace std; 33765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 34765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang// constructor for the DC motor(s) mode 35765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangL298::L298(int pwmA, int dir1, int dir2) 36765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 37765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // No stepper in this mode 38765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepper = false; 39765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 40765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // disable until complete 41765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_motor = false; 42765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 43765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( !(m_pwm = mraa_pwm_init(pwmA)) ) 44765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 45765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 46765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_pwm_init() failed, invalid pin?"); 47765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 48765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 49765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 50765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( !(m_dir1 = mraa_gpio_init(dir1)) ) 51765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 52765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 53765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_gpio_init(dir1) failed, invalid pin?"); 54765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_pwm_close(m_pwm); 55765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 56765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 57765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_dir(m_dir1, MRAA_GPIO_OUT); 58765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 59765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( !(m_dir2 = mraa_gpio_init(dir2)) ) 60765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 61765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 62765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_gpio_init(dir2) failed, invalid pin?"); 63765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_pwm_close(m_pwm); 64765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_dir1); 65765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 66765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 67765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_dir(m_dir2, MRAA_GPIO_OUT); 68765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 69765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setPeriodMS(L298_DEFAULT_PWM_PERIOD); 70765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setDirection(DIR_NONE); 71765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setSpeed(0); 72765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_motor = true; 73765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 74765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 75765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang// constructor for the stepper mode 76765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangL298::L298(int stepsPerRev, int en, int i1, int i2, int i3, int i4) 77765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 78765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // no DC motors in this mode 79765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_motor = false; 80765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 81765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // disable until complete 82765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepper = false; 83765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 84765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepsPerRev = stepsPerRev; 85765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_currentStep = 0; 86765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDelay = 0; 87765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDirection = 1; // default is forward 88765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 89765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // init the gpio's we will need 90765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( !(m_stepEnable = mraa_gpio_init(en)) ) 91765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 92765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 93765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_gpio_init(en) failed, invalid pin?"); 94765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 95765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 96765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_dir(m_stepEnable, MRAA_GPIO_OUT); 97765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 98765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( !(m_stepI1 = mraa_gpio_init(i1)) ) 99765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 100765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 101765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_gpio_init(i1) failed, invalid pin?"); 102765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 103765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 104765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_dir(m_stepI1, MRAA_GPIO_OUT); 105765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 106765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( !(m_stepI2 = mraa_gpio_init(i2)) ) 107765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 108765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 109765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_gpio_init(i2) failed, invalid pin?"); 110765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI1); 111765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 112765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 113765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_dir(m_stepI2, MRAA_GPIO_OUT); 114765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 115765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( !(m_stepI3 = mraa_gpio_init(i3)) ) 116765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 117765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 118765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_gpio_init(i3) failed, invalid pin?"); 119765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI1); 120765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI2); 121765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 122765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 123765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_dir(m_stepI3, MRAA_GPIO_OUT); 124765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 125765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if ( !(m_stepI4 = mraa_gpio_init(i4)) ) 126765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 127765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 128765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_gpio_init(i4) failed, invalid pin?"); 129765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI1); 130765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI2); 131765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI3); 132765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 133765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return; 134765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 135765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_dir(m_stepI4, MRAA_GPIO_OUT); 136765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 137765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepper = true; 138765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 139765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 140765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 141765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid L298::initClock() 142765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 143765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gettimeofday(&m_startTime, NULL); 144765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 145765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 146765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhanguint32_t L298::getMillis() 147765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 148765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang struct timeval elapsed, now; 149765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang uint32_t elapse; 150765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 151765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // get current time 152765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang gettimeofday(&now, NULL); 153765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 154765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // compute the delta since m_startTime 155765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if( (elapsed.tv_usec = now.tv_usec - m_startTime.tv_usec) < 0 ) 156765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 157765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_usec += 1000000; 158765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec - 1; 159765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 160765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 161765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 162765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapsed.tv_sec = now.tv_sec - m_startTime.tv_sec; 163765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 164765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 165765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapse = (uint32_t)((elapsed.tv_sec * 1000) + (elapsed.tv_usec / 1000)); 166765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 167765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // never return 0 168765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (elapse == 0) 169765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang elapse = 1; 170765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 171765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return elapse; 172765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 173765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 174765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 175765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangL298::~L298() 176765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 177765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepper) 178765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 179765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang enable(false); 180765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepEnable); 181765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI1); 182765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI2); 183765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI3); 184765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_stepI4); 185765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 186765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 187765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_motor) 188765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 189765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setDirection(DIR_NONE); 190765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang setSpeed(0); 191765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang enable(false); 192765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_pwm_close(m_pwm); 193765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_dir1); 194765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_close(m_dir2); 195765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 196765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 197765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 198765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid L298::setPeriodMS(int ms) 199765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 200765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_motor) 201765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 202765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (mraa_pwm_period_ms(m_pwm, ms) != MRAA_SUCCESS) 203765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang throw std::invalid_argument(std::string(__FUNCTION__) + 204765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang ": mraa_pwm_period_ms() failed"); 205765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 206765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 207765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 208765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid L298::enable(bool enable) 209765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 210765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_motor) 211765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 212765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_pwm_enable(m_pwm, ((enable) ? 1 : 0)); 213765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 214765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 215765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepper) 216765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 217765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepEnable, ((enable) ? 1 : 0)); 218765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 219765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 220765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 221765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid L298::setSpeed(int speed) 222765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 223765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_motor) 224765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 225765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (speed < 0) 226765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang speed = 0; 227765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 228765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (speed > 100) 229765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang speed = 100; 230765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 231765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang float percent = float(speed) / 100.0; 232765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 233765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_motor) 234765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 235765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_pwm_write(m_pwm, percent); 236765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 237765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 238765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 239765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepper) 240765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 241765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDelay = 60 * 1000 / m_stepsPerRev / speed; 242765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 243765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 244765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 245765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid L298::setDirection(L298_DIRECTION_T dir) 246765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 247765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_motor) 248765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 249765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (dir & 0x01) 250765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_dir1, 1); 251765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 252765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_dir1, 0); 253765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 254765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (dir & 0x02) 255765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_dir2, 1); 256765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 257765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_dir2, 0); 258765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 259765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 260765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepper) 261765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 262765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang switch (dir) 263765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 264765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case DIR_CW: 265765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDirection = 1; 266765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 267765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case DIR_CCW: 268765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDirection = -1; 269765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 270765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang default: // default to 1 if DIR_NONE specified 271765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stepDirection = 1; 272765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 273765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 274765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 275765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 276765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 277765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 278765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 279765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid L298::stepperStep() 280765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 281765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang int step = m_currentStep % 4; 282765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 283765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // Step I0 I1 I2 I3 284765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // 1 1 0 1 0 285765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // 2 0 1 1 0 286765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // 3 0 1 0 1 287765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // 4 1 0 0 1 288765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 289765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang switch (step) 290765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 291765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 0: // 1010 292765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI1, 1); 293765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI2, 0); 294765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI3, 1); 295765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI4, 0); 296765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 297765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 1: // 0110 298765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI1, 0); 299765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI2, 1); 300765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI3, 1); 301765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI4, 0); 302765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 303765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 2: //0101 304765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI1, 0); 305765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI2, 1); 306765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI3, 0); 307765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI4, 1); 308765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 309765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang case 3: //1001 310765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI1, 1); 311765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI2, 0); 312765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI3, 0); 313765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa_gpio_write(m_stepI4, 1); 314765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang break; 315765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 316765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 317765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 318765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid L298::stepperSteps(unsigned int steps) 319765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang{ 320765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang while (steps > 0) 321765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 322765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (getMillis() >= m_stepDelay) 323765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 324765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // reset the clock 325765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang initClock(); 326765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 327765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_currentStep += m_stepDirection; 328765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 329765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_stepDirection == 1) 330765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 331765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_currentStep >= m_stepsPerRev) 332765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_currentStep = 0; 333765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 334765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang else 335765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang { 336765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (m_currentStep <= 0) 337765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_currentStep = m_stepsPerRev; 338765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 339765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 340765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang steps--; 341765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang stepperStep(); 342765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang // cerr << "STEPNUM: " << m_currentStep << endl; 343765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 344765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 345765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 346