Team DIANA / MX106_not_working

A lot of change

Dependents:   MX106-finaltest dynamixel Arm_dynamixel_can Arm_dynamixel_can_procedurale

MX106.cpp@1:555b9b4a1ecd, 2016-06-26 (annotated)

Committer:
mattiasub
Date:
Sun Jun 26 20:46:08 2016 +0000
Revision:
1:555b9b4a1ecd
Parent:
0:8fda718cb527
Child:
2:6722a00e6184
Risolti errori su Communication_1 . Da risolvere ancora MX106

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"
mattiasub 1:555b9b4a1ecd 3 #include "communication_1.h"
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
mattiasub 1:555b9b4a1ecd 11 MX106::MX106(int ID, communication_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
mattiasub 1:555b9b4a1ecd 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 }
mattiasub 1:555b9b4a1ecd 112 int MX106::SetCRSpeed(float speed) {
mattiasub 1:555b9b4a1ecd 113
mattiasub 1:555b9b4a1ecd 114 // bit 10 = direction, 0 = CCW, 1=CW
mattiasub 1:555b9b4a1ecd 115 // bits 9-0 = Speed
mattiasub 1:555b9b4a1ecd 116 char data[2];
mattiasub 1:555b9b4a1ecd 117
mattiasub 1:555b9b4a1ecd 118 int goal = (0x3ff * abs(speed));
mattiasub 1:555b9b4a1ecd 119
mattiasub 1:555b9b4a1ecd 120 // Set direction CW if we have a negative speed
mattiasub 1:555b9b4a1ecd 121 if (speed < 0) {
mattiasub 1:555b9b4a1ecd 122 goal |= (0x1 << 10);
mattiasub 1:555b9b4a1ecd 123 }
mattiasub 1:555b9b4a1ecd 124
mattiasub 1:555b9b4a1ecd 125 data[0] = goal & 0xff; // bottom 8 bits
mattiasub 1:555b9b4a1ecd 126 data[1] = goal >> 8; // top 8 bits
mattiasub 1:555b9b4a1ecd 127
mattiasub 1:555b9b4a1ecd 128 // write the packet, return the error code
mattiasub 1:555b9b4a1ecd 129 int rVal = write( _ID, 0x20, 2, data);
mattiasub 1:555b9b4a1ecd 130
mattiasub 1:555b9b4a1ecd 131 return(rVal);
mattiasub 1:555b9b4a1ecd 132 }
dconsoli 0:8fda718cb527 133
dconsoli 0:8fda718cb527 134
mattiasub 1:555b9b4a1ecd 135 float MX106::GetTemp(void) {
dconsoli 0:8fda718cb527 136 char data[1];
mattiasub 1:555b9b4a1ecd 137 int ErrorCode = _line.read( _ID, REG_TEMP, 1, data);
dconsoli 0:8fda718cb527 138 float temp = data[0];
dconsoli 0:8fda718cb527 139 return(temp);
dconsoli 0:8fda718cb527 140 }