From 7f0ced968e5d518776930296ed870a47cc8c1756 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 21 Oct 2013 21:28:26 -0400 Subject: [PATCH 1/7] Working on roboclaw driver. --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/roboclaw/RoboClaw.cpp | 250 +++++++++++++++++++++++++ src/drivers/roboclaw/RoboClaw.hpp | 187 ++++++++++++++++++ src/drivers/roboclaw/module.mk | 41 ++++ src/drivers/roboclaw/roboclaw_main.cpp | 181 ++++++++++++++++++ 6 files changed, 661 insertions(+), 1 deletion(-) create mode 100644 src/drivers/roboclaw/RoboClaw.cpp create mode 100644 src/drivers/roboclaw/RoboClaw.hpp create mode 100644 src/drivers/roboclaw/module.mk create mode 100644 src/drivers/roboclaw/roboclaw_main.cpp diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 46640f3c51..3cccc8447a 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -33,7 +33,7 @@ MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl -MODULES += drivers/md25 +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bb589cb9f3..a2027ded9b 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -31,6 +31,7 @@ MODULES += drivers/hil MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm +MODULES += drivers/roboclaw MODULES += drivers/airspeed MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp new file mode 100644 index 0000000000..88b22e72aa --- /dev/null +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * 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 RoboClaw.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include "RoboClaw.hpp" +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include +#include + +uint8_t RoboClaw::checksum_mask = 0x7f; + +RoboClaw::RoboClaw(const char *deviceName, uint16_t address): + _address(address), + _uart(0), + _controlPoll(), + _actuators(NULL, ORB_ID(actuator_controls_0), 20), + _motor1Position(0), + _motor1Speed(0), + _motor2Position(0), + _motor2Speed(0) +{ + // setup control polling + _controlPoll.fd = _actuators.getHandle(); + _controlPoll.events = POLLIN; + + // start serial port + _uart = open(deviceName, O_RDWR | O_NOCTTY); + struct termios uart_config; + tcgetattr(_uart, &uart_config); + uart_config.c_oflag &= ~ONLCR; // no CR for every LF + cfsetispeed(&uart_config, B38400); + cfsetospeed(&uart_config, B38400); + tcsetattr(_uart, TCSANOW, &uart_config); + + // setup default settings, reset encoders + resetEncoders(); +} + +RoboClaw::~RoboClaw() +{ + setMotorDutyCycle(MOTOR_1, 0.0); + setMotorDutyCycle(MOTOR_2, 0.0); + close(_uart); +} + +int RoboClaw::readEncoder(e_motor motor) +{ + uint16_t sum = 0; + if (motor == MOTOR_1) { + _sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); + } + uint8_t rbuf[6]; + int ret = read(_uart, rbuf, 6); + uint32_t count = 0; + uint8_t * countBytes = (uint8_t *)(&count); + countBytes[3] = rbuf[0]; + countBytes[2] = rbuf[1]; + countBytes[1] = rbuf[2]; + countBytes[0] = rbuf[3]; + uint8_t status = rbuf[4]; + if ((sum + _sumBytes(rbuf,5)) & + checksum_mask == rbuf[5]) return -1; + int overFlow = 0; + if (status & STATUS_UNDERFLOW) { + overFlow = -1; + } else if (status & STATUS_OVERFLOW) { + overFlow = +1; + } + if (motor == MOTOR_1) { + _motor1Overflow += overFlow; + } else if (motor == MOTOR_2) { + _motor2Overflow += overFlow; + } + return ret; +} + +void RoboClaw::status(char *string, size_t n) +{ + snprintf(string, n, + "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n" + "motor 2 position:\t%10.2f\tspeed:\t%10.2f\n", + double(getMotorPosition(MOTOR_1)), + double(getMotorSpeed(MOTOR_1)), + double(getMotorPosition(MOTOR_2)), + double(getMotorSpeed(MOTOR_2))); +} + +float RoboClaw::getMotorPosition(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Position; + } else if (motor == MOTOR_2) { + return _motor2Position; + } +} + +float RoboClaw::getMotorSpeed(e_motor motor) +{ + if (motor == MOTOR_1) { + return _motor1Speed; + } else if (motor == MOTOR_2) { + return _motor2Speed; + } +} + +int RoboClaw::setMotorSpeed(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + uint8_t speed = fabs(value)*127; + // send command + if (motor == MOTOR_1) { + if (value > 0) { + _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + } else { + _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + } + } else if (motor == MOTOR_2) { + if (value > 0) { + _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + } else { + _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + } + } +} + +int RoboClaw::setMotorDutyCycle(e_motor motor, float value) +{ + uint16_t sum = 0; + // bound + if (value > 1) value = 1; + if (value < -1) value = -1; + int16_t duty = 1500*value; + // send command + if (motor == MOTOR_1) { + _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + (uint8_t *)(&duty), 2, sum); + } else if (motor == MOTOR_2) { + _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + (uint8_t *)(&duty), 2, sum); + } +} + +void RoboClaw::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(); + setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + } +} + +uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) +{ + uint16_t sum = 0; + for (size_t i=0;i +#include +#include +#include +#include + +/** + * This is a driver for the RoboClaw motor controller + */ +class RoboClaw +{ +public: + + /** control channels */ + enum e_channel { + CH_VOLTAGE_LEFT = 0, + CH_VOLTAGE_RIGHT + }; + + /** motors */ + enum e_motor { + MOTOR_1 = 0, + MOTOR_2 + }; + + /** + * constructor + * @param deviceName the name of the + * serial port e.g. "/dev/ttyS2" + * @param address the adddress of the motor + * (selectable on roboclaw) + */ + RoboClaw(const char *deviceName, uint16_t address); + + /** + * deconstructor + */ + virtual ~RoboClaw(); + + /** + * @return position of a motor, rev + */ + float getMotorPosition(e_motor motor); + + /** + * @return speed of a motor, rev/sec + */ + float getMotorSpeed(e_motor motor); + + /** + * set the speed of a motor, rev/sec + */ + int setMotorSpeed(e_motor motor, float value); + + /** + * set the duty cycle of a motor, rev/sec + */ + int setMotorDutyCycle(e_motor motor, float value); + + /** + * reset the encoders + * @return status + */ + int resetEncoders(); + + /** + * main update loop that updates RoboClaw motor + * dutycycle based on actuator publication + */ + void update(); + + /** + * read data from serial + */ + int readEncoder(e_motor motor); + + /** + * print status + */ + void status(char *string, size_t n); + +private: + + // Quadrature status flags + enum e_quadrature_status_flags { + STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/ + STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/ + STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/ + }; + + // commands + // We just list the commands we want from the manual here. + enum e_command { + + // simple + CMD_DRIVE_FWD_1 = 0, + CMD_DRIVE_REV_1 = 1, + CMD_DRIVE_FWD_2 = 4, + CMD_DRIVE_REV_2 = 5, + + // encoder commands + CMD_READ_ENCODER_1 = 16, + CMD_READ_ENCODER_2 = 17, + CMD_RESET_ENCODERS = 20, + + // advanced motor control + CMD_READ_SPEED_HIRES_1 = 30, + CMD_READ_SPEED_HIRES_2 = 31, + CMD_SIGNED_DUTYCYCLE_1 = 32, + CMD_SIGNED_DUTYCYCLE_2 = 33, + }; + + static uint8_t checksum_mask; + + uint16_t _address; + + int _uart; + + /** poll structure for control packets */ + struct pollfd _controlPoll; + + /** actuator controls subscription */ + control::UOrbSubscription _actuators; + + // private data + float _motor1Position; + float _motor1Speed; + int16_t _motor1Overflow; + + float _motor2Position; + float _motor2Speed; + int16_t _motor2Overflow; + + // private methods + uint16_t _sumBytes(uint8_t * buf, size_t n); + int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum); +}; + +// unit testing +int roboclawTest(const char *deviceName, uint8_t address); + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk new file mode 100644 index 0000000000..1abecf198b --- /dev/null +++ b/src/drivers/roboclaw/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# 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. +# +############################################################################ + +# +# RoboClaw Motor Controller +# + +MODULE_COMMAND = roboclaw + +SRCS = roboclaw_main.cpp \ + RoboClaw.cpp diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp new file mode 100644 index 0000000000..a61c646fcf --- /dev/null +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -0,0 +1,181 @@ +/**************************************************************************** + * + * 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 roboclaw_main.cpp + * + * RoboClaw Motor Driver + * + * references: + * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf + * + */ + +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include "RoboClaw.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 roboclaw_main(int argc, char *argv[]); + +/** + * Mainloop of deamon. + */ +int roboclaw_thread_main(int argc, char *argv[]); + +/** + * Print the correct usage. + */ +static void usage(); + +static void usage() +{ + fprintf(stderr, "usage: roboclaw " + "{start|stop|status|test}\n\n"); +} + +/** + * 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 roboclaw_main(int argc, char *argv[]) +{ + + if (argc < 1) + usage(); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + printf("roboclaw already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn_cmd("roboclaw", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 10, + 2048, + roboclaw_thread_main, + (const char **)argv); + exit(0); + + } else if (!strcmp(argv[1], "test")) { + + if (argc < 4) { + printf("usage: roboclaw test device address\n"); + exit(-1); + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + + roboclawTest(deviceName, address); + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "stop")) { + + thread_should_exit = true; + exit(0); + + } else if (!strcmp(argv[1], "status")) { + + if (thread_running) { + printf("\troboclaw app is running\n"); + + } else { + printf("\troboclaw app not started\n"); + } + exit(0); + } + + usage(); + exit(1); +} + +int roboclaw_thread_main(int argc, char *argv[]) +{ + printf("[roboclaw] starting\n"); + + // skip parent process args + argc -=2; + argv +=2; + + if (argc < 3) { + printf("usage: roboclaw start device address\n"); + return -1; + } + + const char *deviceName = argv[1]; + uint8_t address = strtoul(argv[2], nullptr, 0); + + // start + RoboClaw roboclaw(deviceName, address); + + thread_running = true; + + // loop + while (!thread_should_exit) { + roboclaw.update(); + } + + // exit + printf("[roboclaw] exiting.\n"); + thread_running = false; + return 0; +} + +// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78 From ce68f93867abfd3ea528809144f7c045d9bce544 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 21 Oct 2013 23:40:36 -0400 Subject: [PATCH 2/7] Debugging roboclaw comm. --- src/drivers/roboclaw/RoboClaw.cpp | 52 ++++++++++++++++++++++---- src/drivers/roboclaw/RoboClaw.hpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 8 +++- 3 files changed, 52 insertions(+), 10 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 88b22e72aa..aecc511aed 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -75,12 +75,18 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address): // start serial port _uart = open(deviceName, O_RDWR | O_NOCTTY); + if (_uart < 0) err(1, "could not open %s", deviceName); + int ret = 0; struct termios uart_config; - tcgetattr(_uart, &uart_config); + ret = tcgetattr(_uart, &uart_config); + if (ret < 0) err (1, "failed to get attr"); uart_config.c_oflag &= ~ONLCR; // no CR for every LF - cfsetispeed(&uart_config, B38400); - cfsetospeed(&uart_config, B38400); - tcsetattr(_uart, TCSANOW, &uart_config); + ret = cfsetispeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set input speed"); + ret = cfsetospeed(&uart_config, B38400); + if (ret < 0) err (1, "failed to set output speed"); + ret = tcsetattr(_uart, TCSANOW, &uart_config); + if (ret < 0) err (1, "failed to set attr"); // setup default settings, reset encoders resetEncoders(); @@ -110,23 +116,30 @@ int RoboClaw::readEncoder(e_motor motor) countBytes[1] = rbuf[2]; countBytes[0] = rbuf[3]; uint8_t status = rbuf[4]; - if ((sum + _sumBytes(rbuf,5)) & - checksum_mask == rbuf[5]) return -1; + if (((sum + _sumBytes(rbuf,5)) & checksum_mask) + == rbuf[5]) return -1; int overFlow = 0; if (status & STATUS_UNDERFLOW) { + printf("roboclaw: underflow\n"); overFlow = -1; } else if (status & STATUS_OVERFLOW) { + printf("roboclaw: overflow\n"); overFlow = +1; } + static int64_t overflowAmount = 0x100000000LL; if (motor == MOTOR_1) { _motor1Overflow += overFlow; + _motor1Position = count + + _motor1Overflow*_motor1Position; } else if (motor == MOTOR_2) { _motor2Overflow += overFlow; + _motor2Position = count + + _motor2Overflow*_motor2Position; } return ret; } -void RoboClaw::status(char *string, size_t n) +void RoboClaw::printStatus(char *string, size_t n) { snprintf(string, n, "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n" @@ -195,6 +208,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) } } +int RoboClaw::resetEncoders() +{ + uint16_t sum = 0; + return _sendCommand(CMD_RESET_ENCODERS, + nullptr, 0, sum); +} + void RoboClaw::update() { // wait for an actuator publication, @@ -214,9 +234,13 @@ void RoboClaw::update() uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) { uint16_t sum = 0; + printf("sum\n"); for (size_t i=0;i Date: Tue, 22 Oct 2013 05:04:13 -0400 Subject: [PATCH 3/7] Roboclaw encoders/ dutycycledrive complete. --- ROMFS/px4fmu_common/init.d/40_io_segway | 16 +-- ROMFS/px4fmu_common/init.d/rcS | 6 ++ src/drivers/roboclaw/RoboClaw.cpp | 131 ++++++++++++++++-------- src/drivers/roboclaw/RoboClaw.hpp | 11 +- src/drivers/roboclaw/roboclaw_main.cpp | 28 +++-- 5 files changed, 130 insertions(+), 62 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index 4b7ed52869..ffe7f695b1 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -21,8 +21,8 @@ param set MAV_TYPE 10 # # Start MAVLink (depends on orb) # -mavlink start -d /dev/ttyS1 -b 57600 -usleep 5000 +#mavlink start -d /dev/ttyS1 -b 57600 +#usleep 5000 # # Start and configure PX4IO interface @@ -32,25 +32,25 @@ sh /etc/init.d/rc.io # # Start the commander (depends on orb, mavlink) # -commander start +#commander start # # Start the sensors (depends on orb, px4io) # -sh /etc/init.d/rc.sensors +#sh /etc/init.d/rc.sensors # # Start GPS interface (depends on orb) # -gps start +#gps start # # Start the attitude estimator (depends on orb) # -attitude_estimator_ekf start +#attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -md25 start 3 0x58 -segway start +roboclaw test /dev/ttyS2 128 +#segway start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index fd588017f9..3a458286ed 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -272,6 +272,12 @@ then sh /etc/init.d/30_io_camflyer set MODE custom fi + + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi if param compare SYS_AUTOSTART 31 then diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index aecc511aed..3bbd7f48f5 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -59,15 +59,19 @@ uint8_t RoboClaw::checksum_mask = 0x7f; -RoboClaw::RoboClaw(const char *deviceName, uint16_t address): +RoboClaw::RoboClaw(const char *deviceName, uint16_t address, + uint16_t pulsesPerRev): _address(address), + _pulsesPerRev(pulsesPerRev), _uart(0), _controlPoll(), _actuators(NULL, ORB_ID(actuator_controls_0), 20), _motor1Position(0), _motor1Speed(0), + _motor1Overflow(0), _motor2Position(0), - _motor2Speed(0) + _motor2Speed(0), + _motor2Overflow(0) { // setup control polling _controlPoll.fd = _actuators.getHandle(); @@ -90,6 +94,13 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address): // setup default settings, reset encoders resetEncoders(); + + // XXX roboclaw gives 128 as first several csums + // have to read a couple of messages first + for (int i=0;i<5;i++) { + if (readEncoder(MOTOR_1) > 0) break; + usleep(3000); + } } RoboClaw::~RoboClaw() @@ -107,8 +118,18 @@ int RoboClaw::readEncoder(e_motor motor) } else if (motor == MOTOR_2) { _sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum); } - uint8_t rbuf[6]; - int ret = read(_uart, rbuf, 6); + uint8_t rbuf[50]; + usleep(5000); + int nread = read(_uart, rbuf, 50); + if (nread < 6) { + printf("failed to read\n"); + return -1; + } + //printf("received: \n"); + //for (int i=0;i 0) { - _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum); } else { - _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum); } } else if (motor == MOTOR_2) { if (value > 0) { - _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum); } else { - _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); + return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum); } } + return -1; } int RoboClaw::setMotorDutyCycle(e_motor motor, float value) @@ -200,12 +234,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value) int16_t duty = 1500*value; // send command if (motor == MOTOR_1) { - _sendCommand(CMD_SIGNED_DUTYCYCLE_1, + return _sendCommand(CMD_SIGNED_DUTYCYCLE_1, (uint8_t *)(&duty), 2, sum); } else if (motor == MOTOR_2) { - _sendCommand(CMD_SIGNED_DUTYCYCLE_2, + return _sendCommand(CMD_SIGNED_DUTYCYCLE_2, (uint8_t *)(&duty), 2, sum); } + return -1; } int RoboClaw::resetEncoders() @@ -215,13 +250,13 @@ int RoboClaw::resetEncoders() nullptr, 0, sum); } -void RoboClaw::update() +int RoboClaw::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 (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error // if new data, send to motors if (_actuators.updated()) { @@ -229,56 +264,68 @@ void RoboClaw::update() setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); } + return 0; } uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n) { uint16_t sum = 0; - printf("sum\n"); + //printf("sum\n"); for (size_t i=0;i Date: Tue, 22 Oct 2013 05:08:20 -0400 Subject: [PATCH 4/7] Updated script. --- ROMFS/px4fmu_common/init.d/40_io_segway | 16 ++++++++-------- ROMFS/px4fmu_common/init.d/rcS | 12 ++++++------ 2 files changed, 14 insertions(+), 14 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway index ffe7f695b1..fb9dec0437 100644 --- a/ROMFS/px4fmu_common/init.d/40_io_segway +++ b/ROMFS/px4fmu_common/init.d/40_io_segway @@ -21,8 +21,8 @@ param set MAV_TYPE 10 # # Start MAVLink (depends on orb) # -#mavlink start -d /dev/ttyS1 -b 57600 -#usleep 5000 +mavlink start -d /dev/ttyS1 -b 57600 +usleep 5000 # # Start and configure PX4IO interface @@ -32,25 +32,25 @@ sh /etc/init.d/rc.io # # Start the commander (depends on orb, mavlink) # -#commander start +commander start # # Start the sensors (depends on orb, px4io) # -#sh /etc/init.d/rc.sensors +sh /etc/init.d/rc.sensors # # Start GPS interface (depends on orb) # -#gps start +gps start # # Start the attitude estimator (depends on orb) # -#attitude_estimator_ekf start +attitude_estimator_ekf start # # Load mixer and start controllers (depends on px4io) # -roboclaw test /dev/ttyS2 128 -#segway start +roboclaw start /dev/ttyS2 128 1200 +segway start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3a458286ed..424787d54a 100755 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -273,18 +273,18 @@ then set MODE custom fi - if param compare SYS_AUTOSTART 40 - then - sh /etc/init.d/40_io_segway - set MODE custom - fi - if param compare SYS_AUTOSTART 31 then sh /etc/init.d/31_io_phantom set MODE custom fi + if param compare SYS_AUTOSTART 40 + then + sh /etc/init.d/40_io_segway + set MODE custom + fi + if param compare SYS_AUTOSTART 100 then sh /etc/init.d/100_mpx_easystar From 108d723a4902086f883136c7de6a75d65256500b Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:10:26 -0400 Subject: [PATCH 5/7] Removed old timing hack. --- src/drivers/roboclaw/RoboClaw.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 3bbd7f48f5..73bd5dadac 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -94,13 +94,6 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address, // setup default settings, reset encoders resetEncoders(); - - // XXX roboclaw gives 128 as first several csums - // have to read a couple of messages first - for (int i=0;i<5;i++) { - if (readEncoder(MOTOR_1) > 0) break; - usleep(3000); - } } RoboClaw::~RoboClaw() From d143e827dcd774548bca6d6b4cb6fb69ef35a826 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:43:10 -0400 Subject: [PATCH 6/7] Updated segway controller for new state machine. --- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- src/modules/segway/BlockSegwayController.cpp | 13 +++++++------ 3 files changed, 9 insertions(+), 8 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 3cccc8447a..85ac3f546f 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -79,7 +79,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update +MODULES += modules/segway MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index a2027ded9b..c0972be9eb 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -78,7 +78,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -#MODULES += modules/segway # XXX needs state machine update +MODULES += modules/segway MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control MODULES += modules/multirotor_att_control diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index b1dc39445f..96a443c6ea 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -26,7 +26,7 @@ void BlockSegwayController::update() { _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.main_state == MAIN_STATE_AUTO) { // update guidance } @@ -34,17 +34,18 @@ void BlockSegwayController::update() { float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed); // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.main_state == MAIN_STATE_AUTO || + _status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else if (_status.main_state == MAIN_STATE_MANUAL) { + if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { _actuators.control[CH_LEFT] = _manual.throttle; _actuators.control[CH_RIGHT] = _manual.pitch; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { + } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; } From c4a1a338ff4378c908ec34c5a5f408c1b96d0735 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Tue, 22 Oct 2013 05:43:27 -0400 Subject: [PATCH 7/7] Changed driver to control motor duty cycle. --- src/drivers/roboclaw/RoboClaw.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp index 73bd5dadac..d65a9be361 100644 --- a/src/drivers/roboclaw/RoboClaw.cpp +++ b/src/drivers/roboclaw/RoboClaw.cpp @@ -254,8 +254,8 @@ int RoboClaw::update() // if new data, send to motors if (_actuators.updated()) { _actuators.update(); - setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); - setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); + setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]); + setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]); } return 0; }