Kang mingyo / motor

v1

motor.cpp@2:01227a4bd3d7, 2019-05-09 (annotated)

Committer:
kangmingyo
Date:
Thu May 09 23:53:27 2019 +0000
Revision:
2:01227a4bd3d7
Parent:
1:911f5a86c105
Child:
3:df8469e71728
k

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kangmingyo 0:20e026e254d6 1 #include "motor.h"
kangmingyo 0:20e026e254d6 2
kangmingyo 0:20e026e254d6 3 MotorCtl::MotorCtl(PinName Pwm, PinName Dir, PinName tachoA, PinName tachoB):
kangmingyo 0:20e026e254d6 4 _pwm(Pwm), _Dir(Dir), _tachoA(tachoA), _tachoB(tachoB)
kangmingyo 0:20e026e254d6 5 {
kangmingyo 0:20e026e254d6 6 _Dir=1;
kangmingyo 0:20e026e254d6 7 dr = 10;
kangmingyo 1:911f5a86c105 8 _pwm.period_ms(32);
kangmingyo 0:20e026e254d6 9 _pwm.write(0);
kangmingyo 0:20e026e254d6 10 _tick.attach_us(this,&MotorCtl::PIDControl,32000);
kangmingyo 0:20e026e254d6 11 kp=7.0;
kangmingyo 0:20e026e254d6 12 ki=0.05;
kangmingyo 0:20e026e254d6 13 kd=0.0;
kangmingyo 0:20e026e254d6 14 integ=0;
kangmingyo 0:20e026e254d6 15 derv=0;
kangmingyo 0:20e026e254d6 16 cpidx =0;
kangmingyo 0:20e026e254d6 17
kangmingyo 0:20e026e254d6 18 DeltaT = 32; // Timer Interrupt period = 32ms = 32,000 us
kangmingyo 0:20e026e254d6 19 CntPerRev = 2400; // 100 shft/rev x 6 pls/shft x 4 evnt/pls
kangmingyo 0:20e026e254d6 20 duty = 50;
kangmingyo 0:20e026e254d6 21 PreviousEncode = 0;
kangmingyo 0:20e026e254d6 22 TargetSpeed = 50; //initial target speed
kangmingyo 0:20e026e254d6 23
kangmingyo 0:20e026e254d6 24 }
kangmingyo 0:20e026e254d6 25
kangmingyo 0:20e026e254d6 26 MotorCtl::~MotorCtl()
kangmingyo 0:20e026e254d6 27 {
kangmingyo 0:20e026e254d6 28 }
kangmingyo 0:20e026e254d6 29
kangmingyo 0:20e026e254d6 30 int MotorCtl::getRPM()
kangmingyo 0:20e026e254d6 31 {
kangmingyo 0:20e026e254d6 32 return CurrentSpeed;
kangmingyo 0:20e026e254d6 33 }
kangmingyo 0:20e026e254d6 34 // Rturn target speed
kangmingyo 0:20e026e254d6 35 int MotorCtl::getTarget()
kangmingyo 0:20e026e254d6 36 {
kangmingyo 0:20e026e254d6 37 return TargetSpeed;
kangmingyo 0:20e026e254d6 38 }
kangmingyo 0:20e026e254d6 39 int MotorCtl::getError()
kangmingyo 0:20e026e254d6 40 {
kangmingyo 0:20e026e254d6 41 return Error;
kangmingyo 0:20e026e254d6 42 }
kangmingyo 0:20e026e254d6 43 float MotorCtl::getKP()
kangmingyo 0:20e026e254d6 44 {
kangmingyo 0:20e026e254d6 45 return kp;
kangmingyo 0:20e026e254d6 46 }
kangmingyo 0:20e026e254d6 47 float MotorCtl::getKI()
kangmingyo 0:20e026e254d6 48 {
kangmingyo 0:20e026e254d6 49 return ki;
kangmingyo 0:20e026e254d6 50 }
kangmingyo 0:20e026e254d6 51 float MotorCtl::getKD()
kangmingyo 0:20e026e254d6 52 {
kangmingyo 0:20e026e254d6 53 return kd;
kangmingyo 0:20e026e254d6 54 }
kangmingyo 0:20e026e254d6 55 int *MotorCtl::getHistory()
kangmingyo 0:20e026e254d6 56 {
kangmingyo 0:20e026e254d6 57 return cw0;
kangmingyo 0:20e026e254d6 58 }
kangmingyo 0:20e026e254d6 59 int MotorCtl::getCurrentPosition()
kangmingyo 0:20e026e254d6 60 {
kangmingyo 0:20e026e254d6 61 return CurrentPosition;
kangmingyo 0:20e026e254d6 62 }
kangmingyo 0:20e026e254d6 63
kangmingyo 0:20e026e254d6 64 //set Method
kangmingyo 0:20e026e254d6 65
kangmingyo 0:20e026e254d6 66 void MotorCtl::SetPeriod(long pwmPeriod)
kangmingyo 0:20e026e254d6 67 {
kangmingyo 0:20e026e254d6 68 _pwm.period_ms(pwmPeriod);
kangmingyo 0:20e026e254d6 69 }
kangmingyo 0:20e026e254d6 70
kangmingyo 0:20e026e254d6 71 void MotorCtl::setTarget(int spd)
kangmingyo 0:20e026e254d6 72 {
kangmingyo 0:20e026e254d6 73 if(spd<0) dr=0;
kangmingyo 0:20e026e254d6 74 else dr=10;
kangmingyo 0:20e026e254d6 75 setDirection();
kangmingyo 0:20e026e254d6 76 TargetSpeed = spd;
kangmingyo 0:20e026e254d6 77 integ = 0;
kangmingyo 0:20e026e254d6 78 derv=0;
kangmingyo 0:20e026e254d6 79 cpidx=0; // clear
kangmingyo 0:20e026e254d6 80 }
kangmingyo 0:20e026e254d6 81 void MotorCtl:: setPID(float p, float i, float d)
kangmingyo 0:20e026e254d6 82 {
kangmingyo 0:20e026e254d6 83 kp = p;
kangmingyo 0:20e026e254d6 84 ki = i;
kangmingyo 0:20e026e254d6 85 kd = d;
kangmingyo 0:20e026e254d6 86 integ =0;
kangmingyo 0:20e026e254d6 87 derv=0;
kangmingyo 0:20e026e254d6 88 cpidx=0;
kangmingyo 0:20e026e254d6 89 }
kangmingyo 0:20e026e254d6 90
kangmingyo 0:20e026e254d6 91 void MotorCtl::Reset(void)
kangmingyo 0:20e026e254d6 92 {
kangmingyo 0:20e026e254d6 93 integ=0;
kangmingyo 0:20e026e254d6 94 derv=0;
kangmingyo 0:20e026e254d6 95 cpidx=0;
kangmingyo 0:20e026e254d6 96 }
kangmingyo 0:20e026e254d6 97
kangmingyo 0:20e026e254d6 98 void MotorCtl::setDirection()
kangmingyo 0:20e026e254d6 99 {
kangmingyo 0:20e026e254d6 100 if (dr>5) {
kangmingyo 0:20e026e254d6 101 _Dir=1;
kangmingyo 0:20e026e254d6 102 } else {
kangmingyo 0:20e026e254d6 103 _Dir=0;
kangmingyo 0:20e026e254d6 104 }
kangmingyo 0:20e026e254d6 105 }
kangmingyo 0:20e026e254d6 106
kangmingyo 0:20e026e254d6 107 void MotorCtl::UpdateCurrentPosition()
kangmingyo 0:20e026e254d6 108 {
kangmingyo 0:20e026e254d6 109 unsigned char CurrentEncode;
kangmingyo 0:20e026e254d6 110 CurrentEncode = (_tachoA << 1 | _tachoB);
kangmingyo 0:20e026e254d6 111 switch(CurrentEncode) {
kangmingyo 0:20e026e254d6 112 case 0:
kangmingyo 0:20e026e254d6 113 if (PreviousEncode==2) CurrentPosition +=1;
kangmingyo 0:20e026e254d6 114 else if(PreviousEncode==1) CurrentPosition -=1;
kangmingyo 0:20e026e254d6 115 break;
kangmingyo 0:20e026e254d6 116 case 1:
kangmingyo 0:20e026e254d6 117 if(PreviousEncode==0) CurrentPosition +=1;
kangmingyo 0:20e026e254d6 118 else if(PreviousEncode==3) CurrentPosition -=1;
kangmingyo 0:20e026e254d6 119 break;
kangmingyo 0:20e026e254d6 120 case 2:
kangmingyo 0:20e026e254d6 121 if(PreviousEncode==3) CurrentPosition +=1;
kangmingyo 0:20e026e254d6 122 else if(PreviousEncode==0) CurrentPosition -=1;
kangmingyo 0:20e026e254d6 123 break;
kangmingyo 0:20e026e254d6 124 case 3:
kangmingyo 0:20e026e254d6 125 if(PreviousEncode==1) CurrentPosition +=1;
kangmingyo 0:20e026e254d6 126 else if(PreviousEncode==2) CurrentPosition -=1;
kangmingyo 0:20e026e254d6 127 break;
kangmingyo 0:20e026e254d6 128 default:
kangmingyo 0:20e026e254d6 129 break;
kangmingyo 0:20e026e254d6 130 }
kangmingyo 0:20e026e254d6 131 PreviousEncode = CurrentEncode;
kangmingyo 0:20e026e254d6 132 }
kangmingyo 0:20e026e254d6 133
kangmingyo 0:20e026e254d6 134
kangmingyo 0:20e026e254d6 135 void MotorCtl::PIDControl()
kangmingyo 0:20e026e254d6 136 {
kangmingyo 0:20e026e254d6 137 int DeltaCnt;
kangmingyo 0:20e026e254d6 138 DeltaCnt = CurrentPosition - PreviousPosition;
kangmingyo 0:20e026e254d6 139 PreviousPosition = CurrentPosition;
kangmingyo 0:20e026e254d6 140 CurrentSpeed = (int)CalculateRPM(DeltaCnt);
kangmingyo 0:20e026e254d6 141 if (cpidx<MaxBuf) cw0[cpidx++]=CurrentSpeed;
kangmingyo 0:20e026e254d6 142
kangmingyo 0:20e026e254d6 143 Error = TargetSpeed - CurrentSpeed;
kangmingyo 0:20e026e254d6 144
kangmingyo 0:20e026e254d6 145 integ += Error;
kangmingyo 0:20e026e254d6 146 derv = Error-PreviousError;
kangmingyo 0:20e026e254d6 147 control = (int)(kp*Error+ki*integ+kd*derv);
kangmingyo 0:20e026e254d6 148 if (control>510) control=510;
kangmingyo 0:20e026e254d6 149 else if(control <-510) control = -510;
kangmingyo 0:20e026e254d6 150 PreviousError = Error;
kangmingyo 1:911f5a86c105 151 if(TargetSpeed>0){
kangmingyo 1:911f5a86c105 152 duty += control;
kangmingyo 1:911f5a86c105 153 _Dir=1;
kangmingyo 2:01227a4bd3d7 154 }else{
kangmingyo 2:01227a4bd3d7 155 duty +=- control;
kangmingyo 2:01227a4bd3d7 156 _Dir=0;
kangmingyo 0:20e026e254d6 157 }
kangmingyo 2:01227a4bd3d7 158 vel=duty/1000000;
kangmingyo 1:911f5a86c105 159 _pwm.write(vel);
kangmingyo 1:911f5a86c105 160 printf("duty: %d Error: %d\r\n",duty,Error);
kangmingyo 0:20e026e254d6 161
kangmingyo 0:20e026e254d6 162
kangmingyo 0:20e026e254d6 163
kangmingyo 0:20e026e254d6 164
kangmingyo 0:20e026e254d6 165
kangmingyo 0:20e026e254d6 166 }
kangmingyo 0:20e026e254d6 167
kangmingyo 0:20e026e254d6 168 float MotorCtl::CalculateRPM(int DeltaCnt)
kangmingyo 0:20e026e254d6 169 {
kangmingyo 1:911f5a86c105 170 return DeltaCnt*60/(DeltaT)/CntPerRev; // Time measured in us
kangmingyo 0:20e026e254d6 171 }
kangmingyo 0:20e026e254d6 172
kangmingyo 0:20e026e254d6 173
kangmingyo 0:20e026e254d6 174