PRISMA Lab
mbedos senza corrente
Diff: MX.cpp
- Revision:
- 22:5d3f37356915
- Parent:
- 21:fe5dd48bebc6
--- 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++){