Statistics
| Branch: | Tag: | Revision:

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
}