Diff: ms-94bzb.cpp

Revision:

0:43eb9ccc1583

Child:

1:ef4b86795d79

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ms-94bzb.cpp	Sat Aug 08 23:54:01 2020 +0000
@@ -0,0 +1,87 @@
+#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 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 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);
+}
\ No newline at end of file