robot positie error test ding
Dependencies: MODSERIAL mbed EMG_Input QEI biquadFilter
Motor/motor.cpp
- Committer:
- kbruil
- Date:
- 2016-10-28
- Revision:
- 12:e591729e854a
- Parent:
- 11:4382c567e0d4
File content as of revision 12:e591729e854a:
#include "motor.h" motor::motor(PinName p_pwm, PinName p_dir, PinName p_senseA, PinName p_senseB, double p, double i, double d, bool flipped): pwm(p_pwm), dir(p_dir), sense(p_senseA, p_senseB, NC, 8400, QEI::X4_ENCODING), bq1(2.533015E-01, 5.066030E-01, 2.533015E-01, 1.000000E+00, -4.531195E-01, 4.663256E-01), bq2(1.839030E-01, 3.678060E-01, 1.839030E-01, 1.000000E+00, -3.289757E-01, 6.458765E-02), P(p), I(i), D(d){ flip = flipped; integral = 0; olderr = 0; bqc.add(&bq1).add(&bq2); pwm.period_ms(2); sense.reset(); } void motor::setSpeed(float val){ val = flip?-val:val; dir.write(val>=0?1:0); 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. } double motor::getPosition(void){ return(flip?-sense.getPulses()*(2*M_PI/8400.0):sense.getPulses()*(2*M_PI/8400.0)); } void motor::PID(double setpoint, double dt){ double error; double derivative; double position; double pid; position = this->getPosition(); error = setpoint - position; integral += error * dt; derivative = bqc.step((error - olderr) / dt); olderr = error; pid = (P * error + I * integral + D * derivative); this->setSpeed(pid); } motor::~motor(){ }