A lot of change
Dependents: MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale
Diff: MX106.cpp
- Revision:
- 3:adeaefc32a41
- Parent:
- 2:6722a00e6184
diff -r 6722a00e6184 -r adeaefc32a41 MX106.cpp --- a/MX106.cpp Sun Jun 26 21:46:29 2016 +0000 +++ b/MX106.cpp Sat Jul 02 18:01:01 2016 +0000 @@ -1,6 +1,7 @@ #include "MX106.h" #include "mbed.h" #include "communication_1.h" +#include <cmath> //360°/4095 #define RESOLUTION 0.088 //bit per degrees 4095/360° @@ -8,132 +9,6 @@ #define READ_DEBUG 1 -MX106::MX106(int ID, communication_1& line, float gear_train) : _line(line) { - _gear_train = gear_train; - _ID = ID; -} - - -int MX106::SetMode(int mode) { - switch (mode){ - //Wheel Mode - case (0): - SetCWLimit(0); - SetCCWLimit(0); - SetCRSpeed(0.0); - _mode = mode; - break; - //Joint Mode - case (1): - SetCWLimit(RESOLUTION); - SetCCWLimit(RESOLUTION); - SetCRSpeed(0.0); - _mode = mode; - break; - //Multi-turn Mode - case (2): - SetCWLimit(360); - SetCCWLimit(360); - SetCRSpeed(0.0); - _mode = mode; - break; - //other cases - default: - if(READ_DEBUG){ - printf("Not valid mode"); - } - } - return(0); -} - - -int MX106::SetCWLimit(float degrees) { - - char data[2]; - - short limit = (short)(BIT_DEG * degrees * _gear_train); - - data[0] = limit & 0xff; // bottom 8 bits - data[1] = limit >> 8; // top 8 bits - - // write the packet, return the error code - return(_line.write(_ID, REG_CW_LIMIT, 2, data)); +MX106::MX106(communication_1& line, int ID, float gear_train) : MX(line, ID, gear_train) { } -int MX106::SetCCWLimit (float degrees) { - - char data[2]; - - //TODO: review this - short limit = (short)(BIT_DEG * degrees * _gear_train); - - - data[0] = limit & 0xff; // bottom 8 bits - data[1] = limit >> 8; // top 8 bits - - // write the packet, return the error code - return (_line.write(_ID, REG_CCW_LIMIT, 2, data)); -} - -int MX106::GoalPosition(float degrees) { - - char data[2]; - - short gaol_position = (short)(BIT_DEG * degrees * _gear_train); - - data[0] = gaol_position & 0xff; // bottom 8 bits - data[1] = gaol_position >> 8; // top 8 bits - - // write the packet, return the error code - return(_line.write(_ID, REG_GOAL_POSITION, 2, data)); -} - - -int MX106::SetSpeed(float goal_speed) { - - // bit 10 = direction, 0 = CCW, 1=CW - // bits 9-0 = Speed - char data[2]; - - int goal = (0x3ff * abs(goal_speed * _gear_train)); - - // Set direction CW if we have a negative speed - if (goal_speed < 0) { - goal |= (0x1 << 10); - } - - data[0] = goal & 0xff; // bottom 8 bits - data[1] = goal >> 8; // top 8 bits - - // write the packet, return the error code - return(_line.write(_ID, REG_MOVING_SPEED, 2, data)); -} -int MX106::SetCRSpeed(float speed) { - - // bit 10 = direction, 0 = CCW, 1=CW - // bits 9-0 = Speed - char data[2]; - - int goal = (0x3ff * abs(speed)); - - // Set direction CW if we have a negative speed - if (speed < 0) { - goal |= (0x1 << 10); - } - - data[0] = goal & 0xff; // bottom 8 bits - data[1] = goal >> 8; // top 8 bits - - // write the packet, return the error code - int rVal = _line.write( _ID, 0x20, 2, data); - - return(rVal); -} - - -float MX106::GetTemp(void) { - char data[1]; - int ErrorCode = _line.read( _ID, REG_TEMP, 1, data); - float temp = data[0]; - return(temp); -}