motor_driver.cpp

Go to the documentation of this file.

#include "motor_driver.h"
 
MotorControl::MotorControl(uint8_t _id, uint8_t _can_port_id, int _number_of_inner_motor_rotations, double _position_offset)
{
    id = _id;
 
    address = id + MOTOR_ADDRESS_OFFSET;
 
    can_message.id = address;
 
    can_port_id = _can_port_id;
 
    position_offset = _position_offset;
 
    max_encoder_value = MAX_ENCODER_VALUE;
 
    max_torque = MAX_TORQUE;
 
    max_torque_current = MAX_TORQUE_CURRENT;
 
    // We set the initial encoder value to be just to the left of the shaft pointing straight forward. 
    previous_encoder_value = MAX_ENCODER_VALUE;
 
    // This value is somewhat arbitrary. It should be scaled relative to the 
    // encoder resolution and large enough so that no inner motor turns are skipped
    encoder_turn_threshold = (max_encoder_value + 1)/2;
 
    initial_number_of_inner_motor_rotations = _number_of_inner_motor_rotations;
 
    number_of_inner_motor_rotations = _number_of_inner_motor_rotations;
 
    // Set the PID parameters in the motor
    /*
    while(!writePIDParametersToRAM(10, 1, 50, 5, 50, 5))
    {
        delay_microseconds(1000000.0);
    }
    */
 
    // Get the initial states from the motor
    while(!readMotorStatus())
    {        
        #if SERIAL_PRINT
            Serial.println("Trying to read motor states");
        #endif
        delay_microseconds(1000000.0);
    } 
 
    while(!readMultiTurnAngle())
    {
        #if SERIAL_PRINT
            Serial.println("Constructor - Trying read multi turn angle");
        #endif
    }
 
    // Use the sign of the multi turn angle to decide the whether or not to add a target position offset
    /*
    if (multi_turn_angle_32_bit_001lsb >= 0)
    {
        target_position_offset = 60.0*M_PI/180.0;
    }
    else
    {
        target_position_offset = 0.0;
    }
    */
    /*
    Serial.println("");
    Serial.print("Multiturn angle raw: "); Serial.println(multi_turn_angle_32_bit_001lsb);
    Serial.print("Target_position_offset: "); Serial.println(target_position_offset);
    while(true);
    */
}
 
bool MotorControl::readPIDParameters()
{
    // Create a CAN message for requesting the motor PID parameters
    make_can_msg::readPIDParameters(can_message.buf);
 
    // Send CAN message
    sendMessage(can_message);
 
    // Wait 0.010 seconds for a reply from the motor
    delay_microseconds(10000.0);
 
    // Check if we received a reply
    if(readMessage(received_can_message))
    {
        if((received_can_message.id == address) && (received_can_message.buf[0] == MOTOR_COMMAND_READ_PID_PARAMETERS))
        {
            // Store the PI parameters in the class
            kp_pos = received_can_message.buf[2];
            ki_pos = received_can_message.buf[3];
            kp_speed = received_can_message.buf[4];
            ki_speed = received_can_message.buf[5];
            kp_torque = received_can_message.buf[6];
            ki_torque = received_can_message.buf[7];
 
            // Report that we successfully managed to read the PID parameters
            return true;
        }
        else
        {
            #if SERIAL_PRINT
                errorMessage();
                Serial.println("In the function readPIDParameters an incorrect answer was received");
            #endif
            
            return false; 
        }
    }
    else
    {
        // Report that we failed to read the PID parameters
        #if SERIAL_PRINT
            errorMessage();
            Serial.println("In the function readPIDParameters no reply was received");
        #endif
 
        return false;
    }
}
 
bool MotorControl::writePIDParametersToRAM
(
    double _kp_pos,
    double _ki_pos,
    double _kp_speed,
    double _ki_speed,
    double _kp_torque,
    double _ki_torque
)
{
    // Create a CAN message for writing PID parameters to RAM
    make_can_msg::writePIDParametersToRAM(can_message.buf, 
        _kp_pos,
        _ki_pos,
        _kp_speed,
        _ki_speed,
        _kp_torque,
        _ki_torque
    );
 
    //clearCANBuffer();
 
    // Send CAN message
    sendMessage(can_message);
 
    // Wait 0.10 seconds for a reply from the motor
    delay_microseconds(100000.0);
 
    // Check if we received a reply from the motor
    if(readMessage(received_can_message))
    {
        // Check that the incomming message is the one we anticipated
        if((received_can_message.id == address) && (received_can_message.buf[0] == MOTOR_COMMAND_WRITE_PID_PARAMETERS_TO_RAM))
        {
            // Update the PID parameters to the new ones from the motor
            kp_pos = received_can_message.buf[2];
            ki_pos = received_can_message.buf[3];
            kp_speed = received_can_message.buf[4];
            ki_speed = received_can_message.buf[5];
            kp_torque = received_can_message.buf[6];
            ki_torque = received_can_message.buf[7];
            return true;
        }
        else
        {
            // Report that the reply we received was incorrect
            #if SERIAL_PRINT
                errorMessage();
                Serial.println("In the function writePIDParametersToRAM an incorrect reply was received");
            #endif
 
            return false;
        }
    } 
    else
    {
        // Report that we failed to write the PID parameters to RAM
        #if SERIAL_PRINT
            errorMessage();
            Serial.println("In the function writePIDParametersToRAM no reply was received");
        #endif
 
        return false;
    }
}
 
void MotorControl::setPositionReference(double _angle)
{
    // Convert the desired shaft angle in radians into an inner motor angle in 0.01 degrees 
    double inner_motor_reference_angle = -(_angle - position_offset)*180.0/M_PI*GEAR_REDUCTION*100;
 
    // Store the postion reference for debugging
    raw_position_reference = inner_motor_reference_angle;
 
    // Create a position control can message
    make_can_msg::positionControl1(can_message.buf, inner_motor_reference_angle);
 
    // Send the CAN message
    sendMessage(can_message);
}
 
void MotorControl::setSpeedReference(double _speed)
{
    // The motor has defined a CW rotation as the positive rotation direction. We use CCW as the positve rotation direction.
    _speed = -_speed;
 
    // Convert speed in rad/s to 0.01 deg/sec.
    int32_t speed_001dps = (int)round(GEAR_REDUCTION*_speed*100.0*180.0/M_PI);
 
    // Create a CAN message for motor speed control
    make_can_msg::speedControl(can_message.buf, speed_001dps);
 
    // Send CAN message
    sendMessage(can_message);
}
 
void MotorControl::setTorqueReference(double _torque)
{
    // The motor has defined a CW rotation as the positive rotation direction. We use CCW as the positve rotation direction.
    _torque = -_torque;
 
    // Saturate desired torque to be within maximum limits
    if (_torque > max_torque)
    {
        _torque = max_torque;
    }
    else if (_torque < -max_torque)
    {
        _torque = -max_torque;
    }
 
    // Scale the torque (Nm) to torque currents between [-max_torque_current, max_torque_current]
    int16_t current_torque = (int) round(max_torque_current*_torque/max_torque);
 
    torque_current_reference = current_torque;
 
    // Create a CAN message for current torque control
    make_can_msg::torqueCurrentControl(can_message.buf, current_torque);
 
    // Send CAN message
    sendMessage(can_message); 
}
 
void MotorControl::readMotorControlCommandReply(unsigned char* _can_message)
{
    // Encoder position. 16 bits represents one inner motor rotation
    uint16_t new_encoder_value = _can_message[7]*256 + _can_message[6];
 
    // Update the number of completed turns for the inner motor 
    number_of_inner_motor_rotations += innerMotorTurnCompleted(previous_encoder_value, new_encoder_value);
 
    // Update the latest received encoder value as the innerMotorTurnCompleted function has already been executed
    previous_encoder_value = new_encoder_value;
 
    // Update the shaft position in radians
    //position = (number_of_inner_motor_rotations + 1.0 - (double)new_encoder_value/(double)max_encoder_value)*ROTATION_DISTANCE*M_PI/180.0 - position_offset;
 
    position = -((double)new_encoder_value/(double)max_encoder_value - 1.0 - number_of_inner_motor_rotations)*ROTATION_DISTANCE*M_PI/180.0 + position_offset;
 
    // Get the speed of the inner motor in degrees per second
    int16_t speed_motor_dps = _can_message[5]*256 + _can_message[4];
 
    // Convert the motor speed to shaft speed in radians per second
    speed = -(double)(speed_motor_dps)*M_PI/(180.0*GEAR_REDUCTION);
 
    // Get the torque current 
    int16_t torque_current = _can_message[3]*256 + _can_message[2];
 
    torque_current_measured = torque_current;
 
    // Convert the torque current into torque through a linear scaling
    torque = -max_torque*torque_current/max_torque_current;
}
 
bool MotorControl::stopMotor()
{
    // Create a CAN Message instructing the motor to stop
    make_can_msg::motorStop(can_message.buf);
 
    // Send CAN message
    sendMessage(can_message);
 
    // Wait 0.010 seconds for a reply from the motor
    delay_microseconds(10000.0);
 
    if(readMessage(received_can_message))
    {
        if((received_can_message.id == address) && (received_can_message.buf[0] == MOTOR_COMMAND_MOTOR_STOP))
        {
            return true;
        }
        else
        {
            // Report that the reply we received was incorrect
            #if SERIAL_PRINT
                errorMessage();
                Serial.println("In the function stopMotor an incorrect reply was received");
            #endif
 
            return false;
        }
    } 
    else
    {
        // Report that we failed to stop the motor
        #if SERIAL_PRINT
            errorMessage();
            Serial.println("In the function stopMotor an incorrect reply was received");
        #endif
 
        return false;
    }
}
 
bool MotorControl::turnOffMotor()
{
    // Create a CAN message instructing the motor to turn off
    make_can_msg::motorOff(can_message.buf);
 
    // Send CAN message
    sendMessage(can_message);
 
    // Wait 0.010 seconds for a reply from the motor
    delay_microseconds(10000.0);
 
    if(readMessage(received_can_message))
    {
        if((received_can_message.id == address) && (received_can_message.buf[0] == MOTOR_COMMAND_MOTOR_OFF))
        {
            return true;
        }
        else
        {
            // Report that the reply we received was incorrect
            #if SERIAL_PRINT
                errorMessage();
                Serial.println("In the function stopMotor an incorrect reply was received");
            #endif
 
            return false;
        }
    }
    else
    {
        // Report that we failed to turn off the motor
        #if SERIAL_PRINT
            errorMessage();
            Serial.println("In the function turnOffMotor no reply was received");
        #endif
 
        return false;
    }
}
 
bool MotorControl::readMultiTurnAngle()
{
    // Create a CAN message which requests the motor to send back its multi turn angle
    make_can_msg::readMultiTurnAngle(can_message.buf);
    
    // Send the CAN message
    sendMessage(can_message);
 
    // Wait 0.010 seconds for a reply from the motor
    delay_microseconds(5000.0);
 
    if(readMessage(received_can_message))
    {
        if((received_can_message.id == address) && (received_can_message.buf[0] == MOTOR_COMMAND_READ_MULTI_TURN_ANGLE))
        {
            multi_turn_angle_001lsb = 0;
            *(uint8_t *)(& multi_turn_angle_001lsb) = received_can_message.buf[1];
            *((uint8_t *)(& multi_turn_angle_001lsb)+1) = received_can_message.buf[2];
            *((uint8_t *)(& multi_turn_angle_001lsb)+2) = received_can_message.buf[3];
            *((uint8_t *)(& multi_turn_angle_001lsb)+3) = received_can_message.buf[4];
            *((uint8_t *)(& multi_turn_angle_001lsb)+4) = received_can_message.buf[5];
            *((uint8_t *)(& multi_turn_angle_001lsb)+5) = received_can_message.buf[6];
            *((uint8_t *)(& multi_turn_angle_001lsb)+6) = received_can_message.buf[7];
 
            // Convert the multi turn angle from 0.01 deg representation to a 1.0 degree representation
            multi_turn_angle_64_bit = multi_turn_angle_001lsb/100;
 
            // Convert the multi turn angle into a 32 bit int for printing
            if(multi_turn_angle_001lsb >= 0)
            {
               multi_turn_angle_32_bit_001lsb = multi_turn_angle_001lsb;
            }
            else
            {
                int64_t multi_turn_angle_64_bit_001lsb = - multi_turn_angle_001lsb;
                multi_turn_angle_32_bit_001lsb = multi_turn_angle_64_bit_001lsb;
                multi_turn_angle_32_bit_001lsb = - multi_turn_angle_32_bit_001lsb;
            }
            multi_turn_angle_32_bit = multi_turn_angle_32_bit_001lsb/100;
 
            return true;
        }
        else
        {
            // Report that the reply we received was incorrect
            #if SERIAL_PRINT
                errorMessage();
                Serial.println("In the function readMultiTurnAngle an incorrect reply was received");
            #endif
 
            return false;
        }
    }
    else
    {
        // Report that no reply was received
        #if SERIAL_PRINT
            errorMessage();
            Serial.println("In the function readMultiTurnAngle no reply was received");
        #endif
 
        return false;    
    }
}
 
bool MotorControl::readMotorStatus()
{
    //can_message.id = 0x141;
    make_can_msg::readMotorStatus2(can_message.buf);
    //can_port_1.write(can_message);
    
    sendMessage(can_message);
 
    delay_microseconds(10000.0);
    
 
    //Serial.print("can msg id: "); Serial.print(can_message.id, HEX); Serial.print("\t can_msg type:\t"); Serial.print(can_message.buf[0], HEX); Serial.println("");
 
    if(readMessage(received_can_message))
    {
        //Serial.println("Reply received");
        if((received_can_message.id == address) && (received_can_message.buf[0] == MOTOR_COMMAND_READ_MOTOR_STATUS_2))
        {
            readMotorControlCommandReply(received_can_message.buf);
            return true;
        }
        else
        {
            // Report that the reply we received was incorrect
            #if SERIAL_PRINT
                errorMessage();
                Serial.println("In the function readMotorStatus an incorrect reply was received");
            #endif
 
            return false;
        }
    }
    else
    {
        // Report that no reply was received
        #if (SERIAL_PRINT)
            errorMessage();
            Serial.println("In the function readMotorStatus no reply was received");
        #endif
 
        return false;
    }
}
 
void MotorControl::requestMotorStatus()
{
    // Create a CAN message which requests the motor to send back a reply
    make_can_msg::readMotorStatus2(can_message.buf);
 
    // Send the CAN message
    sendMessage(can_message);
}
 
bool MotorControl::readCompleteEncoderPosition()
{
    make_can_msg::readEncoderPosition(can_message.buf);
 
    sendMessage(can_message);
 
    delay_microseconds(10000.0);
 
    if(readMessage(received_can_message))
    {
        if((received_can_message.id == address) && (received_can_message.buf[0]) == MOTOR_COMMAND_READ_ENCODER_POSITION)
        {
            double encoder_with_offset = received_can_message.buf[3]*256 + received_can_message.buf[0]; 
            double encoder_raw = received_can_message.buf[5]*256 + received_can_message.buf[4];
            double encoder_offset = received_can_message.buf[7]*256 + received_can_message.buf[6];
 
            //Serial.print("ENC W/O:\t"); Serial.print(encoder_with_offset); Serial.print("\t");
            //Serial.print("ENC RAW:\t"); Serial.print(encoder_raw); Serial.print("\t");
            //Serial.print("ENC OFF:\t"); Serial.print(encoder_offset); Serial.print("\t");
            //Serial.println("");
        }
        else
        {
            #if SERIAL_PRINT
                errorMessage();
                Serial.println("getCompleteEncoderPosition - wrong reply received.");
            #endif
 
            return false;        
        }
    }
    else
    {
        #if SERIAL_PRINT
            errorMessage();
            Serial.println("getCompleteEncoderPosition - no reply received.");
        #endif
 
        return false;
    }
}
 
void MotorControl::getPIDParameters(
    double &_kp_pos,
    double &_ki_pos,
    double &_kp_speed,
    double &_ki_speed,
    double &_kp_torque,
    double &_ki_torque)
{
    _kp_pos = kp_pos;
    _ki_pos = ki_pos;
    _kp_speed = kp_speed;
    _ki_speed = ki_speed;
    _kp_torque = kp_torque;
    _ki_torque = ki_torque;
}
 
void MotorControl::printPIDParameters()
{
    #if SERIAL_PRINT
        Serial.print("Motor "); Serial.print(id); Serial.print(" - CAN "); Serial.print(can_port_id); Serial.println(" ");
        Serial.print("kp_pos:\t"); Serial.println(kp_pos);
        Serial.print("ki_pos:\t"); Serial.println(ki_pos);
        Serial.print("kp_vel:\t"); Serial.println(kp_speed);
        Serial.print("kp_vel:\t"); Serial.println(ki_speed);
        Serial.print("kp_tor:\t"); Serial.println(kp_torque);
        Serial.print("kp_tor:\t"); Serial.println(ki_torque);
    #endif
}
 
void MotorControl::printState()
{
    #if SERIAL_PRINT
        Serial.print("Motor "); Serial.print(id); Serial.print(", CAN "); Serial.print(can_port_id); Serial.print(" - ");
        Serial.print("Pos: "); Serial.print(position); Serial.print("[rad]\t");
        Serial.print("Vel: "); Serial.print(speed); Serial.print("[rad/s]\t");
        Serial.print("Tor: "); Serial.print(torque); Serial.print("[Nm]\t");
        Serial.print("MTA: "); Serial.print(multi_turn_angle_32_bit); Serial.print("\t");
        Serial.print("Enc: "); Serial.print(previous_encoder_value); Serial.print("\t");
        Serial.print("IMR: "); Serial.print(number_of_inner_motor_rotations); Serial.print("\t");
        Serial.println("");
    #endif
 
}
 
void MotorControl::setTorqueCurrent(int _torque_current)
{
    _torque_current = -_torque_current;
    
    if(_torque_current > max_torque_current)
    {
        _torque_current = max_torque_current;
    }
    else if(_torque_current < -max_torque_current)
    {
        _torque_current = -max_torque_current;
    }
 
    //Serial.print(_torque_current); Serial.print("\t");
 
    make_can_msg::torqueCurrentControl(can_message.buf, _torque_current);
 
    sendMessage(can_message);
}
 
double MotorControl::innerMotorTurnCompleted(uint16_t _previous_encoder_value, uint16_t _new_encoder_value)
{
    // If old >> new a CW turn was completed
    if(_previous_encoder_value - _new_encoder_value > encoder_turn_threshold)
    {
        return -1.0;
    }
    // If new >> old a CCW turn was completed
    else if(_new_encoder_value - _previous_encoder_value > encoder_turn_threshold)
    {
        return 1.0;
    }
    // No turn was completed
    else
    {
        return 0.0;
    }
}
 
void MotorControl::sendMessage(CAN_message_t _can_message)
{
    if(can_port_id == CAN_PORT_1)
    {
        can_port_1.write(_can_message);
    }
    else if(can_port_id == CAN_PORT_2)
    {
        can_port_2.write(_can_message);
    }
    else
    {
        #if SERIAL_PRINT
            errorMessage();
            Serial.println("In the function sendMessage, an invalid CAN port was selected");
        #endif
 
        while(true);
    }
}
 
int MotorControl::readMessage(CAN_message_t &_can_message)
{
    if(can_port_id == 1)
    {
        return can_port_1.read(_can_message);
    }
    else if(can_port_id == 2)
    {
        return can_port_2.read(_can_message);
    }
    else
    {
        #if SERIAL_PRINT
            errorMessage();
            Serial.println("In the function readMessage, an invalid CAN port was selected");
        #endif
    }
}
 
void MotorControl::errorMessage()
{
    //Serial.print("ERROR - Motor "); Serial.print(id); 
    //Serial.print(", CAN "); Serial.print(can_port_id); 
    //Serial.print("-\t");
}