From 06e390b5e98b5e83646a8cd5f75b6b546000fc85 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 30 Apr 2013 16:55:12 -0400 Subject: Added MD25 I2C motor controller driver. --- apps/drivers/device/i2c.h | 9 + apps/drivers/md25/MD25.cpp | 553 ++++++++++++++++++++++++++++++++++++++++ apps/drivers/md25/MD25.hpp | 293 +++++++++++++++++++++ apps/drivers/md25/Makefile | 43 ++++ apps/drivers/md25/md25_main.cpp | 264 +++++++++++++++++++ 5 files changed, 1162 insertions(+) create mode 100644 apps/drivers/md25/MD25.cpp create mode 100644 apps/drivers/md25/MD25.hpp create mode 100644 apps/drivers/md25/Makefile create mode 100644 apps/drivers/md25/md25_main.cpp (limited to 'apps') diff --git a/apps/drivers/device/i2c.h b/apps/drivers/device/i2c.h index 66c34dd7c..cc1f4e4d9 100644 --- a/apps/drivers/device/i2c.h +++ b/apps/drivers/device/i2c.h @@ -53,6 +53,15 @@ namespace device __EXPORT class __EXPORT I2C : public CDev { +public: + + /** + * Get the address + */ + uint16_t get_address() { + return _address; + } + protected: /** * The number of times a read or write operation will be retried on diff --git a/apps/drivers/md25/MD25.cpp b/apps/drivers/md25/MD25.cpp new file mode 100644 index 000000000..92778b109 --- /dev/null +++ b/apps/drivers/md25/MD25.cpp @@ -0,0 +1,553 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file md25.cpp + * + * Driver for MD25 I2C Motor Driver + * + * references: + * http://www.robot-electronics.co.uk/htm/md25tech.htm + * http://www.robot-electronics.co.uk/files/rpi_md25.c + * + */ + +#include "MD25.hpp" +#include +#include + +#include +#include + +// registers +enum { + // RW: read/write + // R: read + REG_SPEED1_RW = 0, + REG_SPEED2_RW, + REG_ENC1A_R, + REG_ENC1B_R, + REG_ENC1C_R, + REG_ENC1D_R, + REG_ENC2A_R, + REG_ENC2B_R, + REG_ENC2C_R, + REG_ENC2D_R, + REG_BATTERY_VOLTS_R, + REG_MOTOR1_CURRENT_R, + REG_MOTOR2_CURRENT_R, + REG_SW_VERSION_R, + REG_ACCEL_RATE_RW, + REG_MODE_RW, + REG_COMMAND_RW, +}; + +MD25::MD25(const char *deviceName, int bus, + uint16_t address, uint32_t speed) : + I2C("MD25", deviceName, bus, address, speed), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _version(0), + _motor1Speed(0), + _motor2Speed(0), + _revolutions1(0), + _revolutions2(0), + _batteryVoltage(0), + _motor1Current(0), + _motor2Current(0), + _motorAccel(0), + _mode(MODE_UNSIGNED_SPEED), + _command(CMD_RESET_ENCODERS) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // if initialization fails raise an error, unless + // probing + int ret = I2C::init(); + + if (ret != OK) { + warnc(ret, "I2C::init failed for bus: %d address: %d\n", bus, address); + } + + // setup default settings, reset encoders + setMotor1Speed(0); + setMotor2Speed(0); + resetEncoders(); + _setMode(MD25::MODE_UNSIGNED_SPEED); + setSpeedRegulation(true); + setTimeout(true); +} + +MD25::~MD25() +{ +} + +int MD25::readData() +{ + uint8_t sendBuf[1]; + sendBuf[0] = REG_SPEED1_RW; + uint8_t recvBuf[17]; + int ret = transfer(sendBuf, sizeof(sendBuf), + recvBuf, sizeof(recvBuf)); + + if (ret == OK) { + _version = recvBuf[REG_SW_VERSION_R]; + _motor1Speed = _uint8ToNorm(recvBuf[REG_SPEED1_RW]); + _motor2Speed = _uint8ToNorm(recvBuf[REG_SPEED2_RW]); + _revolutions1 = -int32_t((recvBuf[REG_ENC1A_R] << 24) + + (recvBuf[REG_ENC1B_R] << 16) + + (recvBuf[REG_ENC1C_R] << 8) + + recvBuf[REG_ENC1D_R]) / 360.0; + _revolutions2 = -int32_t((recvBuf[REG_ENC2A_R] << 24) + + (recvBuf[REG_ENC2B_R] << 16) + + (recvBuf[REG_ENC2C_R] << 8) + + recvBuf[REG_ENC2D_R]) / 360.0; + _batteryVoltage = recvBuf[REG_BATTERY_VOLTS_R] / 10.0; + _motor1Current = recvBuf[REG_MOTOR1_CURRENT_R] / 10.0; + _motor2Current = recvBuf[REG_MOTOR2_CURRENT_R] / 10.0; + _motorAccel = recvBuf[REG_ACCEL_RATE_RW]; + _mode = e_mode(recvBuf[REG_MODE_RW]); + _command = e_cmd(recvBuf[REG_COMMAND_RW]); + } + + return ret; +} + +void MD25::status(char *string, size_t n) +{ + snprintf(string, n, + "version:\t%10d\n" \ + "motor 1 speed:\t%10.2f\n" \ + "motor 2 speed:\t%10.2f\n" \ + "revolutions 1:\t%10.2f\n" \ + "revolutions 2:\t%10.2f\n" \ + "battery volts :\t%10.2f\n" \ + "motor 1 current :\t%10.2f\n" \ + "motor 2 current :\t%10.2f\n" \ + "motor accel :\t%10d\n" \ + "mode :\t%10d\n" \ + "command :\t%10d\n", + getVersion(), + double(getMotor1Speed()), + double(getMotor2Speed()), + double(getRevolutions1()), + double(getRevolutions2()), + double(getBatteryVolts()), + double(getMotor1Current()), + double(getMotor2Current()), + getMotorAccel(), + getMode(), + getCommand()); +} + +uint8_t MD25::getVersion() +{ + return _version; +} + +float MD25::getMotor1Speed() +{ + return _motor1Speed; +} + +float MD25::getMotor2Speed() +{ + return _motor2Speed; +} + +float MD25::getRevolutions1() +{ + return _revolutions1; +} + +float MD25::getRevolutions2() +{ + return _revolutions2; +} + +float MD25::getBatteryVolts() +{ + return _batteryVoltage; +} + +float MD25::getMotor1Current() +{ + return _motor1Current; +} +float MD25::getMotor2Current() +{ + return _motor2Current; +} + +uint8_t MD25::getMotorAccel() +{ + return _motorAccel; +} + +MD25::e_mode MD25::getMode() +{ + return _mode; +} + +MD25::e_cmd MD25::getCommand() +{ + return _command; +} + +int MD25::resetEncoders() +{ + return _writeUint8(REG_COMMAND_RW, + CMD_RESET_ENCODERS); +} + +int MD25::_setMode(e_mode mode) +{ + return _writeUint8(REG_MODE_RW, + mode); +} + +int MD25::setSpeedRegulation(bool enable) +{ + if (enable) { + return _writeUint8(REG_COMMAND_RW, + CMD_ENABLE_SPEED_REGULATION); + + } else { + return _writeUint8(REG_COMMAND_RW, + CMD_DISABLE_SPEED_REGULATION); + } +} + +int MD25::setTimeout(bool enable) +{ + if (enable) { + return _writeUint8(REG_COMMAND_RW, + CMD_ENABLE_TIMEOUT); + + } else { + return _writeUint8(REG_COMMAND_RW, + CMD_DISABLE_TIMEOUT); + } +} + +int MD25::setDeviceAddress(uint8_t address) +{ + uint8_t sendBuf[1]; + sendBuf[0] = CMD_CHANGE_I2C_SEQ_0; + int ret = OK; + ret = transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); + + if (ret != OK) { + warnc(ret, "MD25::setDeviceAddress"); + return ret; + } + + usleep(5000); + sendBuf[0] = CMD_CHANGE_I2C_SEQ_1; + ret = transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); + + if (ret != OK) { + warnc(ret, "MD25::setDeviceAddress"); + return ret; + } + + usleep(5000); + sendBuf[0] = CMD_CHANGE_I2C_SEQ_2; + ret = transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); + + if (ret != OK) { + warnc(ret, "MD25::setDeviceAddress"); + return ret; + } + + return OK; +} + +int MD25::setMotor1Speed(float value) +{ + return _writeUint8(REG_SPEED1_RW, + _normToUint8(value)); +} + +int MD25::setMotor2Speed(float value) +{ + return _writeUint8(REG_SPEED2_RW, + _normToUint8(value)); +} + +void MD25::update() +{ + // wait for an actuator publication, + // check for exit condition every second + // note "::poll" is required to distinguish global + // poll from member function for driver + if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error + + // if new data, send to motors + if (_actuators.updated()) { + _actuators.update(); + setMotor1Speed(_actuators.control[CH_SPEED_LEFT]); + setMotor2Speed(_actuators.control[CH_SPEED_RIGHT]); + } +} + +int MD25::probe() +{ + uint8_t goodAddress = 0; + bool found = false; + int ret = OK; + + // try initial address first, if good, then done + if (readData() == OK) return ret; + + // try all other addresses + uint8_t testAddress = 0; + + //printf("searching for MD25 address\n"); + while (true) { + set_address(testAddress); + ret = readData(); + + if (ret == OK && !found) { + //printf("device found at address: 0x%X\n", testAddress); + if (!found) { + found = true; + goodAddress = testAddress; + } + } + + if (testAddress > 254) { + break; + } + + testAddress++; + } + + if (found) { + set_address(goodAddress); + return OK; + + } else { + set_address(0); + return ret; + } +} + +int MD25::search() +{ + uint8_t goodAddress = 0; + bool found = false; + int ret = OK; + // try all other addresses + uint8_t testAddress = 0; + + //printf("searching for MD25 address\n"); + while (true) { + set_address(testAddress); + ret = readData(); + + if (ret == OK && !found) { + printf("device found at address: 0x%X\n", testAddress); + + if (!found) { + found = true; + goodAddress = testAddress; + } + } + + if (testAddress > 254) { + break; + } + + testAddress++; + } + + if (found) { + set_address(goodAddress); + return OK; + + } else { + set_address(0); + return ret; + } +} + +int MD25::_writeUint8(uint8_t reg, uint8_t value) +{ + uint8_t sendBuf[2]; + sendBuf[0] = reg; + sendBuf[1] = value; + return transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); +} + +int MD25::_writeInt8(uint8_t reg, int8_t value) +{ + uint8_t sendBuf[2]; + sendBuf[0] = reg; + sendBuf[1] = value; + return transfer(sendBuf, sizeof(sendBuf), + nullptr, 0); +} + +float MD25::_uint8ToNorm(uint8_t value) +{ + // TODO, should go from 0 to 255 + // possibly should handle this differently + return (value - 128) / 127.0; +} + +uint8_t MD25::_normToUint8(float value) +{ + if (value > 1) value = 1; + + if (value < -1) value = -1; + + // TODO, should go from 0 to 255 + // possibly should handle this differently + return 127 * value + 128; +} + +int md25Test(const char *deviceName, uint8_t bus, uint8_t address) +{ + printf("md25 test: starting\n"); + + // setup + MD25 md25("/dev/md25", bus, address); + + // print status + char buf[200]; + md25.status(buf, sizeof(buf)); + printf("%s\n", buf); + + // setup for test + md25.setSpeedRegulation(true); + md25.setTimeout(true); + float dt = 0.1; + float speed = 0.2; + float t = 0; + + // motor 1 test + printf("md25 test: spinning motor 1 forward for 1 rev at 0.1 speed\n"); + t = 0; + + while (true) { + t += dt; + md25.setMotor1Speed(speed); + md25.readData(); + usleep(1000000 * dt); + + if (md25.getRevolutions1() > 1) { + printf("finished 1 revolution fwd\n"); + break; + } + + if (t > 2.0f) break; + } + + md25.setMotor1Speed(0); + printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1())); + md25.resetEncoders(); + + t = 0; + + while (true) { + t += dt; + md25.setMotor1Speed(-speed); + md25.readData(); + usleep(1000000 * dt); + + if (md25.getRevolutions1() < -1) { + printf("finished 1 revolution rev\n"); + break; + } + + if (t > 2.0f) break; + } + + md25.setMotor1Speed(0); + printf("revolution of wheel 1: %8.4f\n", double(md25.getRevolutions1())); + md25.resetEncoders(); + + // motor 2 test + printf("md25 test: spinning motor 2 forward for 1 rev at 0.1 speed\n"); + t = 0; + + while (true) { + t += dt; + md25.setMotor2Speed(speed); + md25.readData(); + usleep(1000000 * dt); + + if (md25.getRevolutions2() > 1) { + printf("finished 1 revolution fwd\n"); + break; + } + + if (t > 2.0f) break; + } + + md25.setMotor2Speed(0); + printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2())); + md25.resetEncoders(); + + t = 0; + + while (true) { + t += dt; + md25.setMotor2Speed(-speed); + md25.readData(); + usleep(1000000 * dt); + + if (md25.getRevolutions2() < -1) { + printf("finished 1 revolution rev\n"); + break; + } + + if (t > 2.0f) break; + } + + md25.setMotor2Speed(0); + printf("revolution of wheel 2: %8.4f\n", double(md25.getRevolutions2())); + md25.resetEncoders(); + + printf("Test complete\n"); + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/apps/drivers/md25/MD25.hpp b/apps/drivers/md25/MD25.hpp new file mode 100644 index 000000000..e77511b16 --- /dev/null +++ b/apps/drivers/md25/MD25.hpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file md25.cpp + * + * Driver for MD25 I2C Motor Driver + * + * references: + * http://www.robot-electronics.co.uk/htm/md25tech.htm + * http://www.robot-electronics.co.uk/files/rpi_md25.c + * + */ + +#pragma once + +#include +#include +#include +#include +#include + +/** + * This is a driver for the MD25 motor controller utilizing the I2C interface. + */ +class MD25 : public device::I2C +{ +public: + + /** + * modes + * + * NOTE: this driver assumes we are always + * in mode 0! + * + * seprate speed mode: + * motor speed1 = speed1 + * motor speed2 = speed2 + * turn speed mode: + * motor speed1 = speed1 + speed2 + * motor speed2 = speed2 - speed2 + * unsigned: + * full rev (0), stop(128), full fwd (255) + * signed: + * full rev (-127), stop(0), full fwd (128) + * + * modes numbers: + * 0 : unsigned separate (default) + * 1 : signed separate + * 2 : unsigned turn + * 3 : signed turn + */ + enum e_mode { + MODE_UNSIGNED_SPEED = 0, + MODE_SIGNED_SPEED, + MODE_UNSIGNED_SPEED_TURN, + MODE_SIGNED_SPEED_TURN, + }; + + /** commands */ + enum e_cmd { + CMD_RESET_ENCODERS = 32, + CMD_DISABLE_SPEED_REGULATION = 48, + CMD_ENABLE_SPEED_REGULATION = 49, + CMD_DISABLE_TIMEOUT = 50, + CMD_ENABLE_TIMEOUT = 51, + CMD_CHANGE_I2C_SEQ_0 = 160, + CMD_CHANGE_I2C_SEQ_1 = 170, + CMD_CHANGE_I2C_SEQ_2 = 165, + }; + + /** control channels */ + enum e_channels { + CH_SPEED_LEFT = 0, + CH_SPEED_RIGHT + }; + + /** + * constructor + * @param deviceName the name of the device e.g. "/dev/md25" + * @param bus the I2C bus + * @param address the adddress on the I2C bus + * @param speed the speed of the I2C communication + */ + MD25(const char *deviceName, + int bus, + uint16_t address, + uint32_t speed = 100000); + + /** + * deconstructor + */ + virtual ~MD25(); + + /** + * @return software version + */ + uint8_t getVersion(); + + /** + * @return speed of motor, normalized (-1, 1) + */ + float getMotor1Speed(); + + /** + * @return speed of motor 2, normalized (-1, 1) + */ + float getMotor2Speed(); + + /** + * @return number of rotations since reset + */ + float getRevolutions1(); + + /** + * @return number of rotations since reset + */ + float getRevolutions2(); + + /** + * @return battery voltage, volts + */ + float getBatteryVolts(); + + /** + * @return motor 1 current, amps + */ + float getMotor1Current(); + + /** + * @return motor 2 current, amps + */ + float getMotor2Current(); + + /** + * @return the motor acceleration + * controls motor speed change (1-10) + * accel rate | time for full fwd. to full rev. + * 1 | 6.375 s + * 2 | 1.6 s + * 3 | 0.675 s + * 5(default) | 1.275 s + * 10 | 0.65 s + */ + uint8_t getMotorAccel(); + + /** + * @return motor output mode + * */ + e_mode getMode(); + + /** + * @return current command register value + */ + e_cmd getCommand(); + + /** + * resets the encoders + * @return non-zero -> error + * */ + int resetEncoders(); + + /** + * enable/disable speed regulation + * @return non-zero -> error + */ + int setSpeedRegulation(bool enable); + + /** + * set the timeout for the motors + * enable/disable timeout (motor stop) + * after 2 sec of no i2c messages + * @return non-zero -> error + */ + int setTimeout(bool enable); + + /** + * sets the device address + * can only be done with one MD25 + * on the bus + * @return non-zero -> error + */ + int setDeviceAddress(uint8_t address); + + /** + * set motor 1 speed + * @param normSpeed normalize speed between -1 and 1 + * @return non-zero -> error + */ + int setMotor1Speed(float normSpeed); + + /** + * set motor 2 speed + * @param normSpeed normalize speed between -1 and 1 + * @return non-zero -> error + */ + int setMotor2Speed(float normSpeed); + + /** + * main update loop that updates MD25 motor + * speeds based on actuator publication + */ + void update(); + + /** + * probe for device + */ + virtual int probe(); + + /** + * search for device + */ + int search(); + + /** + * read data from i2c + */ + int readData(); + + /** + * print status + */ + void status(char *string, size_t n); + +private: + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription _actuators; + + // local copy of data from i2c device + uint8_t _version; + float _motor1Speed; + float _motor2Speed; + float _revolutions1; + float _revolutions2; + float _batteryVoltage; + float _motor1Current; + float _motor2Current; + uint8_t _motorAccel; + e_mode _mode; + e_cmd _command; + + // private methods + int _writeUint8(uint8_t reg, uint8_t value); + int _writeInt8(uint8_t reg, int8_t value); + float _uint8ToNorm(uint8_t value); + uint8_t _normToUint8(float value); + + /** + * set motor control mode, + * this driver assumed we are always in mode 0 + * so we don't let the user change the mode + * @return non-zero -> error + */ + int _setMode(e_mode); +}; + +// unit testing +int md25Test(const char *deviceName, uint8_t bus, uint8_t address); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/apps/drivers/md25/Makefile b/apps/drivers/md25/Makefile new file mode 100644 index 000000000..cce2fd095 --- /dev/null +++ b/apps/drivers/md25/Makefile @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# MD25 I2C Based Motor Controller +# http://www.robot-electronics.co.uk/htm/md25tech.htm +# + +APPNAME = md25 +PRIORITY = SCHED_PRIORITY_DEFAULT +STACKSIZE = 2048 + +include $(APPDIR)/mk/app.mk diff --git a/apps/drivers/md25/md25_main.cpp b/apps/drivers/md25/md25_main.cpp new file mode 100644 index 000000000..85aaab7f6 --- /dev/null +++ b/apps/drivers/md25/md25_main.cpp @@ -0,0 +1,264 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + + + +/** + * @ file md25.cpp + * + * Driver for MD25 I2C Motor Driver + * + * references: + * http://www.robot-electronics.co.uk/htm/md25tech.htm + * http://www.robot-electronics.co.uk/files/rpi_md25.c + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "MD25.hpp" + +static bool thread_should_exit = false; /**< Deamon exit flag */ +static bool thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ + +/** + * Deamon management function. + */ +extern "C" __EXPORT int md25_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int md25_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(const char *reason); + +static void +usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: md25 {start|stop|status|search|test|change_address}\n\n"); + exit(1); +} + +/** + * The deamon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ +int md25_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("md25 already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn("md25", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + md25_thread_main, + (const char **)argv); + exit(0); + } + + if (!strcmp(argv[1], "test")) { + + if (argc < 4) { + printf("usage: md25 test bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + md25Test(deviceName, bus, address); + + exit(0); + } + + if (!strcmp(argv[1], "probe")) { + if (argc < 4) { + printf("usage: md25 probe bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + MD25 md25(deviceName, bus, address); + + int ret = md25.probe(); + + if (ret == OK) { + printf("MD25 found on bus %d at address 0x%X\n", bus, md25.get_address()); + + } else { + printf("MD25 not found on bus %d\n", bus); + } + + exit(0); + } + + if (!strcmp(argv[1], "search")) { + if (argc < 3) { + printf("usage: md25 search bus\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t address = strtoul(argv[3], nullptr, 0); + + MD25 md25(deviceName, bus, address); + + md25.search(); + + exit(0); + } + + if (!strcmp(argv[1], "change_address")) { + if (argc < 5) { + printf("usage: md25 change_address bus old_address new_address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[2], nullptr, 0); + + uint8_t old_address = strtoul(argv[3], nullptr, 0); + + uint8_t new_address = strtoul(argv[4], nullptr, 0); + + MD25 md25(deviceName, bus, old_address); + + int ret = md25.setDeviceAddress(new_address); + + if (ret == OK) { + printf("MD25 new address set to 0x%X\n", new_address); + + } else { + printf("MD25 failed to set address to 0x%X\n", new_address); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + printf("\tmd25 app is running\n"); + + } else { + printf("\tmd25 app not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +int md25_thread_main(int argc, char *argv[]) +{ + printf("[MD25] starting\n"); + + if (argc < 5) { + // extra md25 in arg list since this is a thread + printf("usage: md25 start bus address\n"); + exit(0); + } + + const char *deviceName = "/dev/md25"; + + uint8_t bus = strtoul(argv[3], nullptr, 0); + + uint8_t address = strtoul(argv[4], nullptr, 0); + + // start + MD25 md25("/dev/md25", bus, address); + + thread_running = true; + + // loop + while (!thread_should_exit) { + md25.update(); + } + + // exit + printf("[MD25] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 -- cgit v1.2.3