Jeong Hoon Jo / motor

190711

motor.cpp@4:a629f46d6594, 2019-05-23 (annotated)

Committer:
kangmingyo
Date:
Thu May 23 23:02:14 2019 +0000
Revision:
4:a629f46d6594
Parent:
3:df8469e71728
Child:
5:15d0276d7e21
2018/05/24

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 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 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 3:df8469e71728 132 // printf("PreviousEncode: %x CSP: %x\r\n",PreviousEncode,CurrentPosition);
kangmingyo 0:20e026e254d6 133 }
kangmingyo 0:20e026e254d6 134
kangmingyo 0:20e026e254d6 135
kangmingyo 0:20e026e254d6 136 void MotorCtl::PIDControl()
kangmingyo 0:20e026e254d6 137 {
kangmingyo 0:20e026e254d6 138 int DeltaCnt;
kangmingyo 0:20e026e254d6 139 DeltaCnt = CurrentPosition - PreviousPosition;
kangmingyo 0:20e026e254d6 140 PreviousPosition = CurrentPosition;
kangmingyo 0:20e026e254d6 141 CurrentSpeed = (int)CalculateRPM(DeltaCnt);
kangmingyo 4:a629f46d6594 142 // printf("DeltaCnt: %d CurrentSpeed: %ld\r\n",DeltaCnt,CurrentSpeed);
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 4:a629f46d6594 148 if (control>40) control=40;
kangmingyo 4:a629f46d6594 149 else if(control <-40) control = -40;
kangmingyo 0:20e026e254d6 150 PreviousError = Error;
kangmingyo 1:911f5a86c105 151 if(TargetSpeed>0){
kangmingyo 4:a629f46d6594 152 duty =(TargetSpeed+control)/200.0;
kangmingyo 4:a629f46d6594 153 _pwm.write(duty);
kangmingyo 1:911f5a86c105 154 _Dir=1;
kangmingyo 4:a629f46d6594 155 // printf("control: %d\r\n",control);
kangmingyo 2:01227a4bd3d7 156 }else{
kangmingyo 4:a629f46d6594 157 duty =(-TargetSpeed-control)/200.0;
kangmingyo 4:a629f46d6594 158 _pwm.write(duty);
kangmingyo 2:01227a4bd3d7 159 _Dir=0;
kangmingyo 0:20e026e254d6 160 }
kangmingyo 3:df8469e71728 161
kangmingyo 4:a629f46d6594 162
kangmingyo 3:df8469e71728 163
kangmingyo 4:a629f46d6594 164 // printf("Duty: %d vel: %f\r\n",duty,vel);
kangmingyo 4:a629f46d6594 165
kangmingyo 3:df8469e71728 166
kangmingyo 0:20e026e254d6 167
kangmingyo 0:20e026e254d6 168
kangmingyo 0:20e026e254d6 169
kangmingyo 0:20e026e254d6 170
kangmingyo 0:20e026e254d6 171
kangmingyo 0:20e026e254d6 172 }
kangmingyo 0:20e026e254d6 173
kangmingyo 0:20e026e254d6 174 float MotorCtl::CalculateRPM(int DeltaCnt)
kangmingyo 0:20e026e254d6 175 {
kangmingyo 3:df8469e71728 176
kangmingyo 4:a629f46d6594 177 return(DeltaCnt*60.0*1000.0)/(float)(DeltaT*CntPerRev); // Time measured in us
kangmingyo 0:20e026e254d6 178 }
kangmingyo 0:20e026e254d6 179
kangmingyo 0:20e026e254d6 180
kangmingyo 0:20e026e254d6 181