変な加速
P_motor.cpp
- Committer:
- fujikenac
- Date:
- 2017-10-20
- Revision:
- 0:29872002539b
- Child:
- 1:d0b60b54e3bd
File content as of revision 0:29872002539b:
#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; }