Team DIANA / Mbed OS Arm_dynamixel_DEVEL

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:   Servo AX12_final MX106_not_working comunication_1

joint.h@9:34f4eb572c47, 2016-07-02 (annotated)

Committer:
clynamen
Date:
Sat Jul 02 15:52:53 2016 +0000
Revision:
9:34f4eb572c47
Parent:
8:3afb6355c0e7 
add comments for corrections;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ilaria 8:3afb6355c0e7 1 #include <iostream>
ilaria 8:3afb6355c0e7 2 #include "SerialHalfDuplex.h"
ilaria 8:3afb6355c0e7 3 #include <AX12.h>
ilaria 8:3afb6355c0e7 4 #include <communication_1.h>
ilaria 8:3afb6355c0e7 5 #include <MX64.h>
ilaria 8:3afb6355c0e7 6 #include "mbed.h"
ilaria 8:3afb6355c0e7 7 #define MX_REG_ID 0x3
ilaria 8:3afb6355c0e7 8 #define MX_REG_CW_LIMIT 0x06
ilaria 8:3afb6355c0e7 9 #define MX_REG_CCW_LIMIT 0x08
ilaria 8:3afb6355c0e7 10 #define MX_REG_GOAL_POSITION 0x1E
ilaria 8:3afb6355c0e7 11 #define MX_REG_MOVING_SPEED 0x20
ilaria 8:3afb6355c0e7 12 #define REG_VOLTS 0x2A
ilaria 8:3afb6355c0e7 13 #define MX_REG_AMPERE 0x44
ilaria 8:3afb6355c0e7 14 #define MX_REG_TEMP 0x2B
ilaria 8:3afb6355c0e7 15 #define MX_REG_PGAIN 0x1C
ilaria 8:3afb6355c0e7 16 #define MX_REG_IGAIN 0x1B
ilaria 8:3afb6355c0e7 17 #define MX_REG_DGAIN 0X1A
ilaria 8:3afb6355c0e7 18 #define MX_REG_MOVING 0x2E
ilaria 8:3afb6355c0e7 19 #define MX_REG_MAXTORQUE 0x22
ilaria 8:3afb6355c0e7 20 #define MX_REG_POSITION 0x24
ilaria 8:3afb6355c0e7 21 #define MX_RESOLUTION 0,088
ilaria 8:3afb6355c0e7 22 //bit per degrees 4095/360°
ilaria 8:3afb6355c0e7 23 #define MX_REG_MOTORONOFF 0x18
ilaria 8:3afb6355c0e7 24 #define MX_BIT_DEG 11,375
ilaria 8:3afb6355c0e7 25 #define MX_DEBUG 1
ilaria 8:3afb6355c0e7 26 #define MX_TRIGGER_DEBUG 1
ilaria 8:3afb6355c0e7 27
ilaria 8:3afb6355c0e7 28 #define MX_READ_DEBUG 1
ilaria 8:3afb6355c0e7 29 #define AX12_WRITE_DEBUG 1
ilaria 8:3afb6355c0e7 30 #define AX12_READ_DEBUG 1
ilaria 8:3afb6355c0e7 31 #define AX12_TRIGGER_DEBUG 1
ilaria 8:3afb6355c0e7 32 #define AX12_DEBUG 1
ilaria 8:3afb6355c0e7 33
ilaria 8:3afb6355c0e7 34 #define AX12_REG_ID 0x3
ilaria 8:3afb6355c0e7 35 #define AX12_REG_CW_LIMIT 0x06
ilaria 8:3afb6355c0e7 36 #define AX12_REG_CCW_LIMIT 0x08
ilaria 8:3afb6355c0e7 37 #define AX12_REG_GOAL_POSITION 0x1E
ilaria 8:3afb6355c0e7 38 #define AX12_REG_MOVING_SPEED 0x20
ilaria 8:3afb6355c0e7 39 #define AX12_REG_VOLTS 0x2A
ilaria 8:3afb6355c0e7 40 #define AX12_REG_TEMP 0x2B
ilaria 8:3afb6355c0e7 41 #define AX12_REG_MOVING 0x2E
ilaria 8:3afb6355c0e7 42 #define AX12_REG_POSITION 0x24
ilaria 8:3afb6355c0e7 43 #define AX12_REG_MAXTORQUE 0x22
ilaria 8:3afb6355c0e7 44 #define AX12_REG_MOTORONOFF 0x18
ilaria 8:3afb6355c0e7 45 #define AX12_MODE_POSITION 0
ilaria 8:3afb6355c0e7 46 #define AX12_MODE_ROTATION 1
ilaria 8:3afb6355c0e7 47
ilaria 8:3afb6355c0e7 48 #define AX12_CW 1
ilaria 8:3afb6355c0e7 49 #define AX12_CCW 1
ilaria 8:3afb6355c0e7 50 using namespace std;
ilaria 8:3afb6355c0e7 51
clynamen 9:34f4eb572c47 52 // La prima lettera di una classe di solito ha la lettera Maiuscola -> Joint
ilaria 8:3afb6355c0e7 53 class joint
ilaria 8:3afb6355c0e7 54 {
ilaria 8:3afb6355c0e7 55 public:
clynamen 9:34f4eb572c47 56 // general non serve, le classi astratte pure non dovrebbero avere un costruttore (al costruttore ci pensa la classe child)
ilaria 8:3afb6355c0e7 57 virtual void general (PinName tx, PinName rx, int ID){};
clynamen 9:34f4eb572c47 58
ilaria 8:3afb6355c0e7 59 virtual void setID(int CurrentID, int NewID){};
ilaria 8:3afb6355c0e7 60 virtual void setMode(int mode) {};
ilaria 8:3afb6355c0e7 61 virtual void setCWLimit(float degrees) {};
ilaria 8:3afb6355c0e7 62 virtual void setCCWLimit(float degrees){};
clynamen 9:34f4eb572c47 63 // utilizziamo sempre lo stesso formato in una classe per i nomi dei metodi: lettera minuscola all'inizio
clynamen 9:34f4eb572c47 64 // goalPosition
ilaria 8:3afb6355c0e7 65 virtual void GoalPosition(float degrees){};
ilaria 8:3afb6355c0e7 66 virtual void SetSpeed(float goal_speed){};
clynamen 9:34f4eb572c47 67 // GetTemp serve a ottenere la temperature del motore... ma il metodo restituisce void
clynamen 9:34f4eb572c47 68 // deve restituire un float
ilaria 8:3afb6355c0e7 69 virtual void GetTemp(){};
clynamen 9:34f4eb572c47 70 // lo stesso per gli altri metodi get
ilaria 8:3afb6355c0e7 71 virtual void GetVolts(){};
ilaria 8:3afb6355c0e7 72 virtual void GetCurrent (){};
ilaria 8:3afb6355c0e7 73 virtual void GetPGain (){};
ilaria 8:3afb6355c0e7 74 virtual void GetIGain (){};
ilaria 8:3afb6355c0e7 75 virtual void GetDGain(){};
ilaria 8:3afb6355c0e7 76 virtual void SetMaxTorque (float torque){};
ilaria 8:3afb6355c0e7 77 virtual void MotorOnOff (){};
ilaria 8:3afb6355c0e7 78 virtual void trigger(){};
ilaria 8:3afb6355c0e7 79 virtual void ismoving(){};
clynamen 9:34f4eb572c47 80 }
ilaria 8:3afb6355c0e7 81
clynamen 9:34f4eb572c47 82 // Il codice seguente mischia dichiarazione e definizione. La dichiarazione (nomi, parametri) dovrebbe stare in un file .h
clynamen 9:34f4eb572c47 83 // l'implementazione in un file cpp
clynamen 9:34f4eb572c47 84
clynamen 9:34f4eb572c47 85
ilaria 8:3afb6355c0e7 86 class MX : public joint
clynamen 9:34f4eb572c47 87 {
clynamen 9:34f4eb572c47 88 public:
clynamen 9:34f4eb572c47 89 // Qui il costruttore e` giusto invece: MX non e` piu` una classe astratta, ma una normale classe parent
clynamen 9:34f4eb572c47 90 // Baudrate dovrebbe essere un parametro del costruttore
clynamen 9:34f4eb572c47 91 // La classe non deve prendere piu tx e rx, ma soltanto un communication
clynamen 9:34f4eb572c47 92 void general(PinName tx, PinName rx, int ID)
ilaria 8:3afb6355c0e7 93 : _mx(tx,rx) {
ilaria 8:3afb6355c0e7 94
ilaria 8:3afb6355c0e7 95 _mx.baud(9600);
ilaria 8:3afb6355c0e7 96 _ID = ID;
clynamen 9:34f4eb572c47 97 // Questi parametri devono essere letti da un file (preferibile) o inseriti nel codice.
clynamen 9:34f4eb572c47 98 // Digitarli dal terminale non e` pratico :)
ilaria 8:3afb6355c0e7 99 cout<<inserisci gear train: ;
ilaria 8:3afb6355c0e7 100 cin>>_gear_train;
ilaria 8:3afb6355c0e7 101 }
clynamen 9:34f4eb572c47 102
ilaria 8:3afb6355c0e7 103 void setID(int ID){
clynamen 9:34f4eb572c47 104
ilaria 8:3afb6355c0e7 105 char data[1];
ilaria 8:3afb6355c0e7 106 data[0] = NewID;
ilaria 8:3afb6355c0e7 107 if (MX_DEBUG) {
ilaria 8:3afb6355c0e7 108 printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
ilaria 8:3afb6355c0e7 109 }
ilaria 8:3afb6355c0e7 110 return (_line.write(CurrentID, MX_REG_ID, 1, data));
clynamen 9:34f4eb572c47 111 }
clynamen 9:34f4eb572c47 112
ilaria 8:3afb6355c0e7 113 void setMode(int mode){
ilaria 8:3afb6355c0e7 114 switch (mode){
ilaria 8:3afb6355c0e7 115 //Wheel Mode
ilaria 8:3afb6355c0e7 116 case (0):
ilaria 8:3afb6355c0e7 117 SetCWLimit(0);
ilaria 8:3afb6355c0e7 118 SetCCWLimit(0);
ilaria 8:3afb6355c0e7 119 SetCRSpeed(0.0);
ilaria 8:3afb6355c0e7 120 _mode = mode;
ilaria 8:3afb6355c0e7 121 break;
ilaria 8:3afb6355c0e7 122 //Joint Mode
ilaria 8:3afb6355c0e7 123 case (1):
ilaria 8:3afb6355c0e7 124 SetCWLimit(MX_RESOLUTION);
ilaria 8:3afb6355c0e7 125 SetCCWLimit(MX_RESOLUTION);
ilaria 8:3afb6355c0e7 126 SetCRSpeed(0.0);
ilaria 8:3afb6355c0e7 127 _mode = mode;
ilaria 8:3afb6355c0e7 128 break;
ilaria 8:3afb6355c0e7 129 //Multi-turn Mode
ilaria 8:3afb6355c0e7 130 case (2):
ilaria 8:3afb6355c0e7 131 SetCWLimit(360);
ilaria 8:3afb6355c0e7 132 SetCCWLimit(360);
ilaria 8:3afb6355c0e7 133 SetCRSpeed(0.0);
ilaria 8:3afb6355c0e7 134 _mode = mode;
ilaria 8:3afb6355c0e7 135 break;
ilaria 8:3afb6355c0e7 136 //other cases
ilaria 8:3afb6355c0e7 137 default:
ilaria 8:3afb6355c0e7 138 if(READ_DEBUG){
ilaria 8:3afb6355c0e7 139 printf("Not valid mode");
ilaria 8:3afb6355c0e7 140 }
ilaria 8:3afb6355c0e7 141 }
ilaria 8:3afb6355c0e7 142 return(0);
ilaria 8:3afb6355c0e7 143 }
ilaria 8:3afb6355c0e7 144 void setCWLimit(float degrees){
ilaria 8:3afb6355c0e7 145 char data[2];
ilaria 8:3afb6355c0e7 146
ilaria 8:3afb6355c0e7 147 short limit = (short)(MX_BIT_DEG * degrees * _gear_train);
ilaria 8:3afb6355c0e7 148
ilaria 8:3afb6355c0e7 149 data[0] = limit & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 150 data[1] = limit >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 151
ilaria 8:3afb6355c0e7 152 // write the packet, return the error code
ilaria 8:3afb6355c0e7 153 return(_line.write(_ID, MX_REG_CW_LIMIT, 2, data));
ilaria 8:3afb6355c0e7 154 }
ilaria 8:3afb6355c0e7 155 void setCCWLimit(float degrees){
ilaria 8:3afb6355c0e7 156 char data[2];
ilaria 8:3afb6355c0e7 157
ilaria 8:3afb6355c0e7 158 // 1023 / 300 * degrees
ilaria 8:3afb6355c0e7 159 short limit = (4093 * degrees) / 300;
ilaria 8:3afb6355c0e7 160
ilaria 8:3afb6355c0e7 161 if (MX_DEBUG) {
ilaria 8:3afb6355c0e7 162 printf("SetCCWLimit to 0x%x\n",limit);
ilaria 8:3afb6355c0e7 163 }
ilaria 8:3afb6355c0e7 164
ilaria 8:3afb6355c0e7 165 data[0] = limit & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 166 data[1] = limit >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 167
ilaria 8:3afb6355c0e7 168 // write the packet, return the error code
ilaria 8:3afb6355c0e7 169 return (_line.write(_ID, MX_REG_CCW_LIMIT, 2, data));
ilaria 8:3afb6355c0e7 170 }
ilaria 8:3afb6355c0e7 171
ilaria 8:3afb6355c0e7 172
ilaria 8:3afb6355c0e7 173 }
ilaria 8:3afb6355c0e7 174 void GoalPosition(float degrees){
ilaria 8:3afb6355c0e7 175 char data[2];
ilaria 8:3afb6355c0e7 176
ilaria 8:3afb6355c0e7 177 short gaol_position = (short)(MX_BIT_DEG * degrees * _gear_train);
ilaria 8:3afb6355c0e7 178
ilaria 8:3afb6355c0e7 179 data[0] = gaol_position & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 180 data[1] = gaol_position >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 181
ilaria 8:3afb6355c0e7 182 // write the packet, return the error code
ilaria 8:3afb6355c0e7 183 return(_line.write(_ID, MX_REG_GOAL_POSITION, 2, data));
ilaria 8:3afb6355c0e7 184 }
ilaria 8:3afb6355c0e7 185 void SetSpeed(float goal_speed){
ilaria 8:3afb6355c0e7 186 // bit 10 = direction, 0 = CCW, 1=CW
ilaria 8:3afb6355c0e7 187 // bits 9-0 = Speed
ilaria 8:3afb6355c0e7 188 char data[2];
ilaria 8:3afb6355c0e7 189
ilaria 8:3afb6355c0e7 190 int goal = (0x3ff * abs(speed * _gear_train));
ilaria 8:3afb6355c0e7 191
ilaria 8:3afb6355c0e7 192 // Set direction CW if we have a negative speed
ilaria 8:3afb6355c0e7 193 if (speed < 0) {
ilaria 8:3afb6355c0e7 194 goal |= (0x1 << 10);
ilaria 8:3afb6355c0e7 195 }
ilaria 8:3afb6355c0e7 196
ilaria 8:3afb6355c0e7 197 data[0] = goal & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 198 data[1] = goal >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 199
ilaria 8:3afb6355c0e7 200 // write the packet, return the error code
ilaria 8:3afb6355c0e7 201 return(_line.write(_ID, MX_REG_MOVING_SPEED, 2, data));
ilaria 8:3afb6355c0e7 202 }
ilaria 8:3afb6355c0e7 203 void GetTemp(){
ilaria 8:3afb6355c0e7 204 char data[1];
ilaria 8:3afb6355c0e7 205 int ErrorCode = _line.read(_ID, MX_REG_TEMP, 1, data);
ilaria 8:3afb6355c0e7 206 float temp = data[0];
ilaria 8:3afb6355c0e7 207 return(temp);
ilaria 8:3afb6355c0e7 208 }
ilaria 8:3afb6355c0e7 209 void GetVolts(){
ilaria 8:3afb6355c0e7 210 char data[1];
ilaria 8:3afb6355c0e7 211 int ErrorCode = _line.read(_ID, MX_REG_VOLTS, 1, data);
ilaria 8:3afb6355c0e7 212 float volts= data[1]/10.0;
ilaria 8:3afb6355c0e7 213 return volts;
ilaria 8:3afb6355c0e7 214
ilaria 8:3afb6355c0e7 215 }
ilaria 8:3afb6355c0e7 216 void GetCurrent(){
ilaria 8:3afb6355c0e7 217 char data[1];
ilaria 8:3afb6355c0e7 218 int ErrorCode = _line.read(_ID, MX_REG_AMPERE, 1, data);
ilaria 8:3afb6355c0e7 219 float ampere=(4.5)*(data[1]–2048);
ilaria 8:3afb6355c0e7 220 return ampere;//the result is expressed in mA
ilaria 8:3afb6355c0e7 221 }
ilaria 8:3afb6355c0e7 222 void GetPGain(){
ilaria 8:3afb6355c0e7 223
ilaria 8:3afb6355c0e7 224 char data[1];
ilaria 8:3afb6355c0e7 225 int ErrorCode = _line.read(_ID, MX_REG_PGAIN,1,data);
ilaria 8:3afb6355c0e7 226 float Kp= data[1]/8;
ilaria 8:3afb6355c0e7 227 return Kp;
ilaria 8:3afb6355c0e7 228 }
ilaria 8:3afb6355c0e7 229 void GetIGain(){
ilaria 8:3afb6355c0e7 230 char data[1];
ilaria 8:3afb6355c0e7 231 int ErrorCode = _line.read(_ID,MX_REG_IGAIN,1,data);
ilaria 8:3afb6355c0e7 232 float Ki=(data[1]*1000)/2048;
ilaria 8:3afb6355c0e7 233 return Ki;
ilaria 8:3afb6355c0e7 234 }
ilaria 8:3afb6355c0e7 235 void GetDGain(){
ilaria 8:3afb6355c0e7 236 char data[1];
ilaria 8:3afb6355c0e7 237 int ErrorCode = _line.read(_ID, MX_REG_DGAIN,1,data);
ilaria 8:3afb6355c0e7 238 float Kd=(data[1]*4)/1000;
ilaria 8:3afb6355c0e7 239 return Kd;
ilaria 8:3afb6355c0e7 240 }
ilaria 8:3afb6355c0e7 241 void SetMaxTorque (float torque){
ilaria 8:3afb6355c0e7 242 char data[2];
ilaria 8:3afb6355c0e7 243 float read_value=(torque * 1023); //what I have to read in register if I want a torque of #torque Nm
ilaria 8:3afb6355c0e7 244 data[0] = read_value & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 245 data[1] = read_value >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 246 int wrinting= _line.write(_ID, MX_REG_MAXTORQUE, 2, data);
ilaria 8:3afb6355c0e7 247 //BE CAREFULLY!!! if the power is turned on this torque is used as initial value
ilaria 8:3afb6355c0e7 248 //BE CAREFULLY N.2!! If the function of Alarm Shutdown is triggered, the motor loses its torque because the value becomes 0.
ilaria 8:3afb6355c0e7 249 //At this moment, if the value is changed to the value other than 0, the motor can be used again.
ilaria 8:3afb6355c0e7 250 }
ilaria 8:3afb6355c0e7 251 void MotorOnOff(){
ilaria 8:3afb6355c0e7 252 char data[1];
ilaria 8:3afb6355c0e7 253 int read_value=_line.read(_ID, MX_REG_MOTORONOFF,1,data);
ilaria 8:3afb6355c0e7 254 float result=data[1];
ilaria 8:3afb6355c0e7 255 return(result);
ilaria 8:3afb6355c0e7 256 }
ilaria 8:3afb6355c0e7 257 void trigger(){
ilaria 8:3afb6355c0e7 258 char TxBuf[16];
ilaria 8:3afb6355c0e7 259 char sum = 0;
ilaria 8:3afb6355c0e7 260
ilaria 8:3afb6355c0e7 261 if (MX_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 262 printf("\nTriggered\n");
ilaria 8:3afb6355c0e7 263 }
ilaria 8:3afb6355c0e7 264
ilaria 8:3afb6355c0e7 265 // Build the TxPacket first in RAM, then we'll send in one go
ilaria 8:3afb6355c0e7 266 if (MX_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 267 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
ilaria 8:3afb6355c0e7 268 }
ilaria 8:3afb6355c0e7 269
ilaria 8:3afb6355c0e7 270 TxBuf[0] = 0xFF;
ilaria 8:3afb6355c0e7 271 TxBuf[1] = 0xFF;
ilaria 8:3afb6355c0e7 272
ilaria 8:3afb6355c0e7 273 // ID - Broadcast
ilaria 8:3afb6355c0e7 274 TxBuf[2] = 0xFE;
ilaria 8:3afb6355c0e7 275 sum += TxBuf[2];
ilaria 8:3afb6355c0e7 276
ilaria 8:3afb6355c0e7 277 if (MX_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 278 printf(" ID : %d\n",TxBuf[2]);
ilaria 8:3afb6355c0e7 279 }
ilaria 8:3afb6355c0e7 280
ilaria 8:3afb6355c0e7 281 // Length
ilaria 8:3afb6355c0e7 282 TxBuf[3] = 0x02;
ilaria 8:3afb6355c0e7 283 sum += TxBuf[3];
ilaria 8:3afb6355c0e7 284 if (MX_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 285 printf(" Length %d\n",TxBuf[3]);
ilaria 8:3afb6355c0e7 286 }
ilaria 8:3afb6355c0e7 287
ilaria 8:3afb6355c0e7 288 // Instruction - ACTION
ilaria 8:3afb6355c0e7 289 TxBuf[4] = 0x04;
ilaria 8:3afb6355c0e7 290 sum += TxBuf[4];
ilaria 8:3afb6355c0e7 291 if (MX_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 292 printf(" Instruction 0x%X\n",TxBuf[5]);
ilaria 8:3afb6355c0e7 293 }
ilaria 8:3afb6355c0e7 294
ilaria 8:3afb6355c0e7 295 // Checksum
ilaria 8:3afb6355c0e7 296 TxBuf[5] = 0xFF - sum;
ilaria 8:3afb6355c0e7 297 if (MX_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 298 printf(" Checksum 0x%X\n",TxBuf[5]);
ilaria 8:3afb6355c0e7 299 }
ilaria 8:3afb6355c0e7 300
ilaria 8:3afb6355c0e7 301 // Transmit the packet in one burst with no pausing
ilaria 8:3afb6355c0e7 302 for (int i = 0; i < 6 ; i++) {
ilaria 8:3afb6355c0e7 303 _mx.putc(TxBuf[i]);
ilaria 8:3afb6355c0e7 304 }
ilaria 8:3afb6355c0e7 305
ilaria 8:3afb6355c0e7 306 // This is a broadcast packet, so there will be no reply
ilaria 8:3afb6355c0e7 307
ilaria 8:3afb6355c0e7 308 return(0);
ilaria 8:3afb6355c0e7 309 }
ilaria 8:3afb6355c0e7 310 void ismoving(){
ilaria 8:3afb6355c0e7 311
ilaria 8:3afb6355c0e7 312 char data[1];
ilaria 8:3afb6355c0e7 313 _line.read(_ID,MX_REG_MOVING,1,data);
ilaria 8:3afb6355c0e7 314 return(data[0]);
ilaria 8:3afb6355c0e7 315 }
ilaria 8:3afb6355c0e7 316 private:
ilaria 8:3afb6355c0e7 317 comunication_1 _line;
ilaria 8:3afb6355c0e7 318 int _ID;
ilaria 8:3afb6355c0e7 319 float _gear_train;
ilaria 8:3afb6355c0e7 320 SerialHalfDuplex _mx;
ilaria 8:3afb6355c0e7 321 }
ilaria 8:3afb6355c0e7 322 }
ilaria 8:3afb6355c0e7 323
ilaria 8:3afb6355c0e7 324 class AX:public joint{
ilaria 8:3afb6355c0e7 325 public:
ilaria 8:3afb6355c0e7 326 void general(PinName tx, PinName rx, int ID)
ilaria 8:3afb6355c0e7 327 : _ax12(tx,rx) {
ilaria 8:3afb6355c0e7 328 _ax12.baud(9600);
ilaria 8:3afb6355c0e7 329 _ID = ID;
ilaria 8:3afb6355c0e7 330 }
ilaria 8:3afb6355c0e7 331 void setID(int CurrentID, int NewID){
ilaria 8:3afb6355c0e7 332
ilaria 8:3afb6355c0e7 333 char data[1];
ilaria 8:3afb6355c0e7 334 data[0] = NewID;
ilaria 8:3afb6355c0e7 335 if (AX12_DEBUG) {
ilaria 8:3afb6355c0e7 336 printf("Setting ID from 0x%x to 0x%x\n",CurrentID,NewID);
ilaria 8:3afb6355c0e7 337 }
ilaria 8:3afb6355c0e7 338 return (_line.write(CurrentID, AX12_REG_ID, 1, data));
ilaria 8:3afb6355c0e7 339
ilaria 8:3afb6355c0e7 340 }
ilaria 8:3afb6355c0e7 341 void setMode(int mode) {
ilaria 8:3afb6355c0e7 342 if (mode == 1) { // set CR
ilaria 8:3afb6355c0e7 343 SetCWLimit(0);
ilaria 8:3afb6355c0e7 344 SetCCWLimit(0);
ilaria 8:3afb6355c0e7 345 SetCRSpeed(0.0);
ilaria 8:3afb6355c0e7 346 } else {
ilaria 8:3afb6355c0e7 347 SetCWLimit(0);
ilaria 8:3afb6355c0e7 348 SetCCWLimit(360);
ilaria 8:3afb6355c0e7 349 SetCRSpeed(0.0);
ilaria 8:3afb6355c0e7 350 }
ilaria 8:3afb6355c0e7 351 return(0);
ilaria 8:3afb6355c0e7 352 }
ilaria 8:3afb6355c0e7 353 void setCWLimit (int degrees) {
ilaria 8:3afb6355c0e7 354
ilaria 8:3afb6355c0e7 355 char data[2];
ilaria 8:3afb6355c0e7 356
ilaria 8:3afb6355c0e7 357 // 1023 / 300 * degrees
ilaria 8:3afb6355c0e7 358 short limit = (4093 * degrees) / 300;
ilaria 8:3afb6355c0e7 359
ilaria 8:3afb6355c0e7 360 if (AX12_DEBUG) {
ilaria 8:3afb6355c0e7 361 printf("SetCWLimit to 0x%x\n",limit);
ilaria 8:3afb6355c0e7 362 }
ilaria 8:3afb6355c0e7 363
ilaria 8:3afb6355c0e7 364 data[0] = limit & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 365 data[1] = limit >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 366
ilaria 8:3afb6355c0e7 367 // write the packet, return the error code
ilaria 8:3afb6355c0e7 368 return (_line.write(_ID, AX12_REG_CW_LIMIT, 2, data));
ilaria 8:3afb6355c0e7 369
ilaria 8:3afb6355c0e7 370 }
ilaria 8:3afb6355c0e7 371 void setCCWLimit(float degrees){
ilaria 8:3afb6355c0e7 372 char data[2];
ilaria 8:3afb6355c0e7 373
ilaria 8:3afb6355c0e7 374 // 1023 / 300 * degrees
ilaria 8:3afb6355c0e7 375 short limit = (4093 * degrees) / 300;
ilaria 8:3afb6355c0e7 376
ilaria 8:3afb6355c0e7 377 if (AX12_DEBUG) {
ilaria 8:3afb6355c0e7 378 printf("SetCCWLimit to 0x%x\n",limit);
ilaria 8:3afb6355c0e7 379 }
ilaria 8:3afb6355c0e7 380
ilaria 8:3afb6355c0e7 381 data[0] = limit & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 382 data[1] = limit >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 383
ilaria 8:3afb6355c0e7 384 // write the packet, return the error code
ilaria 8:3afb6355c0e7 385 return (_line.write(_ID, AX12_REG_CCW_LIMIT, 2, data));
ilaria 8:3afb6355c0e7 386 }
ilaria 8:3afb6355c0e7 387 void GoalPosition(float degrees){
ilaria 8:3afb6355c0e7 388 // if flag[0] is set, were blocking
ilaria 8:3afb6355c0e7 389 // if flag[1] is set, we're registering
ilaria 8:3afb6355c0e7 390 // they are mutually exclusive operations
ilaria 8:3afb6355c0e7 391 int flag;
ilaria 8:3afb6355c0e7 392 cout<<Inserisci flag: ;
ilaria 8:3afb6355c0e7 393 cin<<flag;
ilaria 8:3afb6355c0e7 394 int reg_flag=0;
ilaria 8:3afb6355c0e7 395 if (flags == 0x2) {
ilaria 8:3afb6355c0e7 396 reg_flag = 1;
ilaria 8:3afb6355c0e7 397 }
ilaria 8:3afb6355c0e7 398
ilaria 8:3afb6355c0e7 399 // 1023 / 300 * degrees
ilaria 8:3afb6355c0e7 400 short goal = (4093 * degrees) / 300;
ilaria 8:3afb6355c0e7 401 if (AX12_DEBUG) {
ilaria 8:3afb6355c0e7 402 printf("SetGoal to 0x%x\n",goal);
ilaria 8:3afb6355c0e7 403 }
ilaria 8:3afb6355c0e7 404
ilaria 8:3afb6355c0e7 405 data[0] = goal & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 406 data[1] = goal >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 407
ilaria 8:3afb6355c0e7 408 // write the packet, return the error code
ilaria 8:3afb6355c0e7 409 int rVal = _line.write(_ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
ilaria 8:3afb6355c0e7 410
ilaria 8:3afb6355c0e7 411 if (flags == 1) {
ilaria 8:3afb6355c0e7 412 // block until it comes to a halt
ilaria 8:3afb6355c0e7 413 while (isMoving()) {}
ilaria 8:3afb6355c0e7 414 }
ilaria 8:3afb6355c0e7 415 return(rVal);
ilaria 8:3afb6355c0e7 416 }
ilaria 8:3afb6355c0e7 417 void SetSpeed(float goal_speed){
ilaria 8:3afb6355c0e7 418 // bit 10 = direction, 0 = CCW, 1=CW
ilaria 8:3afb6355c0e7 419 // bits 9-0 = Speed
ilaria 8:3afb6355c0e7 420 char data[2];
ilaria 8:3afb6355c0e7 421
ilaria 8:3afb6355c0e7 422 int goal = (0x3ff * abs(goal_speed));
ilaria 8:3afb6355c0e7 423
ilaria 8:3afb6355c0e7 424 // Set direction CW if we have a negative speed
ilaria 8:3afb6355c0e7 425 if (goal_speed < 0) {
ilaria 8:3afb6355c0e7 426 goal |= (0x1 << 10);
ilaria 8:3afb6355c0e7 427 }
ilaria 8:3afb6355c0e7 428
ilaria 8:3afb6355c0e7 429 data[0] = goal & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 430 data[1] = goal >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 431
ilaria 8:3afb6355c0e7 432 // write the packet, return the error code
ilaria 8:3afb6355c0e7 433 int rVal = _line.write(_ID, AX12_REG_MOVING_SPEED, 2, data);
ilaria 8:3afb6355c0e7 434
ilaria 8:3afb6355c0e7 435 return(rVal);
ilaria 8:3afb6355c0e7 436 }
ilaria 8:3afb6355c0e7 437 void GetTemp(){
ilaria 8:3afb6355c0e7 438 if (AX12_DEBUG) {
ilaria 8:3afb6355c0e7 439 printf("\nGetTemp(%d)",_ID);
ilaria 8:3afb6355c0e7 440 }
ilaria 8:3afb6355c0e7 441 char data[1];
ilaria 8:3afb6355c0e7 442 int ErrorCode = _line.read(_ID, AX12_REG_TEMP, 1, data);
ilaria 8:3afb6355c0e7 443 float temp = data[0];
ilaria 8:3afb6355c0e7 444 return(temp);
ilaria 8:3afb6355c0e7 445 }
ilaria 8:3afb6355c0e7 446 void GetVolts(){
ilaria 8:3afb6355c0e7 447 if (AX12_DEBUG) {
ilaria 8:3afb6355c0e7 448 printf("\nGetVolts(%d)",_ID);
ilaria 8:3afb6355c0e7 449 }
ilaria 8:3afb6355c0e7 450 char data[1];
ilaria 8:3afb6355c0e7 451 int ErrorCode = _line.read(_ID, AX12_REG_VOLTS, 1, data);
ilaria 8:3afb6355c0e7 452 float volts = data[0]/10.0;
ilaria 8:3afb6355c0e7 453 return(volts);
ilaria 8:3afb6355c0e7 454 }
ilaria 8:3afb6355c0e7 455 void GetCurrent(){
ilaria 8:3afb6355c0e7 456 cout<<Errore!In questo tipo di motore non e possibile rilevare la corrente;
ilaria 8:3afb6355c0e7 457 return(1);
ilaria 8:3afb6355c0e7 458 }
ilaria 8:3afb6355c0e7 459 void GetPGain(){
ilaria 8:3afb6355c0e7 460 cout<<Errore!Questo tipo di motore non può avere un guadagno di questo tipo;
ilaria 8:3afb6355c0e7 461 return(1);
ilaria 8:3afb6355c0e7 462 }
ilaria 8:3afb6355c0e7 463 void GetIGain(){
ilaria 8:3afb6355c0e7 464 cout<<Errore!Questo tipo di motore non può avere un guadagno di questo tipo;
ilaria 8:3afb6355c0e7 465 return(1);
ilaria 8:3afb6355c0e7 466 }
ilaria 8:3afb6355c0e7 467 void GetDGain(){
ilaria 8:3afb6355c0e7 468 cout<<Errore!Questo tipo di motore non può avere un guadagno di questo tipo;
ilaria 8:3afb6355c0e7 469 return(1);
ilaria 8:3afb6355c0e7 470 }
ilaria 8:3afb6355c0e7 471 void SetMaxTorque (float torque){
ilaria 8:3afb6355c0e7 472 char data[2];
ilaria 8:3afb6355c0e7 473 float read_value=(torque * 1023); //what I have to read in register if I want a torque of #torque Nm
ilaria 8:3afb6355c0e7 474 data[0] = read_value & 0xff; // bottom 8 bits
ilaria 8:3afb6355c0e7 475 data[1] = read_value >> 8; // top 8 bits
ilaria 8:3afb6355c0e7 476 int wrinting= _line.write(_ID, AX12_REG_MAXTORQUE, 2, data);
ilaria 8:3afb6355c0e7 477 //BE CAREFULLY!!! if the power is turned on this torque is used as initial value
ilaria 8:3afb6355c0e7 478 //BE CAREFULLY N.2!! If the function of Alarm Shutdown is triggered, the motor loses its torque because the value becomes 0.
ilaria 8:3afb6355c0e7 479 //At this moment, if the value is changed to the value other than 0, the motor can be used again.
ilaria 8:3afb6355c0e7 480 }
ilaria 8:3afb6355c0e7 481 void MotorOnOff (){
ilaria 8:3afb6355c0e7 482 char data[1];
ilaria 8:3afb6355c0e7 483 int read_value=_line.read(_ID, AX12_REG_MOTORONOFF,1,data);
ilaria 8:3afb6355c0e7 484 float result=data[1];
ilaria 8:3afb6355c0e7 485 return(result);
ilaria 8:3afb6355c0e7 486 }
ilaria 8:3afb6355c0e7 487 void trigger(){
ilaria 8:3afb6355c0e7 488 char TxBuf[16];
ilaria 8:3afb6355c0e7 489 char sum = 0;
ilaria 8:3afb6355c0e7 490
ilaria 8:3afb6355c0e7 491 if (AX12_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 492 printf("\nTriggered\n");
ilaria 8:3afb6355c0e7 493 }
ilaria 8:3afb6355c0e7 494
ilaria 8:3afb6355c0e7 495 // Build the TxPacket first in RAM, then we'll send in one go
ilaria 8:3afb6355c0e7 496 if (AX12_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 497 printf("\nTrigger Packet\n Header : 0xFF, 0xFF\n");
ilaria 8:3afb6355c0e7 498 }
ilaria 8:3afb6355c0e7 499
ilaria 8:3afb6355c0e7 500 TxBuf[0] = 0xFF;
ilaria 8:3afb6355c0e7 501 TxBuf[1] = 0xFF;
ilaria 8:3afb6355c0e7 502
ilaria 8:3afb6355c0e7 503 // ID - Broadcast
ilaria 8:3afb6355c0e7 504 TxBuf[2] = 0xFE;
ilaria 8:3afb6355c0e7 505 sum += TxBuf[2];
ilaria 8:3afb6355c0e7 506
ilaria 8:3afb6355c0e7 507 if (AX12_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 508 printf(" ID : %d\n",TxBuf[2]);
ilaria 8:3afb6355c0e7 509 }
ilaria 8:3afb6355c0e7 510
ilaria 8:3afb6355c0e7 511 // Length
ilaria 8:3afb6355c0e7 512 TxBuf[3] = 0x02;
ilaria 8:3afb6355c0e7 513 sum += TxBuf[3];
ilaria 8:3afb6355c0e7 514 if (AX12_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 515 printf(" Length %d\n",TxBuf[3]);
ilaria 8:3afb6355c0e7 516 }
ilaria 8:3afb6355c0e7 517
ilaria 8:3afb6355c0e7 518 // Instruction - ACTION
ilaria 8:3afb6355c0e7 519 TxBuf[4] = 0x04;
ilaria 8:3afb6355c0e7 520 sum += TxBuf[4];
ilaria 8:3afb6355c0e7 521 if (AX12_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 522 printf(" Instruction 0x%X\n",TxBuf[5]);
ilaria 8:3afb6355c0e7 523 }
ilaria 8:3afb6355c0e7 524
ilaria 8:3afb6355c0e7 525 // Checksum
ilaria 8:3afb6355c0e7 526 TxBuf[5] = 0xFF - sum;
ilaria 8:3afb6355c0e7 527 if (AX12_TRIGGER_DEBUG) {
ilaria 8:3afb6355c0e7 528 printf(" Checksum 0x%X\n",TxBuf[5]);
ilaria 8:3afb6355c0e7 529 }
ilaria 8:3afb6355c0e7 530
ilaria 8:3afb6355c0e7 531 // Transmit the packet in one burst with no pausing
ilaria 8:3afb6355c0e7 532 for (int i = 0; i < 6 ; i++) {
ilaria 8:3afb6355c0e7 533 _ax12.putc(TxBuf[i]);
ilaria 8:3afb6355c0e7 534 }
ilaria 8:3afb6355c0e7 535
ilaria 8:3afb6355c0e7 536 // This is a broadcast packet, so there will be no reply
ilaria 8:3afb6355c0e7 537
ilaria 8:3afb6355c0e7 538 return;
ilaria 8:3afb6355c0e7 539 }
ilaria 8:3afb6355c0e7 540 void ismoving(){
ilaria 8:3afb6355c0e7 541 char data[1];
ilaria 8:3afb6355c0e7 542 read(_ID,AX12_REG_MOVING,1,data);
ilaria 8:3afb6355c0e7 543 return(data[0]);
ilaria 8:3afb6355c0e7 544 }
ilaria 8:3afb6355c0e7 545
ilaria 8:3afb6355c0e7 546
ilaria 8:3afb6355c0e7 547 private:
ilaria 8:3afb6355c0e7 548 communication_1 _line;
ilaria 8:3afb6355c0e7 549 int _ID;
ilaria 8:3afb6355c0e7 550 SerialHalfDuplex _ax12;
clynamen 9:34f4eb572c47 551 }
ilaria 8:3afb6355c0e7 552
clynamen 9:34f4eb572c47 553
clynamen 9:34f4eb572c47 554 // Stessa cosa: la classe stepper deve avere due files: .h e .cpp
ilaria 8:3afb6355c0e7 555 class stepper:public joint{
ilaria 8:3afb6355c0e7 556 public:
ilaria 8:3afb6355c0e7 557 void GetPGain(){
ilaria 8:3afb6355c0e7 558 cout<<Errore!Il motore stepper non può avere un guadagno di questo tipo;
ilaria 8:3afb6355c0e7 559 return(1);
ilaria 8:3afb6355c0e7 560 }
ilaria 8:3afb6355c0e7 561 void GetIGain(){
ilaria 8:3afb6355c0e7 562 cout<<Errore!Il motore stepper non può avere un guadagno di questo tipo;
ilaria 8:3afb6355c0e7 563 return(1);
ilaria 8:3afb6355c0e7 564 }
ilaria 8:3afb6355c0e7 565 void GetDGain(){
ilaria 8:3afb6355c0e7 566 cout<<Errore!Il motore stepper non può avere un guadagno di questo tipo;
ilaria 8:3afb6355c0e7 567 return(1);
ilaria 8:3afb6355c0e7 568 }
ilaria 8:3afb6355c0e7 569
ilaria 8:3afb6355c0e7 570