Diff: motor.cpp

Revision:

5:15d0276d7e21

Parent:

4:a629f46d6594

Child:

6:366ec55e64fe

--- a/motor.cpp	Thu May 23 23:02:14 2019 +0000
+++ b/motor.cpp	Tue Jun 25 11:01:39 2019 +0000
@@ -20,7 +20,8 @@
     duty = 50;
     PreviousEncode = 0;
     TargetSpeed = 0; //initial target speed
-
+    vel=0;
+    DistancePosition =0;
 }
 
 MotorCtl::~MotorCtl()
@@ -73,12 +74,14 @@
     if(spd<0) dr=0;
     else dr=10;
     setDirection();
+    
     TargetSpeed = spd;
     integ = 0;
     derv=0;
     cpidx=0; // clear
+//    printf("Target %d\r\n",TargetSpeed);
 }
-void MotorCtl:: setPID(float p, float i, float d)
+void MotorCtl::setPID(float p, float i, float d)
 {
     kp = p;
     ki = i;
@@ -112,24 +115,28 @@
         case 0:
             if (PreviousEncode==2) CurrentPosition +=1;
             else if(PreviousEncode==1) CurrentPosition -=1;
+            DistancePosition +=1;
             break;
         case 1:
             if(PreviousEncode==0) CurrentPosition +=1;
             else if(PreviousEncode==3) CurrentPosition -=1;
+            DistancePosition +=1;
             break;
         case 2:
             if(PreviousEncode==3) CurrentPosition +=1;
             else if(PreviousEncode==0) CurrentPosition -=1;
+            DistancePosition +=1;
             break;
         case 3:
             if(PreviousEncode==1) CurrentPosition +=1;
             else if(PreviousEncode==2) CurrentPosition -=1;
+            DistancePosition +=1;
             break;
         default:
             break;
     }
     PreviousEncode = CurrentEncode;
- //   printf("PreviousEncode: %x  CSP: %x\r\n",PreviousEncode,CurrentPosition);
+//    printf("CSP: %x\r\n",CurrentPosition);
 }
 
 
@@ -139,7 +146,7 @@
     DeltaCnt = CurrentPosition - PreviousPosition;
     PreviousPosition = CurrentPosition;
     CurrentSpeed = (int)CalculateRPM(DeltaCnt);
-//  printf("DeltaCnt: %d  CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed);
+//    printf("DeltaCnt: %d  CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed);
     Error = TargetSpeed - CurrentSpeed;
 
     integ += Error;
@@ -149,25 +156,16 @@
     else if(control <-40) control = -40;
     PreviousError = Error;
     if(TargetSpeed>0){
-        duty =(TargetSpeed+control)/200.0;
-         _pwm.write(duty);
+        vel =(TargetSpeed+control)/200.0;
+         _pwm.write(vel);
         _Dir=1;
 //    printf("control: %d\r\n",control);
     }else{
-        duty =(-TargetSpeed-control)/200.0;
-        _pwm.write(duty);
+        vel =(-TargetSpeed-control)/200.0;
+        _pwm.write(vel);
         _Dir=0;
-    }
-    
-
-    
-//   printf("Duty: %d  vel: %f\r\n",duty,vel);
-
-  
-    
-   
-        
-    
+    } 
+    //    printf("Duty: %d  Target:%d control:%d\r\n",duty,TargetSpeed, control);
     
 }
 
@@ -177,5 +175,16 @@
     return(DeltaCnt*60.0*1000.0)/(float)(DeltaT*CntPerRev); // Time measured in us
 }
 
+float  MotorCtl::CalculateCumDis() //accumulate Distance
+{
+   
+    return (DistancePosition*3.14159265359*0.043/CntPerRev); // Time measured in us
+}
+
+float MotorCtl::CalculateRelaDis() //relative Distance
+{
+    return (CurrentPosition*3.14159265359*0.043/CntPerRev);
+}
 
 
+