Tatsuya Yamamoto / Mbed 2 deprecated UV_Robot_Nucleo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed X_NUCLEO_PLC01A1 ros_lib_melodic

ms-94bzb.cpp

Committer:
yamadola
Date:
2020-08-15
Revision:
1:ef4b86795d79
Parent:
0:43eb9ccc1583
Child:
3:ea5cfd721b53

File content as of revision 1:ef4b86795d79:

#include "ms-94bzb.h"

/**
 * @brief  MS-94BZB constractor
 * @param  motorID: Node device ID of target MS-94BZB motor (7 bits value)
 * @param  pCanController: Address of CAN controller
*/
MS_94BZB::MS_94BZB( uint8_t motorID, CANController *pCanController)
:Motor(motorID),
_pCanController(pCanController),
_gearRatio(10.33),
_refMotorAngularVelocity(0),
DIR(0),
_accel(100)
{}

/**
 * @brief  Initialize motor controller
 * @param  None
 * @retval None
*/
void MS_94BZB::motorInit(){
    uint16_t CAN_ID = CTL_COMMAND|_motorID;
    char Data[8] = {CAN_CTL, 1, 0};
    _pCanController->sendMessage( CAN_ID, Data );
}

/**
 * @brief  Drive motor with set velocity
 * @param  None
 * @retval None
*/
void MS_94BZB::drive(){
    if(_refMotorAngularVelocity > 60){
        uint16_t CAN_ID = MOTOR_COMMAND|_motorID;
        char Data[8] = {DRIVE|DIR, _refMotorAngularVelocity>>8, _refMotorAngularVelocity&0x00FF, 100>>8, 100&0x00FF, 500>>8, 500&0x00FF};
        _pCanController->sendMessage( CAN_ID, Data );
    }else{
        MS_94BZB::stop();
    }
}

/**
 * @brief  Stop motor
 * @param  None
 * @retval None
*/
void MS_94BZB::stop(){
    uint16_t CAN_ID = MOTOR_COMMAND|_motorID;
    char Data[8] = {STOP|SHORT, 0, 0, 0, 0, 0, 0};
    _pCanController->sendMessage( CAN_ID, Data );
}

/**
 * @brief  Make motor free
 * @param  None
 * @retval None
*/
void MS_94BZB::free(){
    uint16_t CAN_ID = MOTOR_COMMAND|_motorID;
    char Data[8] = {STOP|FREE, 0, 0, 0, 0, 0, 0};
    _pCanController->sendMessage( CAN_ID, Data );
}

/**
 * @brief  Set reference angular velocity of MS-94BZB
 * @param  avel: Output angular velocity (6-300[rpm])
 * @retval None
*/
void MS_94BZB::setAngularVelocity(float avel){
    if(avel>0){
        _refMotorAngularVelocity = (uint16_t)(_gearRatio*avel);
        DIR = CW;
    }else{
        _refMotorAngularVelocity = (uint16_t)(-1*_gearRatio*avel);
        DIR = CCW;
    }
}

/**
 * @brief  Set reference acceleration of MS-94BZB
 * @param  accel: Output acceleration (10-1080[rpm/s])
 * @retval None
*/
void MS_94BZB::setAcceleration(uint16_t accel){
    _accel = (uint16_t)(_gearRatio*accel);
}