| /* |
| * Authors: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com> |
| * Mihai Tudor Panu <mihai.tudor.panu@intel.com> |
| * Copyright (c) 2014 Intel Corporation. |
| * |
| * Permission is hereby granted, free of charge, to any person obtaining |
| * a copy of this software and associated documentation files (the |
| * "Software"), to deal in the Software without restriction, including |
| * without limitation the rights to use, copy, modify, merge, publish, |
| * distribute, sublicense, and/or sell copies of the Software, and to |
| * permit persons to whom the Software is furnished to do so, subject to |
| * the following conditions: |
| * |
| * The above copyright notice and this permission notice shall be |
| * included in all copies or substantial portions of the Software. |
| * |
| * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, |
| * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF |
| * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND |
| * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE |
| * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION |
| * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION |
| * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. |
| */ |
| |
| #include <iostream> |
| #include <string> |
| #include <stdexcept> |
| #include <stdlib.h> |
| #include <time.h> |
| #include "stepmotor.h" |
| |
| using namespace upm; |
| using namespace std; |
| |
| StepMotor::StepMotor (int dirPin, int stePin, int steps, int enPin) |
| : m_dirPinCtx(dirPin), |
| m_stePinCtx(stePin), |
| m_enPinCtx(0), |
| m_steps(steps) { |
| m_name = "StepMotor"; |
| setSpeed(60); |
| setPosition(0); |
| |
| if (m_dirPinCtx.dir(mraa::DIR_OUT) != mraa::SUCCESS) { |
| throw std::runtime_error(string(__FUNCTION__) + |
| ": Could not initialize dirPin as output"); |
| return; |
| } |
| m_dirPinCtx.useMmap(true); |
| m_dirPinCtx.write(0); |
| |
| if (m_stePinCtx.dir(mraa::DIR_OUT) != mraa::SUCCESS) { |
| throw std::runtime_error(string(__FUNCTION__) + |
| ": Could not initialize stePin as output"); |
| return; |
| } |
| m_stePinCtx.useMmap(true); |
| m_stePinCtx.write(0); |
| |
| if (enPin >= 0) { |
| m_enPinCtx = new mraa::Gpio(enPin); |
| if(m_enPinCtx->dir(mraa::DIR_OUT) != mraa::SUCCESS) { |
| throw std::runtime_error(string(__FUNCTION__) + |
| ": Could not initialize enPin as output"); |
| return; |
| } |
| m_enPinCtx->useMmap(true); |
| enable(true); |
| } |
| } |
| |
| StepMotor::~StepMotor () { |
| if (m_enPinCtx) |
| delete m_enPinCtx; |
| } |
| |
| void |
| StepMotor::enable (bool flag) { |
| if (m_enPinCtx) { |
| m_enPinCtx->write(flag); |
| } else { |
| throw std::runtime_error(string(__FUNCTION__) + |
| ": Enable pin not defined"); |
| } |
| } |
| |
| void |
| StepMotor::setSpeed (int speed) { |
| if (speed > 0) { |
| m_delay = 60000000 / (speed * m_steps); |
| } else { |
| throw std::invalid_argument(string(__FUNCTION__) + |
| ": Parameter must be greater than 0"); |
| } |
| } |
| |
| mraa::Result |
| StepMotor::step (int ticks) { |
| if (ticks < 0) { |
| return stepBackward(abs(ticks)); |
| } else { |
| return stepForward(ticks); |
| } |
| } |
| |
| mraa::Result |
| StepMotor::stepForward (int ticks) { |
| dirForward(); |
| for (int i = 0; i < ticks; i++) { |
| move(); |
| m_position++; |
| delayus(m_delay - MINPULSE_US - OVERHEAD_US); |
| } |
| return mraa::SUCCESS; |
| } |
| |
| mraa::Result |
| StepMotor::stepBackward (int ticks) { |
| dirBackward(); |
| for (int i = 0; i < ticks; i++) { |
| move(); |
| m_position--; |
| delayus(m_delay - MINPULSE_US - OVERHEAD_US); |
| } |
| return mraa::SUCCESS; |
| } |
| |
| void |
| StepMotor::setPosition (int pos) { |
| m_position = pos; |
| } |
| |
| int |
| StepMotor::getPosition () { |
| return m_position; |
| } |
| |
| int |
| StepMotor::getStep () { |
| return m_position < 0 ? m_steps + m_position % m_steps : |
| m_position % m_steps; |
| } |
| |
| void |
| StepMotor::move () { |
| m_stePinCtx.write(1); |
| delayus(MINPULSE_US); |
| m_stePinCtx.write(0); |
| } |
| |
| mraa::Result |
| StepMotor::dirForward () { |
| mraa::Result error = m_dirPinCtx.write(HIGH); |
| if (error != mraa::SUCCESS) { |
| throw std::runtime_error(string(__FUNCTION__) + |
| ": Could not write to dirPin"); |
| } |
| return error; |
| } |
| |
| mraa::Result |
| StepMotor::dirBackward () { |
| mraa::Result error = m_dirPinCtx.write(LOW); |
| if (error != mraa::SUCCESS) { |
| throw std::runtime_error(string(__FUNCTION__) + |
| ": Could not write to dirPin"); |
| } |
| return error; |
| } |
| |
| void upm::StepMotor::delayus (int us) { |
| int diff = 0; |
| struct timespec gettime_now; |
| |
| clock_gettime(CLOCK_REALTIME, &gettime_now); |
| int start = gettime_now.tv_nsec; |
| while (diff < us * 1000) |
| { |
| clock_gettime(CLOCK_REALTIME, &gettime_now); |
| diff = gettime_now.tv_nsec - start; |
| if (diff < 0) |
| diff += 1000000000; |
| } |
| } |