Statistics
| Branch: | Tag: | Revision:

amiro-os / components / MotorControl.cpp @ 19a69797

History | View | Annotate | Download (17.2 KB)

1
#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*) &global.sercanmux1, "motorCalibrationFactor =   %f  \n" ,this->motorCalibrationFactor);
236
    chprintf((BaseSequentialStream*) &global.sercanmux1, "rw =   %i  \n" ,this->rightWValues[0]);
237
    chprintf((BaseSequentialStream*) &global.sercanmux1, "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*) &global.sercanmux1, "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*) &global.sercanmux1, "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*) &global.sercanmux1, "pgain =   %i  \n" ,this->pGain);
294
           chprintf((BaseSequentialStream*) &global.sercanmux1, "igain =   %f  \n" ,this->iGain);
295
           chprintf((BaseSequentialStream*) &global.sercanmux1, "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
   
318
    int diffv = motorEnable ? this->targetVelocity.x - this->currentVelocity.x : 0;
319
    int diffw = motorEnable ? this->targetVelocity.w_z - this->currentVelocity.w_z: 0;
320
    chSysUnlock();
321

    
322
    //igain ####################################
323
   this->accumulatedErrorV += diffv;
324
   this->accumulatedErrorW += diffw;
325

    
326
    if (this->accumulatedErrorV > this->antiWindupV)
327
      this->accumulatedErrorV = this->antiWindupV;
328
    else if (this->accumulatedErrorV < -this->antiWindupV)
329
      this->accumulatedErrorV = -this->antiWindupV;
330

    
331
    if (this->accumulatedErrorW > this->antiWindupW)
332
      this->accumulatedErrorW = this->antiWindupW;
333
    else if (this->accumulatedErrorW < -this->antiWindupW)
334
      this->accumulatedErrorW = -this->antiWindupW;
335

    
336
    diffv += (int) (this->accumulatedErrorV*this->iGain);
337
    diffw += (int) (this->accumulatedErrorW*this->iGain);
338

    
339
    //dgain ###################################
340
    int derivativeV;
341
    int derivativeW;
342
    int tmp1;
343
    int tmp2;
344

    
345
   tmp1 = static_cast<int32_t>((lastVelocitiesV[0]+lastVelocitiesV[1]+lastVelocitiesV[2])/3);
346
   tmp2 = static_cast<int32_t>((lastVelocitiesV[3]+lastVelocitiesV[4]+lastVelocitiesV[5])/3);
347
   derivativeV = static_cast<int32_t> ((tmp2-tmp1)/(int)(this->period));
348
   tmp1 = static_cast<int32_t>((lastVelocitiesW[0]+lastVelocitiesW[1]+lastVelocitiesW[2])/3);
349
   tmp2 = static_cast<int32_t>((lastVelocitiesW[3]+lastVelocitiesW[4]+lastVelocitiesW[5])/3);
350
   derivativeW = static_cast<int32_t> ((tmp2-tmp1)/(int)(this->period));
351

    
352

    
353
    diffv += (int) (dGain*derivativeV);
354
    diffw += (int) (dGain*derivativeW);
355

    
356
    setLeftWheelSpeed(diffv,diffw);
357
    setRightWheelSpeed(diffv, diffw);
358
}
359

    
360

    
361

    
362
void MotorControl::setLeftWheelSpeed(int diffv, int diffw){
363
    this->pwmPercentage[LEFT_WHEEL] = (int) (this->pGain*2*(diffv-0.5*diffw*wheelBaseDistanceSI*this->Eb)/(wheelDiameter*this->wheelDiameterCorrectionFactor[LEFT_WHEEL]));
364

    
365
}
366

    
367
void MotorControl::setRightWheelSpeed(int diffv, int diffw){
368
    this->pwmPercentage[RIGHT_WHEEL] = (int) (motorCalibrationFactor*this->pGain*2*(diffv+0.5*diffw*wheelBaseDistanceSI*this->Eb)/(wheelDiameter*this->wheelDiameterCorrectionFactor[RIGHT_WHEEL]));
369
}
370

    
371

    
372
// speed in um
373
int MotorControl::getRightWheelSpeed(){
374
    int omega = 2*M_PI*this->actualSpeed[RIGHT_WHEEL]/60; // syslock noetig ? todo
375
    return  omega * wheelDiameter*this->wheelDiameterCorrectionFactor[RIGHT_WHEEL];
376
}
377
// speed in um
378
int MotorControl::getLeftWheelSpeed(){
379
    int omega = 2*M_PI*this->actualSpeed[LEFT_WHEEL]/60;
380
    return  omega * wheelDiameter*this->wheelDiameterCorrectionFactor[LEFT_WHEEL];
381
}
382

    
383

    
384
void MotorControl::updatePastVelocities(){
385
    for (int i=0; i<numberOfLastVelocitiesV-1;i++){
386
        lastVelocitiesVBig[i] = lastVelocitiesVBig[i+1];
387
    }
388

    
389
    lastVelocitiesVBig[numberOfLastVelocitiesV-1] = this->currentVelocity.x;
390

    
391
}
392

    
393

    
394
void MotorControl::updateDerivativeLastVelocities(){
395
    for (int i=0; i<5;i++){
396
        lastVelocitiesV[i] = lastVelocitiesV[i+1];
397
        lastVelocitiesW[i] = lastVelocitiesW[i+1];
398
    }
399

    
400

    
401
    lastVelocitiesV[5] = this->currentVelocity.x;
402
    lastVelocitiesW[5] = this->currentVelocity.w_z;
403

    
404

    
405
}
406

    
407
int MotorControl::getNumberofSignChanges(){
408
    int nsc= 0;
409
    bool ispositive = true;
410
    bool tmpbool = true;
411
    if (lastVelocitiesVBig[0] < 0){
412
        ispositive =false;
413
        tmpbool =false;
414
    }
415
    for (int i=0; i<numberOfLastVelocitiesV-1; i++){
416
        if (lastVelocitiesVBig[i] < 0){
417
            ispositive= false;
418
        } else {
419
            ispositive = true;
420
        }
421
        if (ispositive != tmpbool){
422
            nsc++;
423
            tmpbool = ispositive;
424
        }
425

    
426
    }
427

    
428
    return nsc;
429
}
430

    
431
void MotorControl::printGains(){
432
    chprintf((BaseSequentialStream*)&global.sercanmux1, "motorCalibrationFactor %f\n", this->motorCalibrationFactor );
433
    chprintf((BaseSequentialStream*)&global.sercanmux1, "pGain %i\n", this->pGain );
434
    chprintf((BaseSequentialStream*)&global.sercanmux1, "iGain %f\n", this->iGain );
435
    chprintf((BaseSequentialStream*)&global.sercanmux1, "dGain %f\n", this->dGain );
436
}
437

    
438
void MotorControl::resetGains()
439
{
440
  // reset factors
441
  chSysLock();
442
  this->motorCalibrationFactor = 1.0f;
443
  this->pGain = 1000;
444
  this->iGain = 0.08f;
445
  this->dGain = 0.01f;
446
  chSysUnlock();
447

    
448
  // write reset factors to memory
449
  this->memory->setwheelfactor(this->motorCalibrationFactor);
450
  this->memory->setpGain(this->pGain);
451
  this->memory->setiGain(this->iGain);
452
  this->memory->setdGain(this->dGain);
453

    
454
  return;
455
}
456

    
457

    
458

    
459
void MotorControl::calcVelocity() {
460
  // Velocity in µm/s in x direction
461
  currentVelocity.x = (1.0f*wheelCircumference * (this->actualSpeed[LEFT_WHEEL] + this->actualSpeed[RIGHT_WHEEL])) / secondsPerMinute / 2.0f;
462
  // Angular velocity around z in µrad/s
463
  currentVelocity.w_z = (1.0f*wheelCircumference * (this->actualSpeed[RIGHT_WHEEL] - this->actualSpeed[LEFT_WHEEL])) / (1.0f*secondsPerMinute) / (wheelBaseDistanceSI*this->Eb);
464

    
465
}
466

    
467

    
468

    
469
void MotorControl::updateIncrements(MotorIncrements* motorIncrements, int32_t (&oldIncrement)[2], float (&incrementDifference)[2]) {
470
  int32_t currentIncrement[2];
471

    
472
  chSysLock();
473
  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
474
    currentIncrement[idxWheel] = motorIncrements->qeiGetPosition(idxWheel);
475
  }
476
  chSysUnlock();
477

    
478
  // Calculate the difference between the last and
479
  // actual increments and therefor the actual speed or distance
480
  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
481

    
482
    // Get the difference
483
    int32_t tmpIncrementDifference =  oldIncrement[idxWheel] - currentIncrement[idxWheel];
484

    
485
    // Handle overflow of increments
486
    int range = motorIncrements->getQeiConfigRange();
487
    if (tmpIncrementDifference > (range >> 1))
488
      tmpIncrementDifference -= motorIncrements->getQeiConfigRange();
489
    else if (tmpIncrementDifference < -(range >> 1))
490
      tmpIncrementDifference += motorIncrements->getQeiConfigRange();
491

    
492
    // Correct the difference
493
    incrementDifference[idxWheel] =  static_cast<float>(tmpIncrementDifference) * MotorControl::wheelDiameterCorrectionFactor[idxWheel];
494

    
495
    // Save the actual increments
496
    oldIncrement[idxWheel] = currentIncrement[idxWheel];
497
  }
498
}
499

    
500
void MotorControl::updateSpeed(const float (&incrementDifference)[2], int32_t (&actualSpeed)[2], const uint32_t period) {
501
  const int32_t updatesPerMinute = 60 * 1000 / period;
502

    
503
  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
504
    // Save the actual speed
505
    actualSpeed[idxWheel] = static_cast<int32_t>(static_cast<float>(updatesPerMinute * incrementDifference[idxWheel])) / incrementsPerRevolution;
506
  }
507
}
508

    
509
void MotorControl::updateDistance(const float (&incrementDifference)[2], float (&actualDistance)[2]) {
510

    
511
  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
512
    // Calc. the distance per wheel in meter
513
    actualDistance[idxWheel] = wheelCircumferenceSI * incrementDifference[idxWheel] / static_cast<float>(incrementsPerRevolution);
514
  }
515
}
516

    
517
void MotorControl::writePulseWidthModulation() {
518
  for (int idxWheel = 0; idxWheel < 2; idxWheel++) {
519
    int percentage = this->pwmPercentage[idxWheel];
520
    unsigned int widths[2];
521

    
522
    // 10000 is the max. duty cicle
523
    if (percentage > 10000) {
524
      percentage = 10000;
525
    } else if (percentage < -10000) {
526
      percentage = -10000;
527
    }
528

    
529
    if (percentage < 0) {
530
      widths[0] = 0;
531
      widths[1] = PWM_PERCENTAGE_TO_WIDTH(this->pwmDriver, -percentage);
532
    } else {
533
      widths[0] = PWM_PERCENTAGE_TO_WIDTH(this->pwmDriver, percentage);
534
      widths[1] = 0;
535
    }
536

    
537
    for (int idxPWM = 0; idxPWM < 2; idxPWM++)
538
      pwmEnableChannel(this->pwmDriver, (idxWheel * 2) + idxPWM,widths[idxPWM]);
539
  }
540
}
541

    
542

    
543

    
544
void MotorControl::setMotorEnable(bool enable){
545
  if (this->motorEnable != enable){
546
    this->accumulatedErrorV = 0;
547
    this->accumulatedErrorW = 0;
548
    this->motorEnable = enable;
549
  }
550

    
551
}
552

    
553
bool MotorControl::getMotorEnable(){
554
  return this->motorEnable;
555
}