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@3:ea5cfd721b53, 2020-08-30 (annotated)

Committer:
yamadola
Date:
Sun Aug 30 06:39:43 2020 +0000
Revision:
3:ea5cfd721b53
Parent:
1:ef4b86795d79 
This code includes all functions for Amabie.; However, I have never debugged it with the real body.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
yamadola 0:43eb9ccc1583 1 #include "ms-94bzb.h"
yamadola 0:43eb9ccc1583 2
yamadola 0:43eb9ccc1583 3 /**
yamadola 0:43eb9ccc1583 4 * @brief MS-94BZB constractor
yamadola 0:43eb9ccc1583 5 * @param motorID: Node device ID of target MS-94BZB motor (7 bits value)
yamadola 0:43eb9ccc1583 6 * @param pCanController: Address of CAN controller
yamadola 0:43eb9ccc1583 7 */
yamadola 0:43eb9ccc1583 8 MS_94BZB::MS_94BZB( uint8_t motorID, CANController *pCanController)
yamadola 0:43eb9ccc1583 9 :Motor(motorID),
yamadola 0:43eb9ccc1583 10 _pCanController(pCanController),
yamadola 0:43eb9ccc1583 11 _gearRatio(10.33),
yamadola 0:43eb9ccc1583 12 _refMotorAngularVelocity(0),
yamadola 0:43eb9ccc1583 13 DIR(0),
yamadola 0:43eb9ccc1583 14 _accel(100)
yamadola 0:43eb9ccc1583 15 {}
yamadola 0:43eb9ccc1583 16
yamadola 0:43eb9ccc1583 17 /**
yamadola 0:43eb9ccc1583 18 * @brief Initialize motor controller
yamadola 0:43eb9ccc1583 19 * @param None
yamadola 0:43eb9ccc1583 20 * @retval None
yamadola 0:43eb9ccc1583 21 */
yamadola 0:43eb9ccc1583 22 void MS_94BZB::motorInit(){
yamadola 0:43eb9ccc1583 23 uint16_t CAN_ID = CTL_COMMAND|_motorID;
yamadola 0:43eb9ccc1583 24 char Data[8] = {CAN_CTL, 1, 0};
yamadola 0:43eb9ccc1583 25 _pCanController->sendMessage( CAN_ID, Data );
yamadola 0:43eb9ccc1583 26 }
yamadola 0:43eb9ccc1583 27
yamadola 0:43eb9ccc1583 28 /**
yamadola 0:43eb9ccc1583 29 * @brief Drive motor with set velocity
yamadola 0:43eb9ccc1583 30 * @param None
yamadola 0:43eb9ccc1583 31 * @retval None
yamadola 0:43eb9ccc1583 32 */
yamadola 0:43eb9ccc1583 33 void MS_94BZB::drive(){
yamadola 0:43eb9ccc1583 34 if(_refMotorAngularVelocity > 60){
yamadola 0:43eb9ccc1583 35 uint16_t CAN_ID = MOTOR_COMMAND|_motorID;
yamadola 0:43eb9ccc1583 36 char Data[8] = {DRIVE|DIR, _refMotorAngularVelocity>>8, _refMotorAngularVelocity&0x00FF, 100>>8, 100&0x00FF, 500>>8, 500&0x00FF};
yamadola 0:43eb9ccc1583 37 _pCanController->sendMessage( CAN_ID, Data );
yamadola 0:43eb9ccc1583 38 }else{
yamadola 0:43eb9ccc1583 39 MS_94BZB::stop();
yamadola 0:43eb9ccc1583 40 }
yamadola 0:43eb9ccc1583 41 }
yamadola 0:43eb9ccc1583 42
yamadola 0:43eb9ccc1583 43 /**
yamadola 0:43eb9ccc1583 44 * @brief Stop motor
yamadola 0:43eb9ccc1583 45 * @param None
yamadola 0:43eb9ccc1583 46 * @retval None
yamadola 0:43eb9ccc1583 47 */
yamadola 0:43eb9ccc1583 48 void MS_94BZB::stop(){
yamadola 0:43eb9ccc1583 49 uint16_t CAN_ID = MOTOR_COMMAND|_motorID;
yamadola 0:43eb9ccc1583 50 char Data[8] = {STOP|SHORT, 0, 0, 0, 0, 0, 0};
yamadola 0:43eb9ccc1583 51 _pCanController->sendMessage( CAN_ID, Data );
yamadola 0:43eb9ccc1583 52 }
yamadola 0:43eb9ccc1583 53
yamadola 0:43eb9ccc1583 54 /**
yamadola 0:43eb9ccc1583 55 * @brief Make motor free
yamadola 0:43eb9ccc1583 56 * @param None
yamadola 0:43eb9ccc1583 57 * @retval None
yamadola 0:43eb9ccc1583 58 */
yamadola 0:43eb9ccc1583 59 void MS_94BZB::free(){
yamadola 0:43eb9ccc1583 60 uint16_t CAN_ID = MOTOR_COMMAND|_motorID;
yamadola 0:43eb9ccc1583 61 char Data[8] = {STOP|FREE, 0, 0, 0, 0, 0, 0};
yamadola 0:43eb9ccc1583 62 _pCanController->sendMessage( CAN_ID, Data );
yamadola 0:43eb9ccc1583 63 }
yamadola 0:43eb9ccc1583 64
yamadola 0:43eb9ccc1583 65 /**
yamadola 1:ef4b86795d79 66 * @brief Set reference angular velocity of MS-94BZB
yamadola 0:43eb9ccc1583 67 * @param avel: Output angular velocity (6-300[rpm])
yamadola 0:43eb9ccc1583 68 * @retval None
yamadola 0:43eb9ccc1583 69 */
yamadola 0:43eb9ccc1583 70 void MS_94BZB::setAngularVelocity(float avel){
yamadola 0:43eb9ccc1583 71 if(avel>0){
yamadola 0:43eb9ccc1583 72 _refMotorAngularVelocity = (uint16_t)(_gearRatio*avel);
yamadola 0:43eb9ccc1583 73 DIR = CW;
yamadola 0:43eb9ccc1583 74 }else{
yamadola 0:43eb9ccc1583 75 _refMotorAngularVelocity = (uint16_t)(-1*_gearRatio*avel);
yamadola 0:43eb9ccc1583 76 DIR = CCW;
yamadola 0:43eb9ccc1583 77 }
yamadola 0:43eb9ccc1583 78 }
yamadola 0:43eb9ccc1583 79
yamadola 0:43eb9ccc1583 80 /**
yamadola 1:ef4b86795d79 81 * @brief Set reference acceleration of MS-94BZB
yamadola 0:43eb9ccc1583 82 * @param accel: Output acceleration (10-1080[rpm/s])
yamadola 0:43eb9ccc1583 83 * @retval None
yamadola 0:43eb9ccc1583 84 */
yamadola 0:43eb9ccc1583 85 void MS_94BZB::setAcceleration(uint16_t accel){
yamadola 0:43eb9ccc1583 86 _accel = (uint16_t)(_gearRatio*accel);
yamadola 3:ea5cfd721b53 87 }
yamadola 3:ea5cfd721b53 88
yamadola 3:ea5cfd721b53 89 uint16_t MS_94BZB::getInputVoltage(){
yamadola 3:ea5cfd721b53 90 uint16_t CAN_ID = STATE_COMMAND_2|_motorID;
yamadola 3:ea5cfd721b53 91 unsigned char *rMsg;
yamadola 3:ea5cfd721b53 92 rMsg = _pCanController->recieveMessage( CAN_ID );
yamadola 3:ea5cfd721b53 93 if(rMsg != _pCanController->_NoData){
yamadola 3:ea5cfd721b53 94 _voltage = rMsg[5]<<8 + rMsg[6];
yamadola 3:ea5cfd721b53 95 }
yamadola 3:ea5cfd721b53 96 return _voltage;
yamadola 0:43eb9ccc1583 97 }