ken fuji / P_motor2

変な加速

Dependencies:   T_motor

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?

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 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 }