motor_hardware.cc

Go to the documentation of this file.

#include <ubiquity_motor/motor_hardware.h>
#include <ubiquity_motor/motor_message.h>
#include <boost/assign.hpp>

#include <boost/math/special_functions/round.hpp>

//#define SENSOR_DISTANCE 0.002478

// 60 tics per revolution of the motor (pre gearbox)
// 17.2328767123
// gear ratio of 4.29411764706:1
#define TICS_PER_RADIAN (41.0058030317 / 2)
#define QTICS_PER_RADIAN (TICS_PER_RADIAN * 4)
#define VELOCITY_READ_PER_SECOND \
    10.0  // read = ticks / (100 ms), so we have scale of 10 for ticks/second
#define CURRENT_FIRMWARE_VERSION 24

MotorHardware::MotorHardware(ros::NodeHandle nh, CommsParams serial_params,
                             FirmwareParams firmware_params) {
    ros::V_string joint_names =
        boost::assign::list_of("left_wheel_joint")("right_wheel_joint");

    for (size_t i = 0; i < joint_names.size(); i++) {
        hardware_interface::JointStateHandle joint_state_handle(
            joint_names[i], &joints_[i].position, &joints_[i].velocity,
            &joints_[i].effort);
        joint_state_interface_.registerHandle(joint_state_handle);

        hardware_interface::JointHandle joint_handle(
            joint_state_handle, &joints_[i].velocity_command);
        velocity_joint_interface_.registerHandle(joint_handle);
    }
    registerInterface(&joint_state_interface_);
    registerInterface(&velocity_joint_interface_);

    motor_serial_ =
        new MotorSerial(serial_params.serial_port, serial_params.baud_rate,
                        serial_params.serial_loop_rate);

    leftError = nh.advertise<std_msgs::Int32>("left_error", 1);
    rightError = nh.advertise<std_msgs::Int32>("right_error", 1);

    pubU50 = nh.advertise<std_msgs::UInt32>("u50", 1);
    pubS50 = nh.advertise<std_msgs::Int32>("s50", 1);
    pubU51 = nh.advertise<std_msgs::UInt32>("u51", 1);
    pubS51 = nh.advertise<std_msgs::Int32>("s51", 1);
    pubU52 = nh.advertise<std_msgs::UInt32>("u52", 1);
    pubS52 = nh.advertise<std_msgs::Int32>("s52", 1);
    pubU53 = nh.advertise<std_msgs::UInt32>("u53", 1);
    pubS53 = nh.advertise<std_msgs::Int32>("s53", 1);
    pubU54 = nh.advertise<std_msgs::UInt32>("u54", 1);
    pubS54 = nh.advertise<std_msgs::Int32>("s54", 1);
    pubU55 = nh.advertise<std_msgs::UInt32>("u55", 1);
    pubS55 = nh.advertise<std_msgs::Int32>("s55", 1);
    pubU56 = nh.advertise<std_msgs::UInt32>("u56", 1);
    pubS56 = nh.advertise<std_msgs::Int32>("s56", 1);
    pubU57 = nh.advertise<std_msgs::UInt32>("u57", 1);
    pubS57 = nh.advertise<std_msgs::Int32>("s57", 1);
    pubU58 = nh.advertise<std_msgs::UInt32>("u58", 1);
    pubS58 = nh.advertise<std_msgs::Int32>("s58", 1);
    pubU59 = nh.advertise<std_msgs::UInt32>("u59", 1);
    pubS59 = nh.advertise<std_msgs::Int32>("s59", 1);

    sendPid_count = 0;

    pid_params = firmware_params;

    prev_pid_params.pid_proportional = -1;
    prev_pid_params.pid_integral = -1;
    prev_pid_params.pid_derivative = -1;
    prev_pid_params.pid_denominator = -1;
    prev_pid_params.pid_moving_buffer_size = -1;
    prev_pid_params.deadman_timer = -1;

    firmware_version = 0;
}

MotorHardware::~MotorHardware() { delete motor_serial_; }

void MotorHardware::readInputs() {
    while (motor_serial_->commandAvailable()) {
        MotorMessage mm;
        mm = motor_serial_->receiveCommand();
        if (mm.getType() == MotorMessage::TYPE_RESPONSE) {
            switch (mm.getRegister()) {
                case MotorMessage::REG_FIRMWARE_VERSION:
                    if (mm.getData() < CURRENT_FIRMWARE_VERSION) {
                        ROS_FATAL("Firmware version %d, expect %d or above",
                                  mm.getData(), CURRENT_FIRMWARE_VERSION);
                        throw std::runtime_error("Firmware version too low");
                    } else {
                        ROS_INFO("Firmware version %d", mm.getData());
                        firmware_version = mm.getData();
                    }
                    break;

                case MotorMessage::REG_BOTH_ODOM: {
                    int32_t odom = mm.getData();
                    // ROS_ERROR("odom signed %d", odom);
                    int16_t odomLeft = (odom >> 16) & 0xffff;
                    int16_t odomRight = odom & 0xffff;
                    // ROS_ERROR("left %d right %d", odomLeft, odomRight);

                    joints_[0].position += (odomLeft / TICS_PER_RADIAN);
                    joints_[1].position += (odomRight / TICS_PER_RADIAN);
                    break;
                }
                case MotorMessage::REG_BOTH_ERROR: {
                    std_msgs::Int32 left;
                    std_msgs::Int32 right;
                    int32_t speed = mm.getData();
                    int16_t leftSpeed = (speed >> 16) & 0xffff;
                    int16_t rightSpeed = speed & 0xffff;

                    left.data = leftSpeed;
                    right.data = rightSpeed;
                    leftError.publish(left);
                    rightError.publish(right);
                    break;
                }
                case MotorMessage::REG_LIMIT_REACHED: {
                    int32_t data = mm.getData();

                    if (data & MotorMessage::LIM_M1_PWM) {
                        ROS_ERROR("left PWM limit reached");
                    }
                    if (data & MotorMessage::LIM_M2_PWM) {
                        ROS_ERROR("right PWM limit reached");
                    }
                    if (data & MotorMessage::LIM_M1_INTEGRAL) {
                        ROS_WARN("left Integral limit reached");
                    }
                    if (data & MotorMessage::LIM_M2_INTEGRAL) {
                        ROS_WARN("right Integral limit reached");
                    }
                    break;
                }
                default:
                    uint8_t reg = mm.getRegister();
                    int32_t data = mm.getData();
                    std_msgs::UInt32 umsg;
                    std_msgs::Int32 smsg;
                    umsg.data = data;
                    smsg.data = data;
                    switch (reg) {
                        case 0x50:
                            pubU50.publish(umsg);
                            pubS50.publish(smsg);
                            break;
                        case 0x51:
                            pubU51.publish(umsg);
                            pubS51.publish(smsg);
                            break;
                        case 0x52:
                            pubU52.publish(umsg);
                            pubS52.publish(smsg);
                            break;
                        case 0x53:
                            pubU53.publish(umsg);
                            pubS53.publish(smsg);
                            break;
                        case 0x54:
                            pubU54.publish(umsg);
                            pubS54.publish(smsg);
                            break;
                        case 0x55:
                            pubU55.publish(umsg);
                            pubS55.publish(smsg);
                            break;
                        case 0x56:
                            pubU56.publish(umsg);
                            pubS56.publish(smsg);
                            break;
                        case 0x57:
                            pubU57.publish(umsg);
                            pubS57.publish(smsg);
                            break;
                        case 0x58:
                            pubU58.publish(umsg);
                            pubS58.publish(smsg);
                            break;
                    }
            }
        }
    }
}

void MotorHardware::writeSpeeds() {
    MotorMessage both;
    both.setRegister(MotorMessage::REG_BOTH_SPEED_SET);
    both.setType(MotorMessage::TYPE_WRITE);
    int16_t left_tics = calculateTicsFromRadians(joints_[0].velocity_command);
    int16_t right_tics = calculateTicsFromRadians(joints_[1].velocity_command);

    // The masking with 0x0000ffff is necessary for handling -ve numbers
    int32_t data = (left_tics << 16) | (right_tics & 0x0000ffff);
    both.setData(data);

    std_msgs::Int32 smsg;
    smsg.data = left_tics;
    pubS59.publish(smsg);

    motor_serial_->transmitCommand(both);

    // ROS_ERROR("velocity_command %f rad/s %f rad/s",
    // joints_[0].velocity_command, joints_[1].velocity_command);
    // ROS_ERROR("SPEEDS %d %d", left.getData(), right.getData());
}

void MotorHardware::requestVersion() {
    MotorMessage version;
    version.setRegister(MotorMessage::REG_FIRMWARE_VERSION);
    version.setType(MotorMessage::TYPE_READ);
    version.setData(0);
    motor_serial_->transmitCommand(version);
}

void MotorHardware::setDeadmanTimer(int32_t deadman_timer) {
    ROS_ERROR("setting deadman to %d", (int)deadman_timer);
    MotorMessage mm;
    mm.setRegister(MotorMessage::REG_DEADMAN);
    mm.setType(MotorMessage::TYPE_WRITE);
    mm.setData(deadman_timer);
    motor_serial_->transmitCommand(mm);
}

void MotorHardware::setParams(FirmwareParams fp) {
    pid_params.pid_proportional = fp.pid_proportional;
    pid_params.pid_integral = fp.pid_integral;
    pid_params.pid_derivative = fp.pid_derivative;
    pid_params.pid_denominator = fp.pid_denominator;
    pid_params.pid_moving_buffer_size = fp.pid_moving_buffer_size;
}

void MotorHardware::sendParams() {
    std::vector<MotorMessage> commands;

    // ROS_ERROR("sending PID %d %d %d %d",
    //(int)p_value, (int)i_value, (int)d_value, (int)denominator_value);

    // Only send one register at a time to avoid overwhelming serial comms
    int cycle = (sendPid_count++) % 5;

    if (cycle == 0 &&
        pid_params.pid_proportional != prev_pid_params.pid_proportional) {
        ROS_WARN("Setting P to %d", pid_params.pid_proportional);
        prev_pid_params.pid_proportional = pid_params.pid_proportional;
        MotorMessage p;
        p.setRegister(MotorMessage::REG_PARAM_P);
        p.setType(MotorMessage::TYPE_WRITE);
        p.setData(pid_params.pid_proportional);
        commands.push_back(p);
    }

    if (cycle == 1 && pid_params.pid_integral != prev_pid_params.pid_integral) {
        ROS_WARN("Setting I to %d", pid_params.pid_integral);
        prev_pid_params.pid_integral = pid_params.pid_integral;
        MotorMessage i;
        i.setRegister(MotorMessage::REG_PARAM_I);
        i.setType(MotorMessage::TYPE_WRITE);
        i.setData(pid_params.pid_integral);
        commands.push_back(i);
    }

    if (cycle == 2 &&
        pid_params.pid_derivative != prev_pid_params.pid_derivative) {
        ROS_WARN("Setting D to %d", pid_params.pid_derivative);
        prev_pid_params.pid_derivative = pid_params.pid_derivative;
        MotorMessage d;
        d.setRegister(MotorMessage::REG_PARAM_D);
        d.setType(MotorMessage::TYPE_WRITE);
        d.setData(pid_params.pid_derivative);
        commands.push_back(d);
    }

    if (cycle == 3 &&
        pid_params.pid_denominator != prev_pid_params.pid_denominator) {
        ROS_WARN("Setting Denominator to %d", pid_params.pid_denominator);
        prev_pid_params.pid_denominator = pid_params.pid_denominator;
        MotorMessage denominator;
        denominator.setRegister(MotorMessage::REG_PARAM_C);
        denominator.setType(MotorMessage::TYPE_WRITE);
        denominator.setData(pid_params.pid_denominator);
        commands.push_back(denominator);
    }

    if (cycle == 4 &&
        pid_params.pid_moving_buffer_size !=
            prev_pid_params.pid_moving_buffer_size) {
        ROS_WARN("Setting D window to %d", pid_params.pid_moving_buffer_size);
        prev_pid_params.pid_moving_buffer_size =
            pid_params.pid_moving_buffer_size;
        MotorMessage winsize;
        winsize.setRegister(MotorMessage::REG_MOVING_BUF_SIZE);
        winsize.setType(MotorMessage::TYPE_WRITE);
        winsize.setData(pid_params.pid_moving_buffer_size);
        commands.push_back(winsize);
    }

    if (commands.size() != 0) {
        motor_serial_->transmitCommands(commands);
    }
}

void MotorHardware::setDebugLeds(bool led_1, bool led_2) {
    std::vector<MotorMessage> commands;

    MotorMessage led1;
    led1.setRegister(MotorMessage::REG_LED_1);
    led1.setType(MotorMessage::TYPE_WRITE);
    if (led_1) {
        led1.setData(0x00000001);
    } else {
        led1.setData(0x00000000);
    }
    commands.push_back(led1);

    MotorMessage led2;
    led2.setRegister(MotorMessage::REG_LED_2);
    led2.setType(MotorMessage::TYPE_WRITE);
    if (led_2) {
        led2.setData(0x00000001);
    } else {
        led2.setData(0x00000000);
    }
    commands.push_back(led2);

    motor_serial_->transmitCommands(commands);
}

int16_t MotorHardware::calculateTicsFromRadians(double radians) const {
    return boost::math::iround(radians * QTICS_PER_RADIAN /
                               VELOCITY_READ_PER_SECOND);
}

double MotorHardware::calculateRadiansFromTics(int16_t tics) const {
    return (tics * VELOCITY_READ_PER_SECOND / QTICS_PER_RADIAN);
}