PRISMA Lab
mbedos senza corrente
Diff: MX.cpp
- Revision:
- 21:fe5dd48bebc6
- Parent:
- 20:029e0fe30b0a
- Child:
- 22:5d3f37356915
--- a/MX.cpp Tue Oct 22 16:42:40 2019 +0000
+++ b/MX.cpp Tue Nov 05 08:57:38 2019 +0000
@@ -21,7 +21,7 @@
data[i] = enableVal[i];
}
- #ifdef SYNC_TORQUE_ENABLE_DEBUG
+ #if SYNC_TORQUE_ENABLE_DEBUG
printf("\n SYNC TORQUE ENABLE\n");
for (int i=0 ; i<Nmotor ; i++){
printf("\n Torque Enable value for ID[%d] = %d",_broadcastID[i], enableVal[i]);
@@ -43,7 +43,7 @@
data[i] = MotorBaud[i];
}
- #ifdef SYNC_SET_BAUD_DEBUG
+ #if SYNC_SET_BAUD_DEBUG
printf("\n SYNC_SET_BAUD_DEBUG\n");
for (int i=0 ; i<Nmotor ; i++){
printf("Set Baud rate value to: 0x%02x\n",MotorBaud[i]);
@@ -71,13 +71,15 @@
int MX::SyncSetGoal(float degrees, int ID) {
float goal = (degrees) * (float)(4095/360);
+
+ goal = goal+2048;
char data[4];
for (int j = 0 ; j < 4 ; j++){
data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits
}
- #ifdef SYNC_SETGOAL_DEBUG
+ #if SYNC_SETGOAL_DEBUG
printf("\n SYNC SET GOAL\n ");
printf("\nGOAL CHOSEN FOR EACH MOTOR\n");
printf("goal[ID]: value(0-4095) - value(0 - 360)\n");
@@ -99,7 +101,9 @@
int goal[_Nmotor];
for (int i=0 ; i<_Nmotor ; i++){ //set goal array with goal in RAW(uint32) values from DEGREES(float)
- goal[i] = (degrees[i]) * (float)(4095/360);
+ goal[i] = (degrees[i]) * (float)(4095/360);
+ goal[i] = goal[i]+2048;
+
}
for (int i=0 ; i<_Nmotor ; i++){ //set data array in 4 bytes sequence (split RAW(uint32) in 4x bytes(uint8) 0-255
@@ -108,7 +112,7 @@
}
}
- #ifdef SYNC_SETGOAL_DEBUG
+ #if SYNC_SETGOAL_DEBUG
printf("\n SYNC SET GOAL\n ");
printf("\nGOAL CHOSEN FOR EACH MOTOR\n");
printf("goal[ID]: value(0-4095) - value(0 - 360)\n");
@@ -140,7 +144,7 @@
data[i] = mode[i];
}
- #ifdef SYNC_OPERATING_MODE_DEBUG
+ #if SYNC_OPERATING_MODE_DEBUG
printf("\n SYNC OPERATING MODE DEBUG\n");
for (int i=0 ; i<Nmotor ; i++){
printf("Set Operating Mode value to: 0x%02x\n",mode[i]);
@@ -172,7 +176,7 @@
data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits
}
- #ifdef SYNC_CURRENT_LIMIT_DEBUG
+ #if SYNC_CURRENT_LIMIT_DEBUG
printf("\n SYNC CURRENT LIMIT DEBUG\n ");
printf("\nCURRENT LIMIT CHOSEN FOR EACH MOTOR\n");
printf("Current limit[ID]: value(0-1941) - value(0mA - 6521.76mA)\n");
@@ -205,7 +209,7 @@
}
}
- #ifdef SYNC_CURRENT_LIMIT_DEBUG
+ #if SYNC_CURRENT_LIMIT_DEBUG
printf("\n SYNC CURRENT LIMIT DEBUG\n ");
printf("\nCURRENT LIMIT CHOSEN FOR EACH MOTOR\n");
printf("Current limit[ID]: value(0-1941) - value(0mA - 6521.76mA)\n");
@@ -238,7 +242,7 @@
data[j] = ((int)goal >> (j*8) ) & 0xff; // top 8 bits
}
- #ifdef SYNC_GOAL_CURRENT_DEBUG
+ #if SYNC_GOAL_CURRENT_DEBUG
printf("\n SYNC GOAL CURRENT\n ");
printf("\nGOAL CHOSEN FOR EACH MOTOR\n");
printf("goal[ID]: value(-1941-1941) - value(-6521.76mA - 6521.76mA)\n");
@@ -271,7 +275,7 @@
}
}
- #ifdef SYNC_GOAL_CURRENT_DEBUG
+ #if SYNC_GOAL_CURRENT_DEBUG
printf("\n SYNC GOAL CURRENT\n ");
printf("\nGOAL CHOSEN FOR EACH MOTOR\n");
printf("goal[ID]: value(0-1941) - value(-6521.76mA - 6521.76mA)\n");
@@ -316,19 +320,20 @@
}
-float* MX::SyncGetPosition(void) {
- printf("\n SYNC GET POSITION ");
- char* data;
+void MX::SyncGetPosition(float* angle) {
int bytes = 4;
- int32_t position[bytes];
- int NumberOfMotor = sizeof(_Nmotor);
+ int NumberOfMotor = _Nmotor;
+
+ char data[(11+bytes)*NumberOfMotor];
+
+ int32_t position[NumberOfMotor];
int IDreceived[NumberOfMotor];
- float angle[NumberOfMotor] ;
float ScaleFactor = (float)360/4095;
-
- //char Status[(11+bytes)*_Nmotor]; (11+bytes)
- data = SyncReadPacket(MX_REG_PRESENT_POSITION, bytes);
+
+ SyncReadPacket(MX_REG_PRESENT_POSITION, bytes,data);
+
+
for (int i=0 ;i<_Nmotor ;i++){
IDreceived[i] = (int)data[4 +(11+bytes)*i];
@@ -340,10 +345,11 @@
}
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;
}
-#ifdef SYNC_GET_POSITION_DEBUG
+#if SYNC_GET_POSITION_DEBUG
for (int i=0 ;i<_Nmotor ;i++){
printf("\nGet RAW position data from ID: %d\n",IDreceived[i]);
printf(" Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]);
@@ -355,22 +361,23 @@
}
#endif
- return(angle);
+ //return angle;
}
-float* MX::SyncGetCurrent(void) {
- printf("\n SYNC GET CURRENT ");
- char* data;
+void MX::SyncGetCurrent(float* presentCurrent) {
+
int bytes = 2;
+ char data[(11+bytes)*_Nmotor];
+
int32_t current[bytes];
int NumberOfMotor = sizeof(_Nmotor);
int IDreceived[NumberOfMotor];
- float presentCurrent[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)
- data = SyncReadPacket(MX_REG_PRESENT_CURRENT, bytes);
+ SyncReadPacket(MX_REG_PRESENT_CURRENT, bytes, data);
for (int i=0 ;i<_Nmotor ;i++){
IDreceived[i] = (int)data[4 +(11+bytes)*i];
@@ -386,7 +393,7 @@
presentCurrent[i] = (float)current[i]*scaleFactor;
}
-#ifdef SYNC_GET_CURRENT_DEBUG
+#if SYNC_GET_CURRENT_DEBUG
for (int i=0 ;i<_Nmotor ;i++){
printf("\nGet RAW current data from ID: %d\n",IDreceived[i]);
printf(" Data[%d]: 0x%02x\n",(9 +(11+bytes)*i),data[9 +(11+bytes)*i]);
@@ -396,7 +403,6 @@
}
#endif
- return(presentCurrent);
}
@@ -441,7 +447,7 @@
// data
// Motor parameters for each motors
- #ifdef SYNC_SENDPACKET_DEBUG
+ #if SYNC_SENDPACKET_DEBUG
printf("\nDEBUG DI SyncSendPacket");
printf("\nTxBuf length : %d\n",TxBufSize );
printf("\nTxBuf before data and CRC adding:\n ");
@@ -459,13 +465,13 @@
int t = 12 +((bytes+1)*i); // (bytes+1) => instruction dim + 1 (for ID) * i (for each motor)
if (Nmotor == 1){
- #ifdef SYNC_SENDPACKET_DEBUG
+ #if SYNC_SENDPACKET_DEBUG
printf("\nSINGLE MOTOR ");
#endif
TxBuf[t] = ID;
}else{
TxBuf[t] = _broadcastID[i];
- #ifdef SYNC_SENDPACKET_DEBUG
+ #if SYNC_SENDPACKET_DEBUG
printf("\n\nMULTI MOTOR");
printf("\nbroadcastID: %d", _broadcastID[i]);
#endif
@@ -474,7 +480,7 @@
for (int j = 0; j < bytes ; j++){
//Little endian
TxBuf[t+j+1] = data[j+(i*(bytes))]; //(first byte is a low order byte, second byte are High order byte)
- #ifdef SYNC_SENDPACKET_DEBUG
+ #if SYNC_SENDPACKET_DEBUG
printf("\nTxBuf[%d] = data[%d] = %x ",t+j+1,j+(i*bytes),data[j+(i*bytes)]);
#endif
}
@@ -488,7 +494,7 @@
TxBuf[TxBufSize-1] = (crc16>>8) & 0x00FF;
//printf("\nCRC 1 : %02x CRC 1 : %02x ",TxBuf[14+((bytes)*Nmotor)],TxBuf[15+((bytes)*Nmotor)]);
- #ifdef SYNC_SENDPACKET_DEBUG_PACKETONLY
+ #if SYNC_SENDPACKET_DEBUG_PACKETONLY
printf("\n\n Complete Packet to send of %d elements\n", TxBufSize);
for (int i=0; i<TxBufSize ; i++){
printf("\nTxBuf[%d] = %02x ",i,TxBuf[i]);
@@ -508,12 +514,12 @@
}
-char* MX::SyncReadPacket(int start, int bytes) {
+void MX::SyncReadPacket(int start, int bytes, char* data) {
char TxBuf[12+(_Nmotor)+2];
char Status[(11+bytes)*_Nmotor];
int TxBufSize = sizeof(TxBuf);
- char* data;
+ //char data[(11+bytes)*_Nmotor];
Status[8] = 0x00; // The error is set to zero
// Build the TxPacket (FIXED !) first in RAM, then we'll send in one go
@@ -527,7 +533,7 @@
TxBuf[4] = 0xfe; // Broadcast
// Lenght 7 bytes = inst(1)+ param(4 + numbers of motors)+CRC(2)
TxBuf[5] = 7+(_Nmotor) & 0xff; // packet length low
- TxBuf[6] = 7+(_Nmotor) >> 8; // packet length high
+ TxBuf[6] = (7+(_Nmotor)) >> 8; // packet length high
// Instruction
TxBuf[7] = 0x82;
// Param
@@ -539,13 +545,14 @@
TxBuf[12+i] = _broadcastID[i];
}
// CRC
+
uint16_t crc16 ;
int dataSize = TxBufSize-2;
crc16 = update_crc(0, TxBuf, dataSize);
TxBuf[TxBufSize-2] = (crc16 & 0x00FF);
TxBuf[TxBufSize-1] = (crc16>>8) & 0x00FF;
- #ifdef SYNC_READPACKET_DEBUG_PACKETONLY
+ #if SYNC_READPACKET_DEBUG_PACKETONLY
printf("\n\n Complete Packet to send of %d elements\n", TxBufSize);
for (int i=0; i<TxBufSize ; i++){
printf("\nTxBuf[%d] = %02x ",i,TxBuf[i]);
@@ -556,27 +563,32 @@
}
printf("\n \n");
#endif
-
+
_pCommLayer->flush();
- _pCommLayer->write(TxBuf,TxBufSize); // UART-write
- wait(0.0002);
+ _pCommLayer->write(TxBuf,TxBufSize); // UART-write
+
+ wait_us(200);
+
_pCommLayer->read_timeout(Status, (11+bytes)*_Nmotor, 2.0);
+
+
// Take data from 8th cell of status array
// first byte contain error code, for this we use (bytes+1)
for (int i=0; i<(11+bytes)*_Nmotor ; i++){
data[i] = Status[i];
}
- #ifdef SYNC_READPACKET_DEBUG_PACKETONLY
+
+
+ #if SYNC_READPACKET_DEBUG_PACKETONLY
printf("\n SYNC-READ (entire buffer RAW values)\n");
for (int i=0; i<(11+bytes)*_Nmotor ; i++){
- data[i] = Status[i];
- printf("data[%d]:%02x\n",i,data[i]);
+ printf("Status[%d]:%02x\n",i,Status[i]);
}
#endif
- return(data); // return error code
+ //return(data); // return error code
}
@@ -610,7 +622,7 @@
while (isMoving()) {}
}
-#ifdef SETGOAL_SPEED_DEBUG
+#if SETGOAL_SPEED_DEBUG
int16_t sp = speed * float(234.27/1023); //rev/min
printf("\nSetGoal to: %d Velocity in [rev/min]%d \n" ,speed,sp);
printf("Goal L: 0x%02x ; H: 0x%02x\n",data[0],data[1]);
@@ -645,7 +657,7 @@
while (isMoving()) {}
}
-#ifdef SETGOAL_DEBUG
+#if SETGOAL_DEBUG
printf("\n SetGoal to: %d in degree is: %f\n",goal,degrees);
printf(" Goal L: 0x%02x ; H: 0x%02x\n",data[0],data[1]);
#endif
@@ -661,7 +673,7 @@
char data[1];
data[0] = mode; // bottom 8 bits
-#ifdef OPERATING_MODE_DEBUG
+#if OPERATING_MODE_DEBUG
printf("\nOPERATING_MODE_DEBUG\n");
printf("Set Operating Mode value to: 0x%02x\n",mode);
printf( "* 0x00 = Current Control Mode\n"
@@ -693,7 +705,7 @@
char data[1];
data[0] = MotorBaud;
-#ifdef SETBAUD_DEBUG
+#if SETBAUD_DEBUG
printf("\nSTATUS Packet - SETBAUD_DEBUG\n");
printf("Set Baud rate value to: 0x%02x\n",MotorBaud);
printf( "* 0x00 = 9,600 bps \n"
@@ -739,7 +751,7 @@
_pCommLayer->write(TxBuf,10); // UART-write
_pCommLayer->read_timeout(Status, 11, 2.0);
-#ifdef REBOOT_ENABLE_DEBUG
+#if REBOOT_ENABLE_DEBUG
printf("\n Reboot Motor: (%d)",enableVal);
#endif
}
@@ -750,7 +762,7 @@
char data[1];
data[0] = enableVal;
-#ifdef TORQUE_ENABLE_DEBUG
+#if TORQUE_ENABLE_DEBUG
printf("\n Setting Torque Enable value: (%d)",enableVal);
#endif
@@ -773,7 +785,7 @@
// angle(degree) obtained from position(0 - 4095)
angle = (float)position*ScaleFactor;
-#ifdef GETPOSITION_DEBUG
+#if GETPOSITION_DEBUG
printf("\nGetPosition from ID: %d\n",_ID);
for (uint16_t i=0; i<4 ; i++) {
printf(" Data[%d] : 0x%02x\n",(i+1),data[i]);
@@ -826,7 +838,7 @@
_pCommLayer->read_timeout(Status, 11, 2.0);
-#ifdef SENDPACKET_DEBUG
+#if SENDPACKET_DEBUG
printf("\nWRITE input parameters:\n");
printf(" (ID: %d, Address: 0x%x, Bytes: %d, flag: %d)\n",_ID,start,bytes,flag);
printf(" Data written:\n");
@@ -835,7 +847,7 @@
}
#endif
-#ifdef SENDPACKET_DEBUG_INSTRUCTION_PKT
+#if SENDPACKET_DEBUG_INSTRUCTION_PKT
printf("\nINSTRUCTIONS Packet - WRITE_DEBUG \n");
printf(" H1 : 0x%02x\n",TxBuf[0]);
printf(" H2 : 0x%02x\n",TxBuf[1]);
@@ -859,7 +871,7 @@
printf("\n");
#endif
-#ifdef SENDPACKET_DEBUG_STATUS_PKT
+#if SENDPACKET_DEBUG_STATUS_PKT
printf("\nSTATUS Packet - WRITE_DEBUG\n");
printf(" H1 : 0x%02x\n",Status[0]);
printf(" H2 : 0x%02x\n",Status[1]);
@@ -918,7 +930,7 @@
_pCommLayer->read_timeout(Status, 11+bytes, 2.0);
-#ifdef MX_READPACKET_DEBUG
+#if MX_READPACKET_DEBUG
printf("\nREAD input parameters:\n");
printf(" (ID: %d, Address: 0x%x, Bytes: %d, flag: %d)\n",_ID,start,bytes,flag);
printf(" Data written:\n");
@@ -927,7 +939,7 @@
}
#endif
-#ifdef MX_READPACKET_DEBUG_INSTRUCTION_PKT
+#if MX_READPACKET_DEBUG_INSTRUCTION_PKT
printf("\nINSTRUCTIONS Packet - READ_DEBUG \n");
printf(" H1 - Header : 0x%02x\n",TxBuf[0]);
printf(" H2 - Header : 0x%02x\n",TxBuf[1]);
@@ -950,7 +962,7 @@
printf("\n\n");
#endif
-#ifdef MX_READPACKET_DEBUG_STATUS_PKT
+#if MX_READPACKET_DEBUG_STATUS_PKT
printf("\nSTATUS Packet - READ_DEBUG \n");
printf(" H1 - Header : 0x%02x\n",Status[0]);
printf(" H2 - Header : 0x%02x\n",Status[1]);