変な加速
Diff: P_motor.cpp
- Revision:
- 0:29872002539b
- Child:
- 1:d0b60b54e3bd
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/P_motor.cpp Fri Oct 20 09:56:12 2017 +0000 @@ -0,0 +1,97 @@ +#include "P_motor.h" + +#define R 16 +#define L 4 +#define C 0.04 +#define PI 3.141592654 + + +P_motor::P_motor(I2C *i2c_, char addr_) + : motor(i2c_, addr_), pc(USBTX, USBRX) +{ + pc.baud(115200); + RC = R * C; + LC = L * C; + timer_reset(); + output_reset(); +} + +P_motor& P_motor::operator=(double target_) +{ + target = target_; + compute(); + return *this; +} + +void P_motor::run() +{ + motor = output_m; + pc.printf("%+3f\n\r", output_m); +} + +void P_motor::compute() +{ + stat_old = stat; + double time_now = timer.read_ms(); + double delta = (time_now - time_old) / 1000.0f; + if(target > 0.01) + stat = FORWARD; + else if(target < -0.01) + stat = BACKWARD; + else + free(); + if(stat) + { + if(stat * stat_old < 0) + { + //output[1] = -target / 2; + free(); + } + else + { + output[2] = (delta * delta * target + + output[1] * (delta * RC + 2 * LC) + - output[0] * LC) + / (delta * RC + LC + delta * delta); + output[0] = output[1]; + output[1] = output[2]; + time_old = time_now; + } + if(target * output[2] < 1) + output_m = output[2]; + else + output_m = target * 1; + } +} + +void P_motor::free() +{ + motor.free(); + timer_reset(); + output_reset(); + stat = NONE; +} + +void P_motor::stop() +{ + motor.stop(); + timer_reset(); + output_reset(); + stat = NONE; +} + +void P_motor::timer_reset() +{ + timer.stop(); + timer.reset(); + time_old = 0; + timer.start(); +} + +void P_motor::output_reset(double value) +{ + output[0] = value; + output[1] = value; + output[2] = value; + output_m = value; +} \ No newline at end of file