Diff: lib_L6470DC.cpp

Revision:

7:2ff9788e2827

Parent:

6:7ca2eef852fd

diff -r 7ca2eef852fd -r 2ff9788e2827 lib_L6470DC.cpp
--- a/lib_L6470DC.cpp	Fri Apr 01 11:38:25 2022 +0000
+++ b/lib_L6470DC.cpp	Mon May 02 13:07:52 2022 +0000
@@ -317,14 +317,14 @@
 
 
 
-void L6470DC::Run(unsigned char nMOT,unsigned char dir,int spd)
+void L6470DC::Run(unsigned char nMOT,unsigned char dir,float spd)
 {
     unsigned char temp[4];
-    //spd = L6470_Step_s_2_Speed((float) spd);
+    int tmp_spd = L6470_Step_s_2_Speed(spd);
     temp[3] = 0x50|dir;
-    temp[2] = (unsigned char) (spd >> 16)&0x0F;
-    temp[1] = (unsigned char) (spd >>  8)&0xFF;
-    temp[0] = (unsigned char) (spd >>  0)&0xFF;
+    temp[2] = (unsigned char) (tmp_spd >> 16)&0x0F;
+    temp[1] = (unsigned char) (tmp_spd >>  8)&0xFF;
+    temp[0] = (unsigned char) (tmp_spd >>  0)&0xFF;
     send_bytes(nMOT,temp,sizeof temp/sizeof temp[0]);
 }
 
@@ -427,7 +427,7 @@
 {
     L6470_MotorConf *MotorParameterData = (L6470_MotorConf *) conf;
     StepperMotorRegister MotorRegister;
-    
+
     ResetDevice(nMOT);
 
     MotorRegister.ACC = L6470_Step_s2_2_Acc(MotorParameterData->acc);
@@ -486,7 +486,25 @@
 
 int L6470DC::GetSpeed(int nMOT)
 {
-    return GetParam(nMOT, SPEED);
+    return L6470_Speed_2_Step_s(GetParam(nMOT, SPEED));
+}
+
+int L6470DC::GetStatus(int nMOT, FlagId_t FlagId)
+{
+    int status = GetParam(nMOT, STATUS);
+    int ack = -1;
+
+    switch(FlagId) {
+        case MOT_STATUS_ID:
+            ack = status & (3 << MOT_STATUS_ID);
+            return ack >> MOT_STATUS_ID;
+            break;
+
+        default :
+            ack = status & (1 << FlagId);
+            return ack >> FlagId;
+            break;
+    }
 }
 
 void L6470DC::SetHome(int nMOT)
@@ -496,7 +514,7 @@
 
 void L6470DC::SetMaxSpeed(int nMOT, int speed)
 {
-    SetParam(nMOT, MAX_SPEED, speed);
+    SetParam(nMOT, MAX_SPEED, L6470_Step_s_2_MaxSpeed(speed));
 }
 
 void L6470DC::SetMark(int nMOT, int mark)
@@ -511,25 +529,24 @@
 
 int32_t L6470DC::L6470_AbsPos_2_Position(uint32_t AbsPos)
 {
-    
+
     if (AbsPos > L6470_MAX_POSITION)
         return (AbsPos - (L6470_POSITION_RANGE + 1));
     else
-    
-    return AbsPos;
+
+        return AbsPos;
 }
 
 uint32_t L6470DC::L6470_Position_2_AbsPos(int32_t Position)
 {
-  if ((Position >= 0) && (Position <= L6470_MAX_POSITION))
-    return Position;
-  else
-  {
-    if ((Position >= L6470_MIN_POSITION) && (Position < 0))
-      return (Position + (L6470_POSITION_RANGE + 1));
-    else
-      return (L6470_POSITION_RANGE + 1);        // OVF
-  }
+    if ((Position >= 0) && (Position <= L6470_MAX_POSITION))
+        return Position;
+    else {
+        if ((Position >= L6470_MIN_POSITION) && (Position < 0))
+            return (Position + (L6470_POSITION_RANGE + 1));
+        else
+            return (L6470_POSITION_RANGE + 1);        // OVF
+    }
 }
 
 float L6470DC::L6470_Speed_2_Step_s(uint32_t Speed)
@@ -539,10 +556,10 @@
 
 uint32_t L6470DC::L6470_Step_s_2_Speed(float Step_s)
 {
-    if (Step_s <= (L6470_MAX_SPEED * ((float)14.9012e-3)))
-        return (uint32_t)(Step_s / ((float)14.9012e-3));
-    else
-        return 0;
+    //if (Step_s <= (L6470_MAX_SPEED * ((float)14.9012e-3)))
+    return (uint32_t)(Step_s / ((float)14.9012e-3));
+    //else
+    //return 0;
 }
 
 float L6470DC::L6470_Acc_2_Step_s2(uint16_t Acc)