Team DIANA / MX106_not_working

A lot of change

Dependents:   MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale

MX106.cpp@0:8fda718cb527, 2016-05-23 (annotated)

Committer:
dconsoli
Date:
Mon May 23 02:18:19 2016 +0000
Revision:
0:8fda718cb527
Child:
1:555b9b4a1ecd
A lot of change

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dconsoli 0:8fda718cb527 1 #include "MX106.h"
dconsoli 0:8fda718cb527 2 #include "mbed.h"
dconsoli 0:8fda718cb527 3
dconsoli 0:8fda718cb527 4 //360°/4095
dconsoli 0:8fda718cb527 5 #define RESOLUTION 0,088
dconsoli 0:8fda718cb527 6 //bit per degrees 4095/360°
dconsoli 0:8fda718cb527 7 #define BIT_DEG 11,375
dconsoli 0:8fda718cb527 8
dconsoli 0:8fda718cb527 9 #define READ_DEBUG 1
dconsoli 0:8fda718cb527 10
dconsoli 0:8fda718cb527 11 MX106::MX106MX106(int ID, comunication_1 line, float gear_train) {
dconsoli 0:8fda718cb527 12 _gear_train = gear_train;
dconsoli 0:8fda718cb527 13 _line = line;
dconsoli 0:8fda718cb527 14 _ID = ID;
dconsoli 0:8fda718cb527 15 }
dconsoli 0:8fda718cb527 16
dconsoli 0:8fda718cb527 17
dconsoli 0:8fda718cb527 18 int MX106::SetMode(int mode) {
dconsoli 0:8fda718cb527 19 switch (mode){
dconsoli 0:8fda718cb527 20 //Wheel Mode
dconsoli 0:8fda718cb527 21 case (0):
dconsoli 0:8fda718cb527 22 SetCWLimit(0);
dconsoli 0:8fda718cb527 23 SetCCWLimit(0);
dconsoli 0:8fda718cb527 24 SetCRSpeed(0.0);
dconsoli 0:8fda718cb527 25 _mode = mode;
dconsoli 0:8fda718cb527 26 break;
dconsoli 0:8fda718cb527 27 //Joint Mode
dconsoli 0:8fda718cb527 28 case (1):
dconsoli 0:8fda718cb527 29 SetCWLimit(RESOLUTION);
dconsoli 0:8fda718cb527 30 SetCCWLimit(RESOLUTION);
dconsoli 0:8fda718cb527 31 SetCRSpeed(0.0);
dconsoli 0:8fda718cb527 32 _mode = mode;
dconsoli 0:8fda718cb527 33 break;
dconsoli 0:8fda718cb527 34 //Multi-turn Mode
dconsoli 0:8fda718cb527 35 case (2):
dconsoli 0:8fda718cb527 36 SetCWLimit(360);
dconsoli 0:8fda718cb527 37 SetCCWLimit(360);
dconsoli 0:8fda718cb527 38 SetCRSpeed(0.0);
dconsoli 0:8fda718cb527 39 _mode = mode;
dconsoli 0:8fda718cb527 40 break;
dconsoli 0:8fda718cb527 41 //other cases
dconsoli 0:8fda718cb527 42 default:
dconsoli 0:8fda718cb527 43 if(READ_DEBUG){
dconsoli 0:8fda718cb527 44 printf("Not valid mode");
dconsoli 0:8fda718cb527 45 }
dconsoli 0:8fda718cb527 46 }
dconsoli 0:8fda718cb527 47 return(0);
dconsoli 0:8fda718cb527 48 }
dconsoli 0:8fda718cb527 49
dconsoli 0:8fda718cb527 50
dconsoli 0:8fda718cb527 51 int MX106::SetCWLimit(float degrees) {
dconsoli 0:8fda718cb527 52
dconsoli 0:8fda718cb527 53 char data[2];
dconsoli 0:8fda718cb527 54
dconsoli 0:8fda718cb527 55 short limit = (short)(BIT_DEG * degrees * _gear_train);
dconsoli 0:8fda718cb527 56
dconsoli 0:8fda718cb527 57 data[0] = limit & 0xff; // bottom 8 bits
dconsoli 0:8fda718cb527 58 data[1] = limit >> 8; // top 8 bits
dconsoli 0:8fda718cb527 59
dconsoli 0:8fda718cb527 60 // write the packet, return the error code
dconsoli 0:8fda718cb527 61 return(_line.write(_ID, REG_CW_LIMIT, 2, data));
dconsoli 0:8fda718cb527 62 }
dconsoli 0:8fda718cb527 63
dconsoli 0:8fda718cb527 64
dconsoli 0:8fda718cb527 65 int MX106::SetCWLimit(float degrees) {
dconsoli 0:8fda718cb527 66
dconsoli 0:8fda718cb527 67 char data[2];
dconsoli 0:8fda718cb527 68
dconsoli 0:8fda718cb527 69 short limit = (short)(BIT_DEG * degrees * _gear_train);
dconsoli 0:8fda718cb527 70
dconsoli 0:8fda718cb527 71 data[0] = limit & 0xff; // bottom 8 bits
dconsoli 0:8fda718cb527 72 data[1] = limit >> 8; // top 8 bits
dconsoli 0:8fda718cb527 73
dconsoli 0:8fda718cb527 74 // write the packet, return the error code
dconsoli 0:8fda718cb527 75 return(_line.write(_ID, REG_CCW_LIMIT, 2, data));
dconsoli 0:8fda718cb527 76 }
dconsoli 0:8fda718cb527 77
dconsoli 0:8fda718cb527 78
dconsoli 0:8fda718cb527 79 int MX106::GoalPosition(float degrees); {
dconsoli 0:8fda718cb527 80
dconsoli 0:8fda718cb527 81 char data[2];
dconsoli 0:8fda718cb527 82
dconsoli 0:8fda718cb527 83 short gaol_position = (short)(BIT_DEG * degrees * _gear_train);
dconsoli 0:8fda718cb527 84
dconsoli 0:8fda718cb527 85 data[0] = gaol_position & 0xff; // bottom 8 bits
dconsoli 0:8fda718cb527 86 data[1] = gaol_position >> 8; // top 8 bits
dconsoli 0:8fda718cb527 87
dconsoli 0:8fda718cb527 88 // write the packet, return the error code
dconsoli 0:8fda718cb527 89 return(_line.write(_ID, REG_GOAL_POSITION, 2, data));
dconsoli 0:8fda718cb527 90 }
dconsoli 0:8fda718cb527 91
dconsoli 0:8fda718cb527 92
dconsoli 0:8fda718cb527 93 int MX106::SetSpeed(float goal_speed) {
dconsoli 0:8fda718cb527 94
dconsoli 0:8fda718cb527 95 // bit 10 = direction, 0 = CCW, 1=CW
dconsoli 0:8fda718cb527 96 // bits 9-0 = Speed
dconsoli 0:8fda718cb527 97 char data[2];
dconsoli 0:8fda718cb527 98
dconsoli 0:8fda718cb527 99 int goal = (0x3ff * abs(speed * _gear_train));
dconsoli 0:8fda718cb527 100
dconsoli 0:8fda718cb527 101 // Set direction CW if we have a negative speed
dconsoli 0:8fda718cb527 102 if (speed < 0) {
dconsoli 0:8fda718cb527 103 goal |= (0x1 << 10);
dconsoli 0:8fda718cb527 104 }
dconsoli 0:8fda718cb527 105
dconsoli 0:8fda718cb527 106 data[0] = goal & 0xff; // bottom 8 bits
dconsoli 0:8fda718cb527 107 data[1] = goal >> 8; // top 8 bits
dconsoli 0:8fda718cb527 108
dconsoli 0:8fda718cb527 109 // write the packet, return the error code
dconsoli 0:8fda718cb527 110 return(_line.write(_ID, REG_MOVING_SPEED, 2, data));
dconsoli 0:8fda718cb527 111 }
dconsoli 0:8fda718cb527 112
dconsoli 0:8fda718cb527 113
dconsoli 0:8fda718cb527 114 float MX106::GetTemp (void) {
dconsoli 0:8fda718cb527 115 char data[1];
dconsoli 0:8fda718cb527 116 int ErrorCode = _line.read(_ID, REG_TEMP, 1, data);
dconsoli 0:8fda718cb527 117 float temp = data[0];
dconsoli 0:8fda718cb527 118 return(temp);
dconsoli 0:8fda718cb527 119 }