変な加速
P_motor.cpp@1:d0b60b54e3bd, 2017-10-25 (annotated)
- Committer:
- fujikenac
- Date:
- Wed Oct 25 09:29:53 2017 +0000
- Revision:
- 1:d0b60b54e3bd
- Parent:
- 0:29872002539b
add direct_control function
Who changed what in which revision?
User | Revision | Line number | New 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 | 1:d0b60b54e3bd | 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 | 1:d0b60b54e3bd | 49 | //output_reset(target / -2.0f); |
fujikenac | 0:29872002539b | 50 | } |
fujikenac | 0:29872002539b | 51 | else |
fujikenac | 0:29872002539b | 52 | { |
fujikenac | 0:29872002539b | 53 | output[2] = (delta * delta * target |
fujikenac | 0:29872002539b | 54 | + output[1] * (delta * RC + 2 * LC) |
fujikenac | 0:29872002539b | 55 | - output[0] * LC) |
fujikenac | 0:29872002539b | 56 | / (delta * RC + LC + delta * delta); |
fujikenac | 0:29872002539b | 57 | output[0] = output[1]; |
fujikenac | 0:29872002539b | 58 | output[1] = output[2]; |
fujikenac | 0:29872002539b | 59 | time_old = time_now; |
fujikenac | 0:29872002539b | 60 | } |
fujikenac | 0:29872002539b | 61 | if(target * output[2] < 1) |
fujikenac | 0:29872002539b | 62 | output_m = output[2]; |
fujikenac | 0:29872002539b | 63 | else |
fujikenac | 0:29872002539b | 64 | output_m = target * 1; |
fujikenac | 0:29872002539b | 65 | } |
fujikenac | 0:29872002539b | 66 | } |
fujikenac | 0:29872002539b | 67 | |
fujikenac | 1:d0b60b54e3bd | 68 | void P_motor::direct_controll(double target_) |
fujikenac | 1:d0b60b54e3bd | 69 | { |
fujikenac | 1:d0b60b54e3bd | 70 | motor = target_; |
fujikenac | 1:d0b60b54e3bd | 71 | output_reset(); |
fujikenac | 1:d0b60b54e3bd | 72 | } |
fujikenac | 1:d0b60b54e3bd | 73 | |
fujikenac | 0:29872002539b | 74 | void P_motor::free() |
fujikenac | 0:29872002539b | 75 | { |
fujikenac | 0:29872002539b | 76 | motor.free(); |
fujikenac | 0:29872002539b | 77 | timer_reset(); |
fujikenac | 0:29872002539b | 78 | output_reset(); |
fujikenac | 0:29872002539b | 79 | stat = NONE; |
fujikenac | 0:29872002539b | 80 | } |
fujikenac | 0:29872002539b | 81 | |
fujikenac | 0:29872002539b | 82 | void P_motor::stop() |
fujikenac | 0:29872002539b | 83 | { |
fujikenac | 0:29872002539b | 84 | motor.stop(); |
fujikenac | 0:29872002539b | 85 | timer_reset(); |
fujikenac | 0:29872002539b | 86 | output_reset(); |
fujikenac | 0:29872002539b | 87 | stat = NONE; |
fujikenac | 0:29872002539b | 88 | } |
fujikenac | 0:29872002539b | 89 | |
fujikenac | 0:29872002539b | 90 | void P_motor::timer_reset() |
fujikenac | 0:29872002539b | 91 | { |
fujikenac | 0:29872002539b | 92 | timer.stop(); |
fujikenac | 0:29872002539b | 93 | timer.reset(); |
fujikenac | 0:29872002539b | 94 | time_old = 0; |
fujikenac | 0:29872002539b | 95 | timer.start(); |
fujikenac | 0:29872002539b | 96 | } |
fujikenac | 0:29872002539b | 97 | |
fujikenac | 0:29872002539b | 98 | void P_motor::output_reset(double value) |
fujikenac | 0:29872002539b | 99 | { |
fujikenac | 0:29872002539b | 100 | output[0] = value; |
fujikenac | 0:29872002539b | 101 | output[1] = value; |
fujikenac | 0:29872002539b | 102 | output[2] = value; |
fujikenac | 0:29872002539b | 103 | output_m = value; |
fujikenac | 0:29872002539b | 104 | } |