1765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang/* 2765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Author: Yevgeniy Kiveisha <yevgeniy.kiveisha@intel.com> 3765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang * Copyright (c) 2014 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#include <unistd.h> 29765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include <stdlib.h> 30765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 31765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang#include "stepmotor.h" 32765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 33765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangusing namespace upm; 34765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 35765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangStepMotor::StepMotor (int dirPin, int stePin) 36765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang : m_pwmStepContext(stePin), m_dirPinCtx(dirPin) { 37765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::Result error = mraa::SUCCESS; 38765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_name = "StepMotor"; 39765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 40765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_stePin = stePin; 41765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_dirPin = dirPin; 42765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 43765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang error = m_dirPinCtx.dir (mraa::DIR_OUT); 44765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (error != mraa::SUCCESS) { 45765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::printError (error); 46765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 47765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 48765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 49765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangvoid 50765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangStepMotor::setSpeed (int speed) { 51765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (speed > MAX_PERIOD) { 52765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_speed = MAX_PERIOD; 53765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 54765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 55765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (speed < MIN_PERIOD) { 56765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_speed = MIN_PERIOD; 57765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 58765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 59765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_speed = speed; 60765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 61765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 62765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangmraa::Result 63765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangStepMotor::stepForward (int ticks) { 64765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang dirForward (); 65765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return move (ticks); 66765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 67765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 68765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangmraa::Result 69765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangStepMotor::stepBackwards (int ticks) { 70765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang dirBackwards (); 71765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return move (ticks); 72765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 73765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 74765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangmraa::Result 75765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangStepMotor::move (int ticks) { 76765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::Result error = mraa::SUCCESS; 77765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 78765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pwmStepContext.enable (1); 79765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang for (int tick = 0; tick < ticks; tick++) { 80765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pwmStepContext.period_us (m_speed); 81765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pwmStepContext.pulsewidth_us (PULSEWIDTH); 82765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 83765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang m_pwmStepContext.enable (0); 84765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 85765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return error; 86765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 87765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 88765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangmraa::Result 89765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangStepMotor::dirForward () { 90765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::Result error = mraa::SUCCESS; 91765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 92765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang error = m_dirPinCtx.write (HIGH); 93765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (error != mraa::SUCCESS) { 94765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::printError (error); 95765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 96765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 97765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return error; 98765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 99765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 100765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhangmraa::Result 101765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun ZhangStepMotor::dirBackwards () { 102765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::Result error = mraa::SUCCESS; 103765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 104765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang error = m_dirPinCtx.write (LOW); 105765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang if (error != mraa::SUCCESS) { 106765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang mraa::printError (error); 107765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang } 108765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang 109765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang return error; 110765adb95dc941c32690d6c43bc08b9d07d197fcbJianxun Zhang} 111