Diff: motor.cpp

Revision:

0:20e026e254d6

Child:

1:911f5a86c105

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.cpp	Thu May 09 22:20:02 2019 +0000
@@ -0,0 +1,171 @@
+#include "motor.h"
+
+MotorCtl::MotorCtl(PinName Pwm, PinName Dir, PinName tachoA, PinName tachoB):
+    _pwm(Pwm), _Dir(Dir), _tachoA(tachoA), _tachoB(tachoB)
+{
+    _Dir=1;
+    dr = 10;
+    _pwm.period_ms(50);
+    _pwm.write(0);
+    _tick.attach_us(this,&MotorCtl::PIDControl,32000);
+    kp=7.0;
+    ki=0.05;
+    kd=0.0;
+    integ=0;
+    derv=0;
+    cpidx =0;
+
+    DeltaT = 32; // Timer Interrupt period = 32ms = 32,000 us
+    CntPerRev = 2400; // 100 shft/rev x 6 pls/shft x 4 evnt/pls
+    duty = 50;
+    PreviousEncode = 0;
+    TargetSpeed = 50; //initial target speed
+
+}
+
+MotorCtl::~MotorCtl()
+{
+}
+
+int MotorCtl::getRPM()
+{
+    return CurrentSpeed;
+}
+// Rturn target speed
+int MotorCtl::getTarget()
+{
+    return TargetSpeed;
+}
+int MotorCtl::getError()
+{
+    return Error;
+}
+float MotorCtl::getKP()
+{
+    return kp;
+}
+float MotorCtl::getKI()
+{
+    return ki;
+}
+float MotorCtl::getKD()
+{
+    return kd;
+}
+int *MotorCtl::getHistory()
+{
+    return cw0;
+}
+int MotorCtl::getCurrentPosition()
+{
+    return CurrentPosition;
+}
+
+//set Method
+
+void MotorCtl::SetPeriod(long pwmPeriod)
+{
+    _pwm.period_ms(pwmPeriod);
+}
+
+void MotorCtl::setTarget(int spd)
+{
+    if(spd<0) dr=0;
+    else dr=10;
+    setDirection();
+    TargetSpeed = spd;
+    integ = 0;
+    derv=0;
+    cpidx=0; // clear
+}
+void MotorCtl:: setPID(float p, float i, float d)
+{
+    kp = p;
+    ki = i;
+    kd = d;
+    integ =0;
+    derv=0;
+    cpidx=0;
+}
+
+void MotorCtl::Reset(void)
+{
+    integ=0;
+    derv=0;
+    cpidx=0;
+}
+
+void MotorCtl::setDirection()
+{
+    if (dr>5) {
+        _Dir=1;
+    } else {
+        _Dir=0;
+    }
+}
+
+void MotorCtl::UpdateCurrentPosition()
+{
+    unsigned char CurrentEncode;
+    CurrentEncode = (_tachoA << 1 | _tachoB);
+    switch(CurrentEncode) {
+        case 0:
+            if (PreviousEncode==2) CurrentPosition +=1;
+            else if(PreviousEncode==1) CurrentPosition -=1;
+            break;
+        case 1:
+            if(PreviousEncode==0) CurrentPosition +=1;
+            else if(PreviousEncode==3) CurrentPosition -=1;
+            break;
+        case 2:
+            if(PreviousEncode==3) CurrentPosition +=1;
+            else if(PreviousEncode==0) CurrentPosition -=1;
+            break;
+        case 3:
+            if(PreviousEncode==1) CurrentPosition +=1;
+            else if(PreviousEncode==2) CurrentPosition -=1;
+            break;
+        default:
+            break;
+    }
+    PreviousEncode = CurrentEncode;
+}
+
+
+void MotorCtl::PIDControl()
+{
+    int DeltaCnt;
+    DeltaCnt = CurrentPosition - PreviousPosition;
+    PreviousPosition = CurrentPosition;
+    CurrentSpeed = (int)CalculateRPM(DeltaCnt);
+    if (cpidx<MaxBuf) cw0[cpidx++]=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;
+    PreviousError = Error;
+    if(dr>5){
+        duty+= control;
+        _Dir=1;
+    }else{
+        duty += -control;
+        _Dir=0;
+    }
+    
+   
+        
+    
+    
+}
+
+float  MotorCtl::CalculateRPM(int DeltaCnt)
+{
+    return DeltaCnt*60*(1000/DeltaT)/CntPerRev; // Time measured in us
+}
+
+
+