Diff: motor.cpp

Revision:

1:911f5a86c105

Parent:

0:20e026e254d6

Child:

2:01227a4bd3d7

--- a/motor.cpp	Thu May 09 22:20:02 2019 +0000
+++ b/motor.cpp	Thu May 09 23:51:13 2019 +0000
@@ -5,7 +5,7 @@
 {
     _Dir=1;
     dr = 10;
-    _pwm.period_ms(50);
+    _pwm.period_ms(32);
     _pwm.write(0);
     _tick.attach_us(this,&MotorCtl::PIDControl,32000);
     kp=7.0;
@@ -148,13 +148,16 @@
     if (control>510) control=510;
     else if(control <-510) control = -510;
     PreviousError = Error;
-    if(dr>5){
-        duty+= control;
-        _Dir=1;
-    }else{
+    if(TargetSpeed>0){
         duty += -control;
         _Dir=0;
+    }else{
+        duty += control;
+        _Dir=1;
     }
+    vel=duty/1000;
+    _pwm.write(vel);
+    printf("duty: %d  Error: %d\r\n",duty,Error);
     
    
         
@@ -164,7 +167,7 @@
 
 float  MotorCtl::CalculateRPM(int DeltaCnt)
 {
-    return DeltaCnt*60*(1000/DeltaT)/CntPerRev; // Time measured in us
+    return DeltaCnt*60/(DeltaT)/CntPerRev; // Time measured in us
 }