amiro-os / components / MotorControl.cpp @ 753ccd04
History | View | Annotate | Download (17.5 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 |
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*)&global.sercanmux1, "motorCalibrationFactor %f\n", this->motorCalibrationFactor ); |
432 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "pGain %i\n", this->pGain ); |
433 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "iGain %f\n", this->iGain ); |
434 |
chprintf((BaseSequentialStream*)&global.sercanmux1, "dGain %f\n", this->dGain ); |
435 |
} |
436 |
|
437 |
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 |
|
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, motorEnable ? widths[idxPWM] : 0); |
538 |
} |
539 |
} |
540 |
|
541 |
|
542 |
void MotorControl::setGains(motorGains *motorConfig){
|
543 |
chSysLock(); |
544 |
this->pGain = motorConfig->pGain;
|
545 |
this->iGain = motorConfig->iGain;
|
546 |
this->dGain = motorConfig->dGain;
|
547 |
chSysUnlock(); |
548 |
} |
549 |
|
550 |
void MotorControl::getGains(motorGains *motorConfig){
|
551 |
motorGains gains; |
552 |
motorConfig->pGain = this->pGain;
|
553 |
motorConfig->iGain = this->iGain;
|
554 |
motorConfig->dGain = this->dGain;
|
555 |
} |
556 |
|
557 |
void MotorControl::setMotorEnable(bool enable){ |
558 |
this->motorEnable = enable;
|
559 |
} |
560 |
|
561 |
bool MotorControl::getMotorEnable(){
|
562 |
return this->motorEnable; |
563 |
} |
564 |
|
565 |
void MotorControl::toggleMotorEnable(){
|
566 |
this->motorEnable = !this->motorEnable; |
567 |
} |