robot positie error test ding
Dependencies: MODSERIAL mbed EMG_Input QEI biquadFilter
Motor/motor.cpp@11:4382c567e0d4, 2016-10-28 (annotated)
- Committer:
- kbruil
- Date:
- Fri Oct 28 07:35:36 2016 +0000
- Revision:
- 11:4382c567e0d4
- Parent:
- 10:27fdd43f3b85
- Child:
- 12:e591729e854a
Opgeschoond, motoren eigen class, basis voor EMG input
Who changed what in which revision?
User | Revision | Line number | New 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 | 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 | 10:27fdd43f3b85 | 31 | double position; |
kbruil | 9:19dafcb4a098 | 32 | double pid; |
kbruil | 9:19dafcb4a098 | 33 | |
kbruil | 9:19dafcb4a098 | 34 | position = flip?-this->getPosition():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 | } |