v1
motor.cpp@6:366ec55e64fe, 2019-07-10 (annotated)
- Committer:
- kangmingyo
- Date:
- Wed Jul 10 04:08:29 2019 +0000
- Revision:
- 6:366ec55e64fe
- Parent:
- 5:15d0276d7e21
- Child:
- 7:42c478f9a1fe
velocity update
Who changed what in which revision?
User | Revision | Line number | New 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 | 4:a629f46d6594 | 11 | kp=0.1; |
kangmingyo | 4:a629f46d6594 | 12 | ki=0.2; |
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 | 4:a629f46d6594 | 22 | TargetSpeed = 0; //initial target speed |
kangmingyo | 5:15d0276d7e21 | 23 | vel=0; |
kangmingyo | 5:15d0276d7e21 | 24 | DistancePosition =0; |
kangmingyo | 0:20e026e254d6 | 25 | } |
kangmingyo | 0:20e026e254d6 | 26 | |
kangmingyo | 0:20e026e254d6 | 27 | MotorCtl::~MotorCtl() |
kangmingyo | 0:20e026e254d6 | 28 | { |
kangmingyo | 0:20e026e254d6 | 29 | } |
kangmingyo | 0:20e026e254d6 | 30 | |
kangmingyo | 0:20e026e254d6 | 31 | int MotorCtl::getRPM() |
kangmingyo | 0:20e026e254d6 | 32 | { |
kangmingyo | 0:20e026e254d6 | 33 | return CurrentSpeed; |
kangmingyo | 0:20e026e254d6 | 34 | } |
kangmingyo | 0:20e026e254d6 | 35 | // Rturn target speed |
kangmingyo | 0:20e026e254d6 | 36 | int MotorCtl::getTarget() |
kangmingyo | 0:20e026e254d6 | 37 | { |
kangmingyo | 0:20e026e254d6 | 38 | return TargetSpeed; |
kangmingyo | 0:20e026e254d6 | 39 | } |
kangmingyo | 0:20e026e254d6 | 40 | int MotorCtl::getError() |
kangmingyo | 0:20e026e254d6 | 41 | { |
kangmingyo | 0:20e026e254d6 | 42 | return Error; |
kangmingyo | 0:20e026e254d6 | 43 | } |
kangmingyo | 0:20e026e254d6 | 44 | float MotorCtl::getKP() |
kangmingyo | 0:20e026e254d6 | 45 | { |
kangmingyo | 0:20e026e254d6 | 46 | return kp; |
kangmingyo | 0:20e026e254d6 | 47 | } |
kangmingyo | 0:20e026e254d6 | 48 | float MotorCtl::getKI() |
kangmingyo | 0:20e026e254d6 | 49 | { |
kangmingyo | 0:20e026e254d6 | 50 | return ki; |
kangmingyo | 0:20e026e254d6 | 51 | } |
kangmingyo | 0:20e026e254d6 | 52 | float MotorCtl::getKD() |
kangmingyo | 0:20e026e254d6 | 53 | { |
kangmingyo | 0:20e026e254d6 | 54 | return kd; |
kangmingyo | 0:20e026e254d6 | 55 | } |
kangmingyo | 0:20e026e254d6 | 56 | int *MotorCtl::getHistory() |
kangmingyo | 0:20e026e254d6 | 57 | { |
kangmingyo | 0:20e026e254d6 | 58 | return cw0; |
kangmingyo | 0:20e026e254d6 | 59 | } |
kangmingyo | 0:20e026e254d6 | 60 | int MotorCtl::getCurrentPosition() |
kangmingyo | 0:20e026e254d6 | 61 | { |
kangmingyo | 0:20e026e254d6 | 62 | return CurrentPosition; |
kangmingyo | 0:20e026e254d6 | 63 | } |
kangmingyo | 0:20e026e254d6 | 64 | |
kangmingyo | 0:20e026e254d6 | 65 | //set Method |
kangmingyo | 0:20e026e254d6 | 66 | |
kangmingyo | 0:20e026e254d6 | 67 | void MotorCtl::SetPeriod(long pwmPeriod) |
kangmingyo | 0:20e026e254d6 | 68 | { |
kangmingyo | 0:20e026e254d6 | 69 | _pwm.period_ms(pwmPeriod); |
kangmingyo | 0:20e026e254d6 | 70 | } |
kangmingyo | 0:20e026e254d6 | 71 | |
kangmingyo | 0:20e026e254d6 | 72 | void MotorCtl::setTarget(int spd) |
kangmingyo | 0:20e026e254d6 | 73 | { |
kangmingyo | 0:20e026e254d6 | 74 | if(spd<0) dr=0; |
kangmingyo | 0:20e026e254d6 | 75 | else dr=10; |
kangmingyo | 0:20e026e254d6 | 76 | setDirection(); |
kangmingyo | 5:15d0276d7e21 | 77 | |
kangmingyo | 0:20e026e254d6 | 78 | TargetSpeed = spd; |
kangmingyo | 0:20e026e254d6 | 79 | integ = 0; |
kangmingyo | 0:20e026e254d6 | 80 | derv=0; |
kangmingyo | 0:20e026e254d6 | 81 | cpidx=0; // clear |
kangmingyo | 5:15d0276d7e21 | 82 | // printf("Target %d\r\n",TargetSpeed); |
kangmingyo | 0:20e026e254d6 | 83 | } |
kangmingyo | 5:15d0276d7e21 | 84 | void MotorCtl::setPID(float p, float i, float d) |
kangmingyo | 0:20e026e254d6 | 85 | { |
kangmingyo | 0:20e026e254d6 | 86 | kp = p; |
kangmingyo | 0:20e026e254d6 | 87 | ki = i; |
kangmingyo | 0:20e026e254d6 | 88 | kd = d; |
kangmingyo | 0:20e026e254d6 | 89 | integ =0; |
kangmingyo | 0:20e026e254d6 | 90 | derv=0; |
kangmingyo | 0:20e026e254d6 | 91 | cpidx=0; |
kangmingyo | 0:20e026e254d6 | 92 | } |
kangmingyo | 0:20e026e254d6 | 93 | |
kangmingyo | 0:20e026e254d6 | 94 | void MotorCtl::Reset(void) |
kangmingyo | 0:20e026e254d6 | 95 | { |
kangmingyo | 0:20e026e254d6 | 96 | integ=0; |
kangmingyo | 0:20e026e254d6 | 97 | derv=0; |
kangmingyo | 0:20e026e254d6 | 98 | cpidx=0; |
kangmingyo | 0:20e026e254d6 | 99 | } |
kangmingyo | 0:20e026e254d6 | 100 | |
kangmingyo | 0:20e026e254d6 | 101 | void MotorCtl::setDirection() |
kangmingyo | 0:20e026e254d6 | 102 | { |
kangmingyo | 0:20e026e254d6 | 103 | if (dr>5) { |
kangmingyo | 0:20e026e254d6 | 104 | _Dir=1; |
kangmingyo | 0:20e026e254d6 | 105 | } else { |
kangmingyo | 0:20e026e254d6 | 106 | _Dir=0; |
kangmingyo | 0:20e026e254d6 | 107 | } |
kangmingyo | 0:20e026e254d6 | 108 | } |
kangmingyo | 0:20e026e254d6 | 109 | |
kangmingyo | 0:20e026e254d6 | 110 | void MotorCtl::UpdateCurrentPosition() |
kangmingyo | 0:20e026e254d6 | 111 | { |
kangmingyo | 0:20e026e254d6 | 112 | unsigned char CurrentEncode; |
kangmingyo | 0:20e026e254d6 | 113 | CurrentEncode = (_tachoA << 1 | _tachoB); |
kangmingyo | 0:20e026e254d6 | 114 | switch(CurrentEncode) { |
kangmingyo | 0:20e026e254d6 | 115 | case 0: |
kangmingyo | 0:20e026e254d6 | 116 | if (PreviousEncode==2) CurrentPosition +=1; |
kangmingyo | 0:20e026e254d6 | 117 | else if(PreviousEncode==1) CurrentPosition -=1; |
kangmingyo | 5:15d0276d7e21 | 118 | DistancePosition +=1; |
kangmingyo | 0:20e026e254d6 | 119 | break; |
kangmingyo | 0:20e026e254d6 | 120 | case 1: |
kangmingyo | 0:20e026e254d6 | 121 | if(PreviousEncode==0) CurrentPosition +=1; |
kangmingyo | 0:20e026e254d6 | 122 | else if(PreviousEncode==3) CurrentPosition -=1; |
kangmingyo | 5:15d0276d7e21 | 123 | DistancePosition +=1; |
kangmingyo | 0:20e026e254d6 | 124 | break; |
kangmingyo | 0:20e026e254d6 | 125 | case 2: |
kangmingyo | 0:20e026e254d6 | 126 | if(PreviousEncode==3) CurrentPosition +=1; |
kangmingyo | 0:20e026e254d6 | 127 | else if(PreviousEncode==0) CurrentPosition -=1; |
kangmingyo | 5:15d0276d7e21 | 128 | DistancePosition +=1; |
kangmingyo | 0:20e026e254d6 | 129 | break; |
kangmingyo | 0:20e026e254d6 | 130 | case 3: |
kangmingyo | 0:20e026e254d6 | 131 | if(PreviousEncode==1) CurrentPosition +=1; |
kangmingyo | 0:20e026e254d6 | 132 | else if(PreviousEncode==2) CurrentPosition -=1; |
kangmingyo | 5:15d0276d7e21 | 133 | DistancePosition +=1; |
kangmingyo | 0:20e026e254d6 | 134 | break; |
kangmingyo | 0:20e026e254d6 | 135 | default: |
kangmingyo | 0:20e026e254d6 | 136 | break; |
kangmingyo | 0:20e026e254d6 | 137 | } |
kangmingyo | 0:20e026e254d6 | 138 | PreviousEncode = CurrentEncode; |
kangmingyo | 5:15d0276d7e21 | 139 | // printf("CSP: %x\r\n",CurrentPosition); |
kangmingyo | 0:20e026e254d6 | 140 | } |
kangmingyo | 0:20e026e254d6 | 141 | |
kangmingyo | 0:20e026e254d6 | 142 | |
kangmingyo | 0:20e026e254d6 | 143 | void MotorCtl::PIDControl() |
kangmingyo | 0:20e026e254d6 | 144 | { |
kangmingyo | 0:20e026e254d6 | 145 | int DeltaCnt; |
kangmingyo | 0:20e026e254d6 | 146 | DeltaCnt = CurrentPosition - PreviousPosition; |
kangmingyo | 0:20e026e254d6 | 147 | PreviousPosition = CurrentPosition; |
kangmingyo | 0:20e026e254d6 | 148 | CurrentSpeed = (int)CalculateRPM(DeltaCnt); |
kangmingyo | 5:15d0276d7e21 | 149 | // printf("DeltaCnt: %d CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed); |
kangmingyo | 0:20e026e254d6 | 150 | Error = TargetSpeed - CurrentSpeed; |
kangmingyo | 0:20e026e254d6 | 151 | |
kangmingyo | 0:20e026e254d6 | 152 | integ += Error; |
kangmingyo | 0:20e026e254d6 | 153 | derv = Error-PreviousError; |
kangmingyo | 0:20e026e254d6 | 154 | control = (int)(kp*Error+ki*integ+kd*derv); |
kangmingyo | 4:a629f46d6594 | 155 | if (control>40) control=40; |
kangmingyo | 4:a629f46d6594 | 156 | else if(control <-40) control = -40; |
kangmingyo | 0:20e026e254d6 | 157 | PreviousError = Error; |
kangmingyo | 1:911f5a86c105 | 158 | if(TargetSpeed>0){ |
kangmingyo | 5:15d0276d7e21 | 159 | vel =(TargetSpeed+control)/200.0; |
kangmingyo | 5:15d0276d7e21 | 160 | _pwm.write(vel); |
kangmingyo | 1:911f5a86c105 | 161 | _Dir=1; |
kangmingyo | 4:a629f46d6594 | 162 | // printf("control: %d\r\n",control); |
kangmingyo | 2:01227a4bd3d7 | 163 | }else{ |
kangmingyo | 5:15d0276d7e21 | 164 | vel =(-TargetSpeed-control)/200.0; |
kangmingyo | 5:15d0276d7e21 | 165 | _pwm.write(vel); |
kangmingyo | 2:01227a4bd3d7 | 166 | _Dir=0; |
kangmingyo | 5:15d0276d7e21 | 167 | } |
kangmingyo | 5:15d0276d7e21 | 168 | // printf("Duty: %d Target:%d control:%d\r\n",duty,TargetSpeed, control); |
kangmingyo | 0:20e026e254d6 | 169 | |
kangmingyo | 0:20e026e254d6 | 170 | } |
kangmingyo | 0:20e026e254d6 | 171 | |
kangmingyo | 0:20e026e254d6 | 172 | float MotorCtl::CalculateRPM(int DeltaCnt) |
kangmingyo | 0:20e026e254d6 | 173 | { |
kangmingyo | 3:df8469e71728 | 174 | |
kangmingyo | 4:a629f46d6594 | 175 | return(DeltaCnt*60.0*1000.0)/(float)(DeltaT*CntPerRev); // Time measured in us |
kangmingyo | 0:20e026e254d6 | 176 | } |
kangmingyo | 0:20e026e254d6 | 177 | |
kangmingyo | 5:15d0276d7e21 | 178 | float MotorCtl::CalculateCumDis() //accumulate Distance |
kangmingyo | 5:15d0276d7e21 | 179 | { |
kangmingyo | 5:15d0276d7e21 | 180 | |
kangmingyo | 5:15d0276d7e21 | 181 | return (DistancePosition*3.14159265359*0.043/CntPerRev); // Time measured in us |
kangmingyo | 5:15d0276d7e21 | 182 | } |
kangmingyo | 5:15d0276d7e21 | 183 | |
kangmingyo | 5:15d0276d7e21 | 184 | float MotorCtl::CalculateRelaDis() //relative Distance |
kangmingyo | 5:15d0276d7e21 | 185 | { |
kangmingyo | 5:15d0276d7e21 | 186 | return (CurrentPosition*3.14159265359*0.043/CntPerRev); |
kangmingyo | 5:15d0276d7e21 | 187 | } |
kangmingyo | 6:366ec55e64fe | 188 | float MotorCtl::CalculateVelocity() |
kangmingyo | 6:366ec55e64fe | 189 | { |
kangmingyo | 6:366ec55e64fe | 190 | return (DeltaCnt*1000.0)*3.14159265359*0.043/(float)(DeltaT*CntPerRev) |
kangmingyo | 6:366ec55e64fe | 191 | } |
kangmingyo | 0:20e026e254d6 | 192 | |
kangmingyo | 0:20e026e254d6 | 193 | |
kangmingyo | 5:15d0276d7e21 | 194 | |
kangmingyo | 6:366ec55e64fe | 195 |