/* * Author: Jon Trulson * Copyright (c) 2016 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 #include #include #include #include "md.h" // static (private) functions static void md_stepper_step(const md_context dev) { assert(dev != NULL); int step = dev->currentStep % 4; switch (step) { case 0: md_write_packet(dev, MD_REG_SET_DIRECTION, 0b0101, MD_NOOP); break; case 1: md_write_packet(dev, MD_REG_SET_DIRECTION, 0b0110, MD_NOOP); break; case 2: md_write_packet(dev, MD_REG_SET_DIRECTION, 0b1010, MD_NOOP); break; case 3: md_write_packet(dev, MD_REG_SET_DIRECTION, 0b1001, MD_NOOP); break; default: // can't happen in this universe break; } } md_context md_init(int bus, uint8_t address) { md_context dev = (md_context)malloc(sizeof(struct _md_context)); if (!dev) return NULL; memset((void *)dev, 0, sizeof(struct _md_context)); dev->i2c = NULL; dev->initialized = false; // make sure MRAA is initialized int mraa_rv; if ((mraa_rv = mraa_init()) != MRAA_SUCCESS) { printf("%s: mraa_init() failed (%d).\n", __FUNCTION__, mraa_rv); md_close(dev); return NULL; } // MRAA contexts... if ( !(dev->i2c = mraa_i2c_init(bus)) ) { printf("%s: mraa_gpio_init() failed\n", __FUNCTION__); md_close(dev); return NULL; } // this board *requires* 100Khz i2c bus only if (mraa_i2c_frequency(dev->i2c, MRAA_I2C_STD)) { printf("%s: mraa_i2c_frequency(MRAA_I2C_STD) failed\n", __FUNCTION__); md_close(dev); return NULL; } if (mraa_i2c_address(dev->i2c, address)) { printf("%s: mraa_i2c_frequency(MRAA_I2C_STD) failed\n", __FUNCTION__); md_close(dev); return NULL; } // default to mode1 stepper operation, 200 steps per rev. md_config_stepper(dev, 200, MD_STEP_MODE1); dev->initialized = true; return dev; } void md_close(md_context dev) { assert(dev != NULL); if (dev->initialized) { md_set_motor_speeds(dev, 0, 0); md_write_packet(dev, MD_REG_SET_DIRECTION, 0, MD_NOOP); } if (dev->i2c) mraa_i2c_stop(dev->i2c); free(dev); } bool md_write_packet(const md_context dev, MD_REG_T reg, uint8_t data1, uint8_t data2) { assert(dev != NULL); uint8_t buf[3]; buf[0] = reg; buf[1] = data1; buf[2] = data2; if (mraa_i2c_write(dev->i2c, buf, 3)) { printf("%s: mraa_i2c_write() failed\n", __FUNCTION__); return false; } // This sleep appears to be required. Without it, writes randomly // fail (no ACK received). This happens most often on the SET_SPEED // packet. I am guessing that there is a timing problem and/or bug // in the motor driver's firmware. upm_delay_us(100); return true; } bool md_set_motor_speeds(const md_context dev, uint8_t speedA, uint8_t speedB) { assert(dev != NULL); return md_write_packet(dev, MD_REG_SET_SPEED, speedA, speedB); } bool md_set_pwm_frequency_prescale(const md_context dev, uint8_t freq) { assert(dev != NULL); return md_write_packet(dev, MD_REG_SET_PWM_FREQ, freq, MD_NOOP); } bool md_set_motor_directions(const md_context dev, MD_DC_DIRECTION_T dirA, MD_DC_DIRECTION_T dirB) { assert(dev != NULL); uint8_t dir = ((dirB & 0x03) << 2) | (dirA & 0x03); return md_write_packet(dev, MD_REG_SET_DIRECTION, dir, MD_NOOP); } bool md_enable_stepper(const md_context dev, MD_STEP_DIRECTION_T dir, uint8_t speed) { assert(dev != NULL); // If mode 2, send the command and return immediately if (dev->stepMode == MD_STEP_MODE2) return md_write_packet(dev, MD_REG_STEPPER_ENABLE, dir, speed); // otherwise, mode 1, setup the basics and start stepping. dev->stepDelay = 60 * 1000 / dev->stepsPerRev / speed; dev->stepDirection = ((dir == MD_STEP_DIR_CW) ? 1 : -1); // seeed says speed should always be 255,255 for stepper operation md_set_motor_speeds(dev, 255, 255); while (dev->totalSteps > 0) { upm_delay_ms(dev->stepDelay); dev->currentStep += dev->stepDirection; if (dev->stepDirection == 1) { if (dev->currentStep >= dev->stepsPerRev) dev->currentStep = 0; } else { if (dev->currentStep <= 0) dev->currentStep = dev->stepsPerRev; } dev->totalSteps--; md_stepper_step(dev); } // and... we're done return true; } bool md_disable_stepper(const md_context dev) { assert(dev != NULL); if (dev->stepMode == MD_STEP_MODE2) return md_write_packet(dev, MD_REG_STEPPER_DISABLE, MD_NOOP, MD_NOOP); // else, mode 1 md_write_packet(dev, MD_REG_SET_DIRECTION, 0, MD_NOOP); return md_set_motor_speeds(dev, 0, 0); } bool md_set_stepper_steps(const md_context dev, unsigned int steps) { assert(dev != NULL); if (dev->stepMode == MD_STEP_MODE2) { if (steps == 0) { // invalid printf("%s: invalid number of steps. " "Valid values are between 1 and 255. \n", __FUNCTION__); return false; } return md_write_packet(dev, MD_REG_STEPPER_NUM_STEPS, steps, MD_NOOP); } // for mode one, just store it for future use by enableStepper() dev->totalSteps = steps; return true; } void md_config_stepper(const md_context dev, unsigned int stepsPerRev, MD_STEP_MODE_T mode) { assert(dev != NULL); dev->stepsPerRev = stepsPerRev; dev->stepMode = mode; dev->currentStep = 0; dev->stepDelay = 0; dev->stepDirection = 1; dev->totalSteps = 0; }