Diff: motor.cpp

Revision:

4:a629f46d6594

Parent:

3:df8469e71728

Child:

5:15d0276d7e21

--- a/motor.cpp	Thu May 23 08:53:19 2019 +0000
+++ b/motor.cpp	Thu May 23 23:02:14 2019 +0000
@@ -8,8 +8,8 @@
     _pwm.period_ms(32);
     _pwm.write(0);
     _tick.attach_us(this,&MotorCtl::PIDControl,32000);
-    kp=0.2;
-    ki=0.05;
+    kp=0.1;
+    ki=0.2;
     kd=0.0;
     integ=0;
     derv=0;
@@ -19,7 +19,7 @@
     CntPerRev = 2400; // 100 shft/rev x 6 pls/shft x 4 evnt/pls
     duty = 50;
     PreviousEncode = 0;
-    TargetSpeed = 50; //initial target speed
+    TargetSpeed = 0; //initial target speed
 
 }
 
@@ -139,29 +139,30 @@
     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;
     derv = Error-PreviousError;
     control = (int)(kp*Error+ki*integ+kd*derv);
-    if (control>35) control=35;
-    else if(control <-35) control = -35;
+    if (control>40) control=40;
+    else if(control <-40) control = -40;
     PreviousError = Error;
     if(TargetSpeed>0){
-        duty =TargetSpeed+control;
+        duty =(TargetSpeed+control)/200.0;
+         _pwm.write(duty);
         _Dir=1;
-    //printf("control: %d\r\n",control);
+//    printf("control: %d\r\n",control);
     }else{
-        duty =TargetSpeed-control;
+        duty =(-TargetSpeed-control)/200.0;
+        _pwm.write(duty);
         _Dir=0;
     }
     
-    //if(duty<=0){duty=-duty;}
-    vel=duty/180.0;
+
     
-   printf("Duty: %d  vel: %f\r\n",duty,vel);
-    _pwm.write(vel);
+//   printf("Duty: %d  vel: %f\r\n",duty,vel);
+
   
     
    
@@ -173,7 +174,7 @@
 float  MotorCtl::CalculateRPM(int DeltaCnt)
 {
    
-    return(DeltaCnt*60*1000)/(DeltaT*CntPerRev); // Time measured in us
+    return(DeltaCnt*60.0*1000.0)/(float)(DeltaT*CntPerRev); // Time measured in us
 }