Kim Bruil / Mbed 2 deprecated r_robot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

Motor/motor.cpp@9:19dafcb4a098, 2016-10-27 (annotated)

Committer:
kbruil
Date:
Thu Oct 27 19:36:12 2016 +0000
Revision:
9:19dafcb4a098
Child:
10:27fdd43f3b85
Robot PID cleaned up a bit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kbruil 9:19dafcb4a098 1 #include "motor.h"
kbruil 9:19dafcb4a098 2
kbruil 9:19dafcb4a098 3 motor::motor(PinName p_pwm, PinName p_dir, PinName p_senseA, PinName p_senseB, double p, double i, double d, bool flipped):
kbruil 9:19dafcb4a098 4 pwm(p_pwm),
kbruil 9:19dafcb4a098 5 dir(p_dir),
kbruil 9:19dafcb4a098 6 sense(p_senseA, p_senseB, NC, 8400, QEI::X4_ENCODING),
kbruil 9:19dafcb4a098 7 bq1(2.533015E-01, 5.066030E-01, 2.533015E-01, 1.000000E+00, -4.531195E-01, 4.663256E-01),
kbruil 9:19dafcb4a098 8 bq2(1.839030E-01, 3.678060E-01, 1.839030E-01, 1.000000E+00, -3.289757E-01, 6.458765E-02),
kbruil 9:19dafcb4a098 9 P(p), I(i), D(d){
kbruil 9:19dafcb4a098 10 flip = flipped;
kbruil 9:19dafcb4a098 11 position = 0;
kbruil 9:19dafcb4a098 12 integral = 0;
kbruil 9:19dafcb4a098 13 olderr = 0;
kbruil 9:19dafcb4a098 14 bqc.add(&bq1).add(&bq2);
kbruil 9:19dafcb4a098 15 sense.reset();
kbruil 9:19dafcb4a098 16 }
kbruil 9:19dafcb4a098 17
kbruil 9:19dafcb4a098 18 void motor::setSpeed(float val){
kbruil 9:19dafcb4a098 19 val = flip?-val:val;
kbruil 9:19dafcb4a098 20 dir.write(val>=0?1:0);
kbruil 9:19dafcb4a098 21 pwm.write(fabs(val)>1?1.0f:pow((double)fabs(val), 0.75)); // Use a root to make the motor give more power in low end.
kbruil 9:19dafcb4a098 22 }
kbruil 9:19dafcb4a098 23
kbruil 9:19dafcb4a098 24 double motor::getPosition(void){
kbruil 9:19dafcb4a098 25 return(sense.getPulses()*(2*M_PI/8400.0));
kbruil 9:19dafcb4a098 26 }
kbruil 9:19dafcb4a098 27
kbruil 9:19dafcb4a098 28 void motor::PID(double setpoint, double dt){
kbruil 9:19dafcb4a098 29 double error;
kbruil 9:19dafcb4a098 30 double derivative;
kbruil 9:19dafcb4a098 31 double pid;
kbruil 9:19dafcb4a098 32
kbruil 9:19dafcb4a098 33 position = flip?-this->getPosition():this->getPosition();
kbruil 9:19dafcb4a098 34 error = setpoint - position;
kbruil 9:19dafcb4a098 35 integral += error * dt;
kbruil 9:19dafcb4a098 36 derivative = bqc.step((error - olderr) / dt);
kbruil 9:19dafcb4a098 37 olderr = error;
kbruil 9:19dafcb4a098 38 pid = (P * error + I * integral + D * derivative);
kbruil 9:19dafcb4a098 39
kbruil 9:19dafcb4a098 40 this->setSpeed(pid);
kbruil 9:19dafcb4a098 41 }