Statistics
| Branch: | Tag: | Revision:

amiro-os / components / MotorControl.cpp @ 58fe0e0b

History | View | Annotate | Download (16.3 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*) &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

    
438

    
439
void MotorControl::calcVelocity() {
440
  // Velocity in µm/s in x direction
441
  currentVelocity.x = (1.0f*wheelCircumference * (this->actualSpeed[LEFT_WHEEL] + this->actualSpeed[RIGHT_WHEEL])) / secondsPerMinute / 2.0f;
442
  // Angular velocity around z in µrad/s
443
  currentVelocity.w_z = (1.0f*wheelCircumference * (this->actualSpeed[RIGHT_WHEEL] - this->actualSpeed[LEFT_WHEEL])) / (1.0f*secondsPerMinute) / (wheelBaseDistanceSI*this->Eb);
444

    
445
}
446

    
447

    
448

    
449
void MotorControl::updateIncrements(MotorIncrements* motorIncrements, int32_t (&oldIncrement)[2], float (&incrementDifference)[2]) {
450
  int32_t currentIncrement[2];
451

    
452
  chSysLock();
453
  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
454
    currentIncrement[idxWheel] = motorIncrements->qeiGetPosition(idxWheel);
455
  }
456
  chSysUnlock();
457

    
458
  // Calculate the difference between the last and
459
  // actual increments and therefor the actual speed or distance
460
  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
461

    
462
    // Get the difference
463
    int32_t tmpIncrementDifference =  oldIncrement[idxWheel] - currentIncrement[idxWheel];
464

    
465
    // Handle overflow of increments
466
    int range = motorIncrements->getQeiConfigRange();
467
    if (tmpIncrementDifference > (range >> 1))
468
      tmpIncrementDifference -= motorIncrements->getQeiConfigRange();
469
    else if (tmpIncrementDifference < -(range >> 1))
470
      tmpIncrementDifference += motorIncrements->getQeiConfigRange();
471

    
472
    // Correct the difference
473
    incrementDifference[idxWheel] =  static_cast<float>(tmpIncrementDifference) * MotorControl::wheelDiameterCorrectionFactor[idxWheel];
474

    
475
    // Save the actual increments
476
    oldIncrement[idxWheel] = currentIncrement[idxWheel];
477
  }
478
}
479

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

    
483
  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
484
    // Save the actual speed
485
    actualSpeed[idxWheel] = static_cast<int32_t>(static_cast<float>(updatesPerMinute * incrementDifference[idxWheel])) / incrementsPerRevolution;
486
  }
487
}
488

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

    
491
  for (uint8_t idxWheel = 0; idxWheel < 2; idxWheel++) {
492
    // Calc. the distance per wheel in meter
493
    actualDistance[idxWheel] = wheelCircumferenceSI * incrementDifference[idxWheel] / static_cast<float>(incrementsPerRevolution);
494
  }
495
}
496

    
497
void MotorControl::writePulseWidthModulation() {
498
  for (int idxWheel = 0; idxWheel < 2; idxWheel++) {
499
    int percentage = this->pwmPercentage[idxWheel];
500
    unsigned int widths[2];
501

    
502
    // 10000 is the max. duty cicle
503
    if (percentage > 10000) {
504
      percentage = 10000;
505
    } else if (percentage < -10000) {
506
      percentage = -10000;
507
    }
508

    
509
    if (percentage < 0) {
510
      widths[0] = 0;
511
      widths[1] = PWM_PERCENTAGE_TO_WIDTH(this->pwmDriver, -percentage);
512
    } else {
513
      widths[0] = PWM_PERCENTAGE_TO_WIDTH(this->pwmDriver, percentage);
514
      widths[1] = 0;
515
    }
516

    
517
    for (int idxPWM = 0; idxPWM < 2; idxPWM++)
518
      pwmEnableChannel(this->pwmDriver, (idxWheel * 2) + idxPWM, widths[idxPWM]);
519
  }
520
}