Diff: motor.cpp

Revision:

3:df8469e71728

Parent:

2:01227a4bd3d7

Child:

4:a629f46d6594

--- a/motor.cpp	Thu May 09 23:53:27 2019 +0000
+++ b/motor.cpp	Thu May 23 08:53:19 2019 +0000
@@ -8,7 +8,7 @@
     _pwm.period_ms(32);
     _pwm.write(0);
     _tick.attach_us(this,&MotorCtl::PIDControl,32000);
-    kp=7.0;
+    kp=0.2;
     ki=0.05;
     kd=0.0;
     integ=0;
@@ -129,6 +129,7 @@
             break;
     }
     PreviousEncode = CurrentEncode;
+ //   printf("PreviousEncode: %x  CSP: %x\r\n",PreviousEncode,CurrentPosition);
 }
 
 
@@ -138,26 +139,30 @@
     DeltaCnt = CurrentPosition - PreviousPosition;
     PreviousPosition = CurrentPosition;
     CurrentSpeed = (int)CalculateRPM(DeltaCnt);
-    if (cpidx<MaxBuf) cw0[cpidx++]=CurrentSpeed;
-
+    printf("DeltaCnt: %d  CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed);
     Error = TargetSpeed - CurrentSpeed;
 
     integ += Error;
     derv = Error-PreviousError;
     control = (int)(kp*Error+ki*integ+kd*derv);
-    if (control>510) control=510;
-    else if(control <-510) control = -510;
+    if (control>35) control=35;
+    else if(control <-35) control = -35;
     PreviousError = Error;
     if(TargetSpeed>0){
-        duty += control;
+        duty =TargetSpeed+control;
         _Dir=1;
+    //printf("control: %d\r\n",control);
     }else{
-        duty +=- control;
+        duty =TargetSpeed-control;
         _Dir=0;
     }
-    vel=duty/1000000;
+    
+    //if(duty<=0){duty=-duty;}
+    vel=duty/180.0;
+    
+   printf("Duty: %d  vel: %f\r\n",duty,vel);
     _pwm.write(vel);
-    printf("duty: %d  Error: %d\r\n",duty,Error);
+  
     
    
         
@@ -167,7 +172,8 @@
 
 float  MotorCtl::CalculateRPM(int DeltaCnt)
 {
-    return DeltaCnt*60/(DeltaT)/CntPerRev; // Time measured in us
+   
+    return(DeltaCnt*60*1000)/(DeltaT*CntPerRev); // Time measured in us
 }