Kim Bruil / Mbed 2 deprecated r_robot

robot positie error test ding

Dependencies:   MODSERIAL mbed EMG_Input QEI biquadFilter

Motor/motor.cpp@14:d0e5894aa352, 2016-10-31 (annotated)

Committer:
kbruil
Date:
Mon Oct 31 12:32:54 2016 +0000
Revision:
14:d0e5894aa352
Parent:
12:e591729e854a 
Code biorobotics 31-10-2016

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 integral = 0;
kbruil 9:19dafcb4a098 12 olderr = 0;
kbruil 9:19dafcb4a098 13 bqc.add(&bq1).add(&bq2);
kbruil 10:27fdd43f3b85 14 pwm.period_ms(2);
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 12:e591729e854a 25 return(flip?-sense.getPulses()*(2*M_PI/8400.0):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 10:27fdd43f3b85 31 double position;
kbruil 9:19dafcb4a098 32 double pid;
kbruil 9:19dafcb4a098 33
kbruil 12:e591729e854a 34 position = this->getPosition();
kbruil 9:19dafcb4a098 35 error = setpoint - position;
kbruil 9:19dafcb4a098 36 integral += error * dt;
kbruil 9:19dafcb4a098 37 derivative = bqc.step((error - olderr) / dt);
kbruil 9:19dafcb4a098 38 olderr = error;
kbruil 9:19dafcb4a098 39 pid = (P * error + I * integral + D * derivative);
kbruil 9:19dafcb4a098 40
kbruil 9:19dafcb4a098 41 this->setSpeed(pid);
kbruil 9:19dafcb4a098 42 }
kbruil 11:4382c567e0d4 43
kbruil 11:4382c567e0d4 44 motor::~motor(){
kbruil 11:4382c567e0d4 45 }