PRISMA Lab
/
MX64_senzaCorrente
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.
Revision 22:5d3f37356915, committed 2019-11-05
- Comitter:
- marcodesilva
- Date:
- Tue Nov 05 14:26:34 2019 +0000
- Parent:
- 21:fe5dd48bebc6
- Commit message:
- con Corrente e Settings Profili di Acc e Vel
Changed in this revision
| MX.cpp | Show annotated file Show diff for this revision Revisions of this file |
| MX.h | Show annotated file Show diff for this revision Revisions of this file |
--- a/MX.cpp Tue Nov 05 08:57:38 2019 +0000
+++ b/MX.cpp Tue Nov 05 14:26:34 2019 +0000
@@ -320,6 +320,84 @@
}
+
+int MX::SyncProfileAccel(float profileValAcc[]) {
+
+ char data[4*_Nmotor]; //4 is dimension in bytes of instruction
+ int goal[_Nmotor];
+
+ for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float)
+ goal[i] = (profileValAcc[i]);
+ }
+
+ for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255
+ for (int j = 0 ; j < 4; j++){
+ data[j+(i*4)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits
+ }
+ }
+
+ #if SYNC_SETGOAL_DEBUG
+ printf("\n##########################################");
+ printf("\n SYNC SET PROFILE ACCELERATION \n ");
+ printf("\nACCELLERATION CHOSEN FOR EACH MOTOR\n");
+ printf("Acceleration[ID]: value(0-32767) \n");
+ for (int i=0; i<_Nmotor ; i++){
+ printf("goal[%d] : %d ",_broadcastID[i],goal[i]);
+ }
+ printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
+ for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8)
+ for (int j = 0 ; j < 4; j++){
+ printf("data[%d]: %02x\n",j+(i*4),data[j+(i*4)]); //debug data
+ }
+ printf("\n");
+ }
+ #endif
+
+ // write the packet, and return the error code
+ int rVal = SyncSendPacket( MX_REG_PROFILE_ACCELER , 4, data);
+
+ return(rVal);
+}
+
+
+int MX::SyncProfileVel(float profileValueVel[]) {
+
+ char data[4*_Nmotor]; //4 is dimension in bytes of instruction
+ int goal[_Nmotor];
+
+ for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float)
+ goal[i] = (profileValueVel[i]);
+ }
+
+ for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255
+ for (int j = 0 ; j < 4; j++){
+ data[j+(i*4)] = (goal[i] >> (j*8) ) & 0xff; // top 8 bits
+ }
+ }
+
+ #if SYNC_SETGOAL_DEBUG
+ printf("\n##########################################");
+ printf("\n SYNC SET PROFILE VELOCITY \n ");
+ printf("\nVELOCITY CHOSEN FOR EACH MOTOR\n");
+ printf("Velocity[ID]: value(0-44) \n");
+ for (int i=0; i<_Nmotor ; i++){
+ printf("goal[%d] : %d ",_broadcastID[i],goal[i]);
+ }
+ printf("\nDATA TO SET FOR EACH MOTOR (entire buffer RAW values) \n");
+ for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8)
+ for (int j = 0 ; j < 4; j++){
+ printf("data[%d]: %02x\n",j+(i*4),data[j+(i*4)]); //debug data
+ }
+ printf("\n");
+ }
+ #endif
+
+ // write the packet, and return the error code
+ int rVal = SyncSendPacket( MX_REG_PROFILE_VELOCITY , 4, data);
+
+ return(rVal);
+}
+
void MX::SyncGetPosition(float* angle) {
int bytes = 4;
int NumberOfMotor = _Nmotor;
@@ -327,7 +405,6 @@
char data[(11+bytes)*NumberOfMotor];
int32_t position[NumberOfMotor];
- int IDreceived[NumberOfMotor];
float ScaleFactor = (float)360/4095;
@@ -335,23 +412,21 @@
for (int i=0 ;i<_Nmotor ;i++){
-
- IDreceived[i] = (int)data[4 +(11+bytes)*i];
-
position[i] = (int)data[12+(11+bytes)*i] << 24;
position[i] |= (int)data[11+(11+bytes)*i] << 16;
position[i] |= (int)data[10+(11+bytes)*i] << 8;
position[i] |= (int)data[9 +(11+bytes)*i];
}
+
for (int i=0 ;i<_Nmotor ;i++){
- // angle(degree) obtained from position(0 - 4095)
- position[i] = position[i]-2048;
- angle[i] = (float)position[i]*ScaleFactor;
+ // angle(degree) obtained from position(0 - 4095)
+ position[i] = position[i]-2048;
+ angle[i] = (float)position[i]*ScaleFactor;
}
#if SYNC_GET_POSITION_DEBUG
for (int i=0 ;i<_Nmotor ;i++){
- printf("\nGet RAW position data from ID: %d\n",IDreceived[i]);
+ printf("\nGet RAW current data from ID: %d\n",_broadcastID[i]);
printf(" Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]);
printf(" Data[%d]: 0x%02x\n",(10 +(11+bytes)*i),data[10 +(11+bytes)*i]);
printf(" Data[%d]: 0x%02x\n",(11 +(11+bytes)*i),data[11 +(11+bytes)*i]);
@@ -366,43 +441,46 @@
void MX::SyncGetCurrent(float* presentCurrent) {
- int bytes = 2;
+ printf("\n##########################################");
+ printf("\n SYNC GET CURRENT ");
+
+ int bytes = 4;
char data[(11+bytes)*_Nmotor];
+ int32_t current[bytes];
- int32_t current[bytes];
- int NumberOfMotor = sizeof(_Nmotor);
- int IDreceived[NumberOfMotor];
//float presentCurrent[NumberOfMotor] ;
float scaleFactor = (float)3.36; // 3.36mA is a unit for scale from 0 to 1941 in DEC
//char Status[(11+bytes)*_Nmotor]; (11+bytes)
SyncReadPacket(MX_REG_PRESENT_CURRENT, bytes, data);
+
for (int i=0 ;i<_Nmotor ;i++){
-
- IDreceived[i] = (int)data[4 +(11+bytes)*i];
-
- current[i] = (int)data[10+(11+bytes)*i] << 8;
+ current[i] = (int)data[12+(11+bytes)*i] << 24;
+ current[i] |= (int)data[11+(11+bytes)*i] << 16;
+ current[i] |= (int)data[10+(11+bytes)*i] << 8;
current[i] |= (int)data[9 +(11+bytes)*i];
-
-
-
}
+
for (int i=0 ;i<_Nmotor ;i++){
+ if ((int)data[10+(11+bytes)*i] == 255){
+ current[i] = current[i] - (float)65535;
+ }
// PresentCurrent (mA) obtained from current (0 - 1941)
presentCurrent[i] = (float)current[i]*scaleFactor;
}
#if SYNC_GET_CURRENT_DEBUG
for (int i=0 ;i<_Nmotor ;i++){
- printf("\nGet RAW current data from ID: %d\n",IDreceived[i]);
+ printf("\nGet RAW current data from ID: %d\n",_broadcastID[i]);
printf(" Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]);
printf(" Data[%d]: 0x%02x\n",(10 +(11+bytes)*i),data[10 +(11+bytes)*i]);
+ printf(" Data[%d]: 0x%02x\n",(11 +(11+bytes)*i),data[11 +(11+bytes)*i]);
+ printf(" Data[%d]: 0x%02x\n",(12 +(11+bytes)*i),data[12 +(11+bytes)*i]);
printf("Converted Current (0 - 1941): %d\n",current[i]);
- printf("Converted Present Current %f\n\n",presentCurrent[i]);
+ printf("Converted Present Current %f mA\n\n",presentCurrent[i]);
}
#endif
-
}
@@ -470,11 +548,11 @@
#endif
TxBuf[t] = ID;
}else{
+ #if SYNC_SENDPACKET_DEBUG
+ printf("\n\nMULTI MOTOR ");
+ printf("\nID: %d", _broadcastID[i]);
+ #endif
TxBuf[t] = _broadcastID[i];
- #if SYNC_SENDPACKET_DEBUG
- printf("\n\nMULTI MOTOR");
- printf("\nbroadcastID: %d", _broadcastID[i]);
- #endif
}
for (int j = 0; j < bytes ; j++){
--- a/MX.h Tue Nov 05 08:57:38 2019 +0000
+++ b/MX.h Tue Nov 05 14:26:34 2019 +0000
@@ -42,16 +42,16 @@
//#define OPERATING_MODE_DEBUG 1
//#define SETBAUD_DEBUG 1
-//#define SYNC_TORQUE_ENABLE_DEBUG 1
-//#define SYNC_SETGOAL_DEBUG 1
+#define SYNC_TORQUE_ENABLE_DEBUG 0
+#define SYNC_SETGOAL_DEBUG 0
#define SYNC_GET_POSITION_DEBUG 0
-//#define SYNC_SENDPACKET_DEBUG 1
-//#define SYNC_SENDPACKET_DEBUG_PACKETONLY 1
-//#define SYNC_READPACKET_DEBUG_PACKETONLY 0
-//#define SYNC_SET_BAUD_DEBUG 1
-//#define SYNC_GET_CURRENT_DEBUG 0
-//#define SYNC_GOAL_CURRENT_DEBUG 1
-//#define SYNC_OPERATING_MODE_DEBUG 1
+#define SYNC_SENDPACKET_DEBUG 0
+#define SYNC_SENDPACKET_DEBUG_PACKETONLY 0
+#define SYNC_READPACKET_DEBUG_PACKETONLY 0
+#define SYNC_SET_BAUD_DEBUG 0
+#define SYNC_GET_CURRENT_DEBUG 0
+#define SYNC_GOAL_CURRENT_DEBUG 0
+#define SYNC_OPERATING_MODE_DEBUG 0
// Protocol 2.0
// N.B. all Register values are in HEX, on the on-line manual all are in DEC
@@ -71,6 +71,8 @@
#define MX_REG_GOAL_CURRENT 0x66
#define MX_REG_CURRENT_LIMIT 0x26
#define MX_REG_GOAL_PWM 0x64
+#define MX_REG_PROFILE_ACCELER 0x6C
+#define MX_REG_PROFILE_VELOCITY 0x70
#define MX_OFF 0
#define MX_ON 1
@@ -214,9 +216,11 @@
int SyncGoalCurrent(float mAmpere[]);
int SyncCurrentLimit(float mAmpere, int ID);
int SyncCurrentLimit(float mAmpere[]);
- int SyncOperatingMode(int mode[], int ID);
- int SyncSetBaud(int MotorBaud[], int ID);
- int SyncGoalPWM(float values[], int ID);
+ int SyncOperatingMode(int mode[], int ID =-1);
+ int SyncSetBaud(int MotorBaud[], int ID =-1 );
+ int SyncGoalPWM(float values[], int ID =-1 );
+ int SyncProfileAccel(float profileValAcc[]);
+ int SyncProfileVel(float profileValueVel[]);
private :