motor.cpp

00001 #include "motor.h"
00002 
00003 MotorCtl::MotorCtl(PinName Pwm, PinName Dir, PinName tachoA, PinName tachoB):
00004     _pwm(Pwm), _Dir(Dir), _tachoA(tachoA), _tachoB(tachoB)
00005 {
00006     _Dir=1;
00007     dr = 10;
00008     _pwm.period_ms(32);
00009     _pwm.write(0);
00010     _tick.attach_us(this,&MotorCtl::PIDControl,32000);
00011     kp=0.1;
00012     ki=0.2;
00013     kd=0.0;
00014     integ=0;
00015     derv=0;
00016     cpidx =0;
00017 
00018     DeltaT = 32; // Timer Interrupt period = 32ms = 32,000 us
00019     CntPerRev = 2400; // 100 shft/rev x 6 pls/shft x 4 evnt/pls
00020     duty = 50;
00021     PreviousEncode = 0;
00022     TargetSpeed = 0; //initial target speed
00023     vel=0;
00024     DistancePosition =0;
00025 }
00026 
00027 MotorCtl::~MotorCtl()
00028 {
00029 }
00030 
00031 int MotorCtl::getRPM()
00032 {
00033     return CurrentSpeed;
00034 }
00035 // Rturn target speed
00036 int MotorCtl::getTarget()
00037 {
00038     return TargetSpeed;
00039 }
00040 int MotorCtl::getError()
00041 {
00042     return Error;
00043 }
00044 float MotorCtl::getKP()
00045 {
00046     return kp;
00047 }
00048 float MotorCtl::getKI()
00049 {
00050     return ki;
00051 }
00052 float MotorCtl::getKD()
00053 {
00054     return kd;
00055 }
00056 int *MotorCtl::getHistory()
00057 {
00058     return cw0;
00059 }
00060 int MotorCtl::getCurrentPosition()
00061 {
00062     return CurrentPosition;
00063 }
00064 
00065 //set Method
00066 
00067 void MotorCtl::SetPeriod(long pwmPeriod)
00068 {
00069     _pwm.period_ms(pwmPeriod);
00070 }
00071 
00072 void MotorCtl::setTarget(int spd)
00073 {
00074     if(spd<0) dr=0;
00075     else dr=10;
00076     setDirection();
00077     
00078     TargetSpeed = spd;
00079     integ = 0;
00080     derv=0;
00081     cpidx=0; // clear
00082 //    printf("Target %d\r\n",TargetSpeed);
00083 }
00084 void MotorCtl::setPID(float p, float i, float d)
00085 {
00086     kp = p;
00087     ki = i;
00088     kd = d;
00089     integ =0;
00090     derv=0;
00091     cpidx=0;
00092 }
00093 
00094 void MotorCtl::Reset(void)
00095 {
00096     integ=0;
00097     derv=0;
00098     cpidx=0;
00099 }
00100 
00101 void MotorCtl::setDirection()
00102 {
00103     if (dr>5) {
00104         _Dir=1;
00105     } else {
00106         _Dir=0;
00107     }
00108 }
00109 
00110 void MotorCtl::UpdateCurrentPosition()
00111 {
00112     unsigned char CurrentEncode;
00113     CurrentEncode = (_tachoA << 1 | _tachoB);
00114     switch(CurrentEncode) {
00115         case 0:
00116             if (PreviousEncode==2) CurrentPosition +=1;
00117             else if(PreviousEncode==1) CurrentPosition -=1;
00118             DistancePosition +=1;
00119             break;
00120         case 1:
00121             if(PreviousEncode==0) CurrentPosition +=1;
00122             else if(PreviousEncode==3) CurrentPosition -=1;
00123             DistancePosition +=1;
00124             break;
00125         case 2:
00126             if(PreviousEncode==3) CurrentPosition +=1;
00127             else if(PreviousEncode==0) CurrentPosition -=1;
00128             DistancePosition +=1;
00129             break;
00130         case 3:
00131             if(PreviousEncode==1) CurrentPosition +=1;
00132             else if(PreviousEncode==2) CurrentPosition -=1;
00133             DistancePosition +=1;
00134             break;
00135         default:
00136             break;
00137     }
00138     PreviousEncode = CurrentEncode;
00139 //    printf("CSP: %x\r\n",CurrentPosition);
00140 }
00141 
00142 
00143 void MotorCtl::PIDControl()
00144 {
00145     int DeltaCnt;
00146     DeltaCnt = CurrentPosition - PreviousPosition;
00147     PreviousPosition = CurrentPosition;
00148     CurrentSpeed = (int)CalculateRPM(DeltaCnt);
00149 //    printf("DeltaCnt: %d  CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed);
00150     Error = TargetSpeed - CurrentSpeed;
00151 
00152     integ += Error;
00153     derv = Error-PreviousError;
00154     control = (int)(kp*Error+ki*integ+kd*derv);
00155     if (control>40) control=40;
00156     else if(control <-40) control = -40;
00157     PreviousError = Error;
00158     if(TargetSpeed>0){
00159         vel =(TargetSpeed+control)/200.0;
00160          _pwm.write(vel);
00161         _Dir=1;
00162 //    printf("control: %d\r\n",control);
00163     }else{
00164         vel =(-TargetSpeed-control)/200.0;
00165         _pwm.write(vel);
00166         _Dir=0;
00167     } 
00168     //    printf("Duty: %d  Target:%d control:%d\r\n",duty,TargetSpeed, control);
00169     
00170 }
00171 
00172 float  MotorCtl::CalculateRPM(int DeltaCnt)
00173 {
00174    
00175     return(DeltaCnt*60.0*1000.0)/(float)(DeltaT*CntPerRev); // Time measured in us
00176 }
00177 
00178 float  MotorCtl::CalculateCumDis() //accumulate Distance
00179 {
00180    
00181     return (DistancePosition*3.14159265359*0.043/CntPerRev); // Time measured in us
00182 }
00183 
00184 float MotorCtl::CalculateRelaDis() //relative Distance
00185 {
00186     return (CurrentPosition*3.14159265359*0.043/CntPerRev);
00187 }
00188 float MotorCtl::CalculateVelocity()
00189 {
00190     return getRPM()*3.14159265359*0.043/60;
00191 }
00192 
00193 
00194 
00195