ken fuji / P_motor2

変な加速

Dependencies:   T_motor

P_motor.cpp@0:29872002539b, 2017-10-20 (annotated)

Committer:
fujikenac
Date:
Fri Oct 20 09:56:12 2017 +0000
Revision:
0:29872002539b
Child:
1:d0b60b54e3bd
1st commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
fujikenac 0:29872002539b 1 #include "P_motor.h"
fujikenac 0:29872002539b 2
fujikenac 0:29872002539b 3 #define R 16
fujikenac 0:29872002539b 4 #define L 4
fujikenac 0:29872002539b 5 #define C 0.04
fujikenac 0:29872002539b 6 #define PI 3.141592654
fujikenac 0:29872002539b 7
fujikenac 0:29872002539b 8
fujikenac 0:29872002539b 9 P_motor::P_motor(I2C *i2c_, char addr_)
fujikenac 0:29872002539b 10 : motor(i2c_, addr_), pc(USBTX, USBRX)
fujikenac 0:29872002539b 11 {
fujikenac 0:29872002539b 12 pc.baud(115200);
fujikenac 0:29872002539b 13 RC = R * C;
fujikenac 0:29872002539b 14 LC = L * C;
fujikenac 0:29872002539b 15 timer_reset();
fujikenac 0:29872002539b 16 output_reset();
fujikenac 0:29872002539b 17 }
fujikenac 0:29872002539b 18
fujikenac 0:29872002539b 19 P_motor& P_motor::operator=(double target_)
fujikenac 0:29872002539b 20 {
fujikenac 0:29872002539b 21 target = target_;
fujikenac 0:29872002539b 22 compute();
fujikenac 0:29872002539b 23 return *this;
fujikenac 0:29872002539b 24 }
fujikenac 0:29872002539b 25
fujikenac 0:29872002539b 26 void P_motor::run()
fujikenac 0:29872002539b 27 {
fujikenac 0:29872002539b 28 motor = output_m;
fujikenac 0:29872002539b 29 pc.printf("%+3f\n\r", output_m);
fujikenac 0:29872002539b 30 }
fujikenac 0:29872002539b 31
fujikenac 0:29872002539b 32 void P_motor::compute()
fujikenac 0:29872002539b 33 {
fujikenac 0:29872002539b 34 stat_old = stat;
fujikenac 0:29872002539b 35 double time_now = timer.read_ms();
fujikenac 0:29872002539b 36 double delta = (time_now - time_old) / 1000.0f;
fujikenac 0:29872002539b 37 if(target > 0.01)
fujikenac 0:29872002539b 38 stat = FORWARD;
fujikenac 0:29872002539b 39 else if(target < -0.01)
fujikenac 0:29872002539b 40 stat = BACKWARD;
fujikenac 0:29872002539b 41 else
fujikenac 0:29872002539b 42 free();
fujikenac 0:29872002539b 43 if(stat)
fujikenac 0:29872002539b 44 {
fujikenac 0:29872002539b 45 if(stat * stat_old < 0)
fujikenac 0:29872002539b 46 {
fujikenac 0:29872002539b 47 //output[1] = -target / 2;
fujikenac 0:29872002539b 48 free();
fujikenac 0:29872002539b 49 }
fujikenac 0:29872002539b 50 else
fujikenac 0:29872002539b 51 {
fujikenac 0:29872002539b 52 output[2] = (delta * delta * target
fujikenac 0:29872002539b 53 + output[1] * (delta * RC + 2 * LC)
fujikenac 0:29872002539b 54 - output[0] * LC)
fujikenac 0:29872002539b 55 / (delta * RC + LC + delta * delta);
fujikenac 0:29872002539b 56 output[0] = output[1];
fujikenac 0:29872002539b 57 output[1] = output[2];
fujikenac 0:29872002539b 58 time_old = time_now;
fujikenac 0:29872002539b 59 }
fujikenac 0:29872002539b 60 if(target * output[2] < 1)
fujikenac 0:29872002539b 61 output_m = output[2];
fujikenac 0:29872002539b 62 else
fujikenac 0:29872002539b 63 output_m = target * 1;
fujikenac 0:29872002539b 64 }
fujikenac 0:29872002539b 65 }
fujikenac 0:29872002539b 66
fujikenac 0:29872002539b 67 void P_motor::free()
fujikenac 0:29872002539b 68 {
fujikenac 0:29872002539b 69 motor.free();
fujikenac 0:29872002539b 70 timer_reset();
fujikenac 0:29872002539b 71 output_reset();
fujikenac 0:29872002539b 72 stat = NONE;
fujikenac 0:29872002539b 73 }
fujikenac 0:29872002539b 74
fujikenac 0:29872002539b 75 void P_motor::stop()
fujikenac 0:29872002539b 76 {
fujikenac 0:29872002539b 77 motor.stop();
fujikenac 0:29872002539b 78 timer_reset();
fujikenac 0:29872002539b 79 output_reset();
fujikenac 0:29872002539b 80 stat = NONE;
fujikenac 0:29872002539b 81 }
fujikenac 0:29872002539b 82
fujikenac 0:29872002539b 83 void P_motor::timer_reset()
fujikenac 0:29872002539b 84 {
fujikenac 0:29872002539b 85 timer.stop();
fujikenac 0:29872002539b 86 timer.reset();
fujikenac 0:29872002539b 87 time_old = 0;
fujikenac 0:29872002539b 88 timer.start();
fujikenac 0:29872002539b 89 }
fujikenac 0:29872002539b 90
fujikenac 0:29872002539b 91 void P_motor::output_reset(double value)
fujikenac 0:29872002539b 92 {
fujikenac 0:29872002539b 93 output[0] = value;
fujikenac 0:29872002539b 94 output[1] = value;
fujikenac 0:29872002539b 95 output[2] = value;
fujikenac 0:29872002539b 96 output_m = value;
fujikenac 0:29872002539b 97 }