amiro-os / components / MotorControl.cpp @ ff7ad65b
History | View | Annotate | Download (16.7 KB)
1 | 58fe0e0b | Thomas Schöpping | #include <ch.hpp> |
---|---|---|---|
2 | #include <hal.h> |
||
3 | |||
4 | #include <qei.h> |
||
5 | #include <chprintf.h> |
||
6 | #include <amiro/MotorControl.h> |
||
7 | #include <global.hpp> |
||
8 | |||
9 | using namespace chibios_rt; |
||
10 | using namespace amiro; |
||
11 | using namespace types; |
||
12 | using namespace constants; |
||
13 | using namespace constants::DiWheelDrive; |
||
14 | |||
15 | float MotorControl::wheelDiameterCorrectionFactor[2] = {1.0f, 1.0f}; |
||
16 | float MotorControl::actualWheelBaseDistanceSI = wheelBaseDistanceSI;
|
||
17 | extern Global global;
|
||
18 | |||
19 | MotorControl::MotorControl(PWMDriver* pwm, MotorIncrements* mi, GPIO_TypeDef* port, int pad, fileSystemIo::FSIODiWheelDrive *memory)
|
||
20 | : BaseStaticThread<512>(),
|
||
21 | pwmDriver(pwm), |
||
22 | motorIncrements(mi), |
||
23 | powerEnablePort(port), |
||
24 | powerEnablePad(pad), |
||
25 | eventSource(), |
||
26 | period(10),
|
||
27 | memory(memory) { |
||
28 | |||
29 | this->pwmConfig.frequency = 7200000; |
||
30 | this->pwmConfig.period = 360; |
||
31 | this->pwmConfig.callback = NULL; |
||
32 | this->pwmConfig.channels[0].mode = PWM_OUTPUT_ACTIVE_HIGH; |
||
33 | this->pwmConfig.channels[0].callback = NULL; |
||
34 | this->pwmConfig.channels[1].mode = PWM_OUTPUT_ACTIVE_HIGH; |
||
35 | this->pwmConfig.channels[1].callback = NULL; |
||
36 | this->pwmConfig.channels[2].mode = PWM_OUTPUT_ACTIVE_HIGH; |
||
37 | this->pwmConfig.channels[2].callback = NULL; |
||
38 | this->pwmConfig.channels[3].mode = PWM_OUTPUT_ACTIVE_HIGH; |
||
39 | this->pwmConfig.channels[3].callback = NULL; |
||
40 | this->pwmConfig.cr2 = 0; |
||
41 | |||
42 | this->increment[0] = 0; |
||
43 | this->increment[1] = 0; |
||
44 | |||
45 | this->errorSum[0] = 0; |
||
46 | this->errorSum[1] = 0; |
||
47 | |||
48 | this->errorSumDiff = 0; |
||
49 | |||
50 | // Init the velocities
|
||
51 | this->currentVelocity.x = 0; |
||
52 | this->currentVelocity.y = 0; |
||
53 | this->currentVelocity.z = 0; |
||
54 | this->currentVelocity.w_x = 0; |
||
55 | this->currentVelocity.w_y = 0; |
||
56 | this->currentVelocity.w_z = 0; |
||
57 | this->targetVelocity.x = 0; |
||
58 | this->targetVelocity.w_z = 0; |
||
59 | |||
60 | this->newTargetVelocities = false; |
||
61 | this->lastVelocitiesV[0] = 0; |
||
62 | this->lastVelocitiesV[1] = 0; |
||
63 | this->lastVelocitiesV[2] = 0; |
||
64 | this->lastVelocitiesV[3] = 0; |
||
65 | this->lastVelocitiesV[4] = 0; |
||
66 | this->lastVelocitiesV[5] = 0; |
||
67 | |||
68 | this->newTargetVelocities = true; |
||
69 | |||
70 | // calibration stuff;
|
||
71 | for (int i =0; i<3; i++){ |
||
72 | this->leftWValues[i] = i;
|
||
73 | this->rightWValues[i] = i;
|
||
74 | } |
||
75 | |||
76 | } |
||
77 | |||
78 | int MotorControl::getCurrentRPMLeft() {
|
||
79 | return this->actualSpeed[LEFT_WHEEL]; |
||
80 | } |
||
81 | |||
82 | int MotorControl::getCurrentRPMRight() {
|
||
83 | return this->actualSpeed[RIGHT_WHEEL]; |
||
84 | } |
||
85 | |||
86 | kinematic MotorControl::getCurrentVelocity() { |
||
87 | return this->currentVelocity; |
||
88 | } |
||
89 | |||
90 | msg_t MotorControl::setWheelDiameterCorrectionFactor(float Ed /* = 1.0f */, bool_t storeEdToMemory /* = false */) { |
||
91 | // cl (Eq. 17a)
|
||
92 | MotorControl::wheelDiameterCorrectionFactor[LEFT_WHEEL] = 2.0f / (Ed + 1.0f); |
||
93 | // cl (Eq. 17a)
|
||
94 | MotorControl::wheelDiameterCorrectionFactor[RIGHT_WHEEL] = 2.0f / ((1.0f / Ed) + 1.0f); |
||
95 | // Store Ed to memory
|
||
96 | if (storeEdToMemory)
|
||
97 | return memory->setEd(Ed);
|
||
98 | else
|
||
99 | return RDY_OK;
|
||
100 | } |
||
101 | |||
102 | msg_t MotorControl::setActualWheelBaseDistance(float Eb /* = 1.0f */, bool_t storeEbToMemory /* = false */) { |
||
103 | // bActual (Eq. 4)
|
||
104 | MotorControl::actualWheelBaseDistanceSI = wheelBaseDistanceSI * Eb; |
||
105 | // Store Eb to memory
|
||
106 | if (storeEbToMemory)
|
||
107 | return memory->setEb(Eb);
|
||
108 | else
|
||
109 | return RDY_OK;
|
||
110 | } |
||
111 | |||
112 | EvtSource* MotorControl::getEventSource() { |
||
113 | return &this->eventSource; |
||
114 | } |
||
115 | |||
116 | void MotorControl::setTargetRPM(int32_t targetURpmLeft, int32_t targetURpmRight) {
|
||
117 | // Velocity in µm/s in x direction
|
||
118 | int32_t targetVelocityX = (wheelCircumferenceSI * (targetURpmLeft + targetURpmRight)) / secondsPerMinute / 2.0f; |
||
119 | // Angular velocity around z in µrad/s
|
||
120 | int32_t targetVelocityWz = (wheelCircumferenceSI * (targetURpmRight - targetURpmLeft)) / secondsPerMinute / MotorControl::actualWheelBaseDistanceSI; |
||
121 | chSysLock(); |
||
122 | this->targetVelocity.x = targetVelocityX;
|
||
123 | this->targetVelocity.w_z = targetVelocityWz;
|
||
124 | this->newTargetVelocities = true; |
||
125 | chSysUnlock(); |
||
126 | } |
||
127 | |||
128 | void MotorControl::setTargetSpeed(const kinematic &targetVelocity) { |
||
129 | chSysLock(); |
||
130 | this->targetVelocity.x = targetVelocity.x;
|
||
131 | this->targetVelocity.w_z = targetVelocity.w_z;
|
||
132 | this->newTargetVelocities = true; |
||
133 | chSysUnlock(); |
||
134 | } |
||
135 | |||
136 | msg_t MotorControl::main(void) {
|
||
137 | systime_t time = System::getTime(); |
||
138 | this->setName("MotorControl"); |
||
139 | |||
140 | // load controller parameters from memory
|
||
141 | this->memory->getWheelFactor(&this->motorCalibrationFactor); |
||
142 | this->memory->getpGain(&this->pGain); |
||
143 | this->memory->getiGain(&this->iGain); |
||
144 | this->memory->getdGain(&this->dGain); |
||
145 | this->memory->getEb(&this->Eb); |
||
146 | this->memory->getEd(&this->Ed); |
||
147 | |||
148 | pwmStart(this->pwmDriver, &this->pwmConfig); |
||
149 | |||
150 | palSetPad(this->powerEnablePort, this->powerEnablePad); |
||
151 | |||
152 | while (!this->shouldTerminate()) { |
||
153 | time += MS2ST(this->period);
|
||
154 | |||
155 | // Get the increments from the QEI
|
||
156 | MotorControl::updateIncrements(this->motorIncrements, this->increment, this->incrementDifference); |
||
157 | |||
158 | // Get the actual speed from the gathered increments
|
||
159 | MotorControl::updateSpeed(this->incrementDifference, this->actualSpeed, this->period); |
||
160 | |||
161 | // Calculate velocities
|
||
162 | this->calcVelocity();
|
||
163 | |||
164 | // updates past velocities for the derivate part of the controller
|
||
165 | updateDerivativeLastVelocities(); |
||
166 | |||
167 | controllerAndCalibrationLogic(); |
||
168 | |||
169 | // Write the calculated duty cycle to the pwm driver
|
||
170 | this->writePulseWidthModulation();
|
||
171 | |||
172 | chThdSleepUntil(time); |
||
173 | |||
174 | delay ++; |
||
175 | if (delay > 50){ |
||
176 | delay = 0;
|
||
177 | } |
||
178 | |||
179 | } |
||
180 | |||
181 | // Reset the PWM befor shutdown
|
||
182 | this->pwmPercentage[LEFT_WHEEL] = 0; |
||
183 | this->pwmPercentage[RIGHT_WHEEL] = 0; |
||
184 | this->writePulseWidthModulation();
|
||
185 | |||
186 | return true; |
||
187 | } |
||
188 | |||
189 | void MotorControl::controllerAndCalibrationLogic(){
|
||
190 | if (!isCalibrating){
|
||
191 | this->PIDController();
|
||
192 | startedZieglerCalibration = true;
|
||
193 | startedWheelCalibration = true;
|
||
194 | } else {
|
||
195 | if (motorCalibration){
|
||
196 | if (startedWheelCalibration){
|
||
197 | wheelCalibrationTime = System::getTime(); |
||
198 | startedWheelCalibration = false;
|
||
199 | } |
||
200 | calibrate(); |
||
201 | } else {
|
||
202 | this->PIDController();
|
||
203 | this->calibrateZiegler();
|
||
204 | this->updatePastVelocities();
|
||
205 | |||
206 | if (startedZieglerCalibration){
|
||
207 | zieglerCalibrationTime = System::getTime(); |
||
208 | startedZieglerCalibration = false;
|
||
209 | } |
||
210 | } |
||
211 | } |
||
212 | } |
||
213 | |||
214 | void MotorControl::calibrate() {
|
||
215 | |||
216 | this->pwmPercentage[LEFT_WHEEL] = 3000; |
||
217 | this->pwmPercentage[RIGHT_WHEEL] = 3000; |
||
218 | |||
219 | this->rightWValues[0] = this->rightWValues[1]; |
||
220 | this->rightWValues[1] = this->rightWValues[2]; |
||
221 | this->rightWValues[2] = getRightWheelSpeed(); |
||
222 | |||
223 | this->leftWValues[0] = this->leftWValues[1]; |
||
224 | this->leftWValues[1] = this->leftWValues[2]; |
||
225 | this->leftWValues[2] = getLeftWheelSpeed(); |
||
226 | |||
227 | |||
228 | if (this->rightWValues[0] == this->rightWValues[1] && |
||
229 | this->rightWValues[1] == this->rightWValues[2] && |
||
230 | this->leftWValues[0] == this->leftWValues[1] && |
||
231 | this->leftWValues[1] == this->leftWValues[2] && |
||
232 | System::getTime() - this->wheelCalibrationTime > 1500) { |
||
233 | this->motorCalibrationFactor = (float)this->rightWValues[0]/(float)this->leftWValues[0]; |
||
234 | |||
235 | chprintf((BaseSequentialStream*) &SD1, "motorCalibrationFactor = %f \n" ,this->motorCalibrationFactor); |
||
236 | chprintf((BaseSequentialStream*) &SD1, "rw = %i \n" ,this->rightWValues[0]); |
||
237 | chprintf((BaseSequentialStream*) &SD1, "lw = %i \n" ,this->leftWValues[0]); |
||
238 | |||
239 | this->pwmPercentage[LEFT_WHEEL] = 0; |
||
240 | this->pwmPercentage[RIGHT_WHEEL] = 0; |
||
241 | this->motorCalibration = false; |
||
242 | this->memory->setwheelfactor(motorCalibrationFactor);
|
||
243 | } |
||
244 | } |
||
245 | |||
246 | void MotorControl::calibrateZiegler() {
|
||
247 | |||
248 | this->iGain =0; |
||
249 | this->dGain = 0; |
||
250 | int nsc = this->getNumberofSignChanges(); |
||
251 | |||
252 | if (System::getTime() - this->zieglerCalibrationTime > 1000){ |
||
253 | this->zieglerCalibrationTime = System::getTime() ;
|
||
254 | this->ziegler2 =true; |
||
255 | } |
||
256 | |||
257 | if (ziegler && ziegler2){
|
||
258 | this->targetVelocity.x = 200000 * ((zieglerHelp%2 == 0) ? 1 : -1); |
||
259 | this->pGain = 1000 + 100 * zieglerHelp; |
||
260 | chprintf((BaseSequentialStream*) &SD1, "pgain = %i \n" , this->pGain); |
||
261 | zieglerHelp++; |
||
262 | ziegler = false;
|
||
263 | ziegler2 = false;
|
||
264 | } |
||
265 | |||
266 | if (!ziegler && ziegler2){
|
||
267 | this->targetVelocity.x = 0; |
||
268 | ziegler2= false;
|
||
269 | ziegler = true;
|
||
270 | } |
||
271 | |||
272 | if (zieglerHelp > 20){ |
||
273 | this->isCalibrating = false; |
||
274 | this->targetVelocity.x = 0; |
||
275 | this->iGain = 0.08; |
||
276 | this->pGain = 1000; |
||
277 | } |
||
278 | |||
279 | |||
280 | if ( nsc > 8){ |
||
281 | zieglerHelp2++; |
||
282 | if (zieglerHelp2 > 20){ |
||
283 | this->zieglerPeriod = numberOfLastVelocitiesV * this->period / nsc; |
||
284 | chprintf((BaseSequentialStream*) &SD1, "zieglerPeriod = %f \n" ,this->zieglerPeriod); |
||
285 | |||
286 | this->targetVelocity.x = 0; |
||
287 | this->pGain = (int) (this->pGain* 0.6); |
||
288 | this->iGain = (float) ((1/(0.5*(this->zieglerPeriod/1000))/this->pGain)); |
||
289 | this->dGain = (float) ((1/(0.125*(this->zieglerPeriod/1000))/this->pGain)); |
||
290 | this->memory->setpGain(this->pGain); |
||
291 | this->memory->setiGain(this->iGain); |
||
292 | this->memory->setdGain(this->dGain); |
||
293 | chprintf((BaseSequentialStream*) &SD1, "pgain = %i \n" ,this->pGain); |
||
294 | chprintf((BaseSequentialStream*) &SD1, "igain = %f \n" ,this->iGain); |
||
295 | chprintf((BaseSequentialStream*) &SD1, "dgain = %f \n" ,this->dGain); |
||
296 | |||
297 | this->motorCalibration = true; |
||
298 | ziegler = true;
|
||
299 | ziegler2 = true;
|
||
300 | this->isCalibrating = false; |
||
301 | zieglerHelp = 0;
|
||
302 | zieglerCalibrationTime = 0;
|
||
303 | zieglerHelp2 = 0;
|
||
304 | |||
305 | for (int i = 0; i< numberOfLastVelocitiesV ; i ++){ |
||
306 | lastVelocitiesVBig[i] = 0;
|
||
307 | } |
||
308 | } |
||
309 | } |
||
310 | |||
311 | } |
||
312 | |||
313 | void MotorControl::PIDController(){
|
||
314 | |||
315 | //pgain #####################################
|
||
316 | chSysLock(); |
||
317 | int diffv =this->targetVelocity.x - this->currentVelocity.x; |
||
318 | int diffw = this->targetVelocity.w_z - this->currentVelocity.w_z; |
||
319 | chSysUnlock(); |
||
320 | |||
321 | //igain ####################################
|
||
322 | this->accumulatedErrorV += diffv;
|
||
323 | this->accumulatedErrorW += diffw;
|
||
324 | |||
325 | if (this->accumulatedErrorV > this->antiWindupV) |
||
326 | this->accumulatedErrorV = this->antiWindupV; |
||
327 | else if (this->accumulatedErrorV < -this->antiWindupV) |
||
328 | this->accumulatedErrorV = -this->antiWindupV; |
||
329 | |||
330 | if (this->accumulatedErrorW > this->antiWindupW) |
||
331 | this->accumulatedErrorW = this->antiWindupW; |
||
332 | else if (this->accumulatedErrorW < -this->antiWindupW) |
||
333 | this->accumulatedErrorW = -this->antiWindupW; |
||
334 | |||
335 | diffv += (int) (this->accumulatedErrorV*this->iGain); |
||
336 | diffw += (int) (this->accumulatedErrorW*this->iGain); |
||
337 | |||
338 | //dgain ###################################
|
||
339 | int derivativeV;
|
||
340 | int derivativeW;
|
||
341 | int tmp1;
|
||
342 | int tmp2;
|
||
343 | |||
344 | tmp1 = static_cast<int32_t>((lastVelocitiesV[0]+lastVelocitiesV[1]+lastVelocitiesV[2])/3); |
||
345 | tmp2 = static_cast<int32_t>((lastVelocitiesV[3]+lastVelocitiesV[4]+lastVelocitiesV[5])/3); |
||
346 | derivativeV = static_cast<int32_t> ((tmp2-tmp1)/(int)(this->period)); |
||
347 | tmp1 = static_cast<int32_t>((lastVelocitiesW[0]+lastVelocitiesW[1]+lastVelocitiesW[2])/3); |
||
348 | tmp2 = static_cast<int32_t>((lastVelocitiesW[3]+lastVelocitiesW[4]+lastVelocitiesW[5])/3); |
||
349 | derivativeW = static_cast<int32_t> ((tmp2-tmp1)/(int)(this->period)); |
||
350 | |||
351 | |||
352 | diffv += (int) (dGain*derivativeV);
|
||
353 | diffw += (int) (dGain*derivativeW);
|
||
354 | |||
355 | setLeftWheelSpeed(diffv,diffw); |
||
356 | setRightWheelSpeed(diffv, diffw); |
||
357 | } |
||
358 | |||
359 | |||
360 | |||
361 | void MotorControl::setLeftWheelSpeed(int diffv, int diffw){ |
||
362 | this->pwmPercentage[LEFT_WHEEL] = (int) (this->pGain*2*(diffv-0.5*diffw*wheelBaseDistanceSI*this->Eb)/(wheelDiameter*this->wheelDiameterCorrectionFactor[LEFT_WHEEL])); |
||
363 | |||
364 | } |
||
365 | |||
366 | void MotorControl::setRightWheelSpeed(int diffv, int diffw){ |
||
367 | this->pwmPercentage[RIGHT_WHEEL] = (int) (motorCalibrationFactor*this->pGain*2*(diffv+0.5*diffw*wheelBaseDistanceSI*this->Eb)/(wheelDiameter*this->wheelDiameterCorrectionFactor[RIGHT_WHEEL])); |
||
368 | } |
||
369 | |||
370 | |||
371 | // speed in um
|
||
372 | int MotorControl::getRightWheelSpeed(){
|
||
373 | int omega = 2*M_PI*this->actualSpeed[RIGHT_WHEEL]/60; // syslock noetig ? todo |
||
374 | return omega * wheelDiameter*this->wheelDiameterCorrectionFactor[RIGHT_WHEEL]; |
||
375 | } |
||
376 | // speed in um
|
||
377 | int MotorControl::getLeftWheelSpeed(){
|
||
378 | int omega = 2*M_PI*this->actualSpeed[LEFT_WHEEL]/60; |
||
379 | return omega * wheelDiameter*this->wheelDiameterCorrectionFactor[LEFT_WHEEL]; |
||
380 | } |
||
381 | |||
382 | |||
383 | void MotorControl::updatePastVelocities(){
|
||
384 | for (int i=0; i<numberOfLastVelocitiesV-1;i++){ |
||
385 | lastVelocitiesVBig[i] = lastVelocitiesVBig[i+1];
|
||
386 | } |
||
387 | |||
388 | lastVelocitiesVBig[numberOfLastVelocitiesV-1] = this->currentVelocity.x; |
||
389 | |||
390 | } |
||
391 | |||
392 | |||
393 | void MotorControl::updateDerivativeLastVelocities(){
|
||
394 | for (int i=0; i<5;i++){ |
||
395 | lastVelocitiesV[i] = lastVelocitiesV[i+1];
|
||
396 | lastVelocitiesW[i] = lastVelocitiesW[i+1];
|
||
397 | } |
||
398 | |||
399 | |||
400 | lastVelocitiesV[5] = this->currentVelocity.x; |
||
401 | lastVelocitiesW[5] = this->currentVelocity.w_z; |
||
402 | |||
403 | |||
404 | } |
||
405 | |||
406 | int MotorControl::getNumberofSignChanges(){
|
||
407 | int nsc= 0; |
||
408 | bool ispositive = true; |
||
409 | bool tmpbool = true; |
||
410 | if (lastVelocitiesVBig[0] < 0){ |
||
411 | ispositive =false;
|
||
412 | tmpbool =false;
|
||
413 | } |
||
414 | for (int i=0; i<numberOfLastVelocitiesV-1; i++){ |
||
415 | if (lastVelocitiesVBig[i] < 0){ |
||
416 | ispositive= false;
|
||
417 | } else {
|
||
418 | ispositive = true;
|
||
419 | } |
||
420 | if (ispositive != tmpbool){
|
||
421 | nsc++; |
||
422 | tmpbool = ispositive; |
||
423 | } |
||
424 | |||
425 | } |
||
426 | |||
427 | return nsc;
|
||
428 | } |
||
429 | |||
430 | void MotorControl::printGains(){
|
||
431 | chprintf((BaseSequentialStream*)&SD1, "motorCalibrationFactor %f\n", this->motorCalibrationFactor ); |
||
432 | chprintf((BaseSequentialStream*)&SD1, "pGain %i\n", this->pGain ); |
||
433 | chprintf((BaseSequentialStream*)&SD1, "iGain %f\n", this->iGain ); |
||
434 | chprintf((BaseSequentialStream*)&SD1, "dGain %f\n", this->dGain ); |
||
435 | } |
||
436 | |||
437 | ff7ad65b | Thomas Schöpping | void MotorControl::resetGains()
|
438 | { |
||
439 | // reset factors
|
||
440 | chSysLock(); |
||
441 | this->motorCalibrationFactor = 1.0f; |
||
442 | this->pGain = 1000; |
||
443 | this->iGain = 0.08f; |
||
444 | this->dGain = 0.01f; |
||
445 | chSysUnlock(); |
||
446 | |||
447 | // write reset factors to memory
|
||
448 | this->memory->setwheelfactor(this->motorCalibrationFactor); |
||
449 | this->memory->setpGain(this->pGain); |
||
450 | this->memory->setiGain(this->iGain); |
||
451 | this->memory->setdGain(this->dGain); |
||
452 | |||
453 | return;
|
||
454 | } |
||
455 | |||
456 | 58fe0e0b | Thomas Schöpping | |
457 | |||
458 | void MotorControl::calcVelocity() {
|
||
459 | // Velocity in µm/s in x direction
|
||
460 | currentVelocity.x = (1.0f*wheelCircumference * (this->actualSpeed[LEFT_WHEEL] + this->actualSpeed[RIGHT_WHEEL])) / secondsPerMinute / 2.0f; |
||
461 | // Angular velocity around z in µrad/s
|
||
462 | currentVelocity.w_z = (1.0f*wheelCircumference * (this->actualSpeed[RIGHT_WHEEL] - this->actualSpeed[LEFT_WHEEL])) / (1.0f*secondsPerMinute) / (wheelBaseDistanceSI*this->Eb); |
||
463 | |||
464 | } |
||
465 | |||
466 | |||
467 | |||
468 | void MotorControl::updateIncrements(MotorIncrements* motorIncrements, int32_t (&oldIncrement)[2], float (&incrementDifference)[2]) { |
||
469 | int32_t currentIncrement[2];
|
||
470 | |||
471 | chSysLock(); |
||
472 | for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) { |
||
473 | currentIncrement[idxWheel] = motorIncrements->qeiGetPosition(idxWheel); |
||
474 | } |
||
475 | chSysUnlock(); |
||
476 | |||
477 | // Calculate the difference between the last and
|
||
478 | // actual increments and therefor the actual speed or distance
|
||
479 | for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) { |
||
480 | |||
481 | // Get the difference
|
||
482 | int32_t tmpIncrementDifference = oldIncrement[idxWheel] - currentIncrement[idxWheel]; |
||
483 | |||
484 | // Handle overflow of increments
|
||
485 | int range = motorIncrements->getQeiConfigRange();
|
||
486 | if (tmpIncrementDifference > (range >> 1)) |
||
487 | tmpIncrementDifference -= motorIncrements->getQeiConfigRange(); |
||
488 | else if (tmpIncrementDifference < -(range >> 1)) |
||
489 | tmpIncrementDifference += motorIncrements->getQeiConfigRange(); |
||
490 | |||
491 | // Correct the difference
|
||
492 | incrementDifference[idxWheel] = static_cast<float>(tmpIncrementDifference) * MotorControl::wheelDiameterCorrectionFactor[idxWheel]; |
||
493 | |||
494 | // Save the actual increments
|
||
495 | oldIncrement[idxWheel] = currentIncrement[idxWheel]; |
||
496 | } |
||
497 | } |
||
498 | |||
499 | void MotorControl::updateSpeed(const float (&incrementDifference)[2], int32_t (&actualSpeed)[2], const uint32_t period) { |
||
500 | const int32_t updatesPerMinute = 60 * 1000 / period; |
||
501 | |||
502 | for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) { |
||
503 | // Save the actual speed
|
||
504 | actualSpeed[idxWheel] = static_cast<int32_t>(static_cast<float>(updatesPerMinute * incrementDifference[idxWheel])) / incrementsPerRevolution; |
||
505 | } |
||
506 | } |
||
507 | |||
508 | void MotorControl::updateDistance(const float (&incrementDifference)[2], float (&actualDistance)[2]) { |
||
509 | |||
510 | for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) { |
||
511 | // Calc. the distance per wheel in meter
|
||
512 | actualDistance[idxWheel] = wheelCircumferenceSI * incrementDifference[idxWheel] / static_cast<float>(incrementsPerRevolution); |
||
513 | } |
||
514 | } |
||
515 | |||
516 | void MotorControl::writePulseWidthModulation() {
|
||
517 | for (int idxWheel = 0; idxWheel < 2; idxWheel++) { |
||
518 | int percentage = this->pwmPercentage[idxWheel]; |
||
519 | unsigned int widths[2]; |
||
520 | |||
521 | // 10000 is the max. duty cicle
|
||
522 | if (percentage > 10000) { |
||
523 | percentage = 10000;
|
||
524 | } else if (percentage < -10000) { |
||
525 | percentage = -10000;
|
||
526 | } |
||
527 | |||
528 | if (percentage < 0) { |
||
529 | widths[0] = 0; |
||
530 | widths[1] = PWM_PERCENTAGE_TO_WIDTH(this->pwmDriver, -percentage); |
||
531 | } else {
|
||
532 | widths[0] = PWM_PERCENTAGE_TO_WIDTH(this->pwmDriver, percentage); |
||
533 | widths[1] = 0; |
||
534 | } |
||
535 | |||
536 | for (int idxPWM = 0; idxPWM < 2; idxPWM++) |
||
537 | pwmEnableChannel(this->pwmDriver, (idxWheel * 2) + idxPWM, widths[idxPWM]); |
||
538 | } |
||
539 | } |