Pipe Inpection Robot / sn7544

motor.cpp@9:40d0087ec663, 2019-08-07 (annotated)

Committer:
kangmingyo
Date:
Wed Aug 07 13:39:14 2019 +0000
Revision:
9:40d0087ec663
Parent:
8:4fa7fadc583d 
190807;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kangmingyo 0:20e026e254d6 1 #include "motor.h"
kangmingyo 0:20e026e254d6 2
kangmingyo 9:40d0087ec663 3 MotorCtl::MotorCtl(PinName Pwm, BusOut &Dir, PinName tachoA, PinName tachoB):
kangmingyo 0:20e026e254d6 4 _pwm(Pwm), _Dir(Dir), _tachoA(tachoA), _tachoB(tachoB)
kangmingyo 0:20e026e254d6 5 {
kangmingyo 9:40d0087ec663 6 _Dir=0;
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
Jeonghoon 8:4fa7fadc583d 19 CntPerRev = 2800; // 100 shft/rev x 7 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 9:40d0087ec663 104 _Dir=2;
kangmingyo 0:20e026e254d6 105 } else {
kangmingyo 9:40d0087ec663 106 _Dir=1;
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 9:40d0087ec663 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 9:40d0087ec663 161 _Dir=2;
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 9:40d0087ec663 166 _Dir=1;
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 7:42c478f9a1fe 190 return getRPM()*3.14159265359*0.043/60;
kangmingyo 6:366ec55e64fe 191 }
kangmingyo 0:20e026e254d6 192
kangmingyo 0:20e026e254d6 193
kangmingyo 5:15d0276d7e21 194
kangmingyo 6:366ec55e64fe 195