Yoji KURODA / TB6569

TB6569 - Toshiba's DC motor driver

TB6569.cpp@1:2dc0a85a05b1, 2013-06-23 (annotated)

Committer:
ykuroda
Date:
Sun Jun 23 10:16:16 2013 +0000
Revision:
1:2dc0a85a05b1
Parent:
0:ae0a960a4a2b
Child:
2:88e59101a275
First edition

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ykuroda 0:ae0a960a4a2b 1 //
ykuroda 1:2dc0a85a05b1 2 // TB6569.h
ykuroda 1:2dc0a85a05b1 3 //
ykuroda 1:2dc0a85a05b1 4 // ... Y.Kuroda
ykuroda 1:2dc0a85a05b1 5 //
ykuroda 1:2dc0a85a05b1 6 // 2012.2.1 ... Originally written by Y.Kuroda
ykuroda 1:2dc0a85a05b1 7 //
ykuroda 0:ae0a960a4a2b 8 //
ykuroda 0:ae0a960a4a2b 9 #include <mbed.h>
ykuroda 0:ae0a960a4a2b 10 #include "TB6569.h"
ykuroda 0:ae0a960a4a2b 11
ykuroda 0:ae0a960a4a2b 12 TB6569::TB6569( PinName pwm, PinName in1, PinName in2,
ykuroda 0:ae0a960a4a2b 13 float pulse_per_rotate, float gear_ratio, float delta_t)
ykuroda 0:ae0a960a4a2b 14 : _pwm(pwm),
ykuroda 0:ae0a960a4a2b 15 _in1(in1),
ykuroda 0:ae0a960a4a2b 16 _in2(in2)
ykuroda 0:ae0a960a4a2b 17 {
ykuroda 0:ae0a960a4a2b 18 _pwm = 0;
ykuroda 0:ae0a960a4a2b 19 _in1 = 0;
ykuroda 0:ae0a960a4a2b 20 _in2 = 0;
ykuroda 0:ae0a960a4a2b 21
ykuroda 0:ae0a960a4a2b 22 _mode = PWM;
ykuroda 0:ae0a960a4a2b 23 _pulse_per_rotate = pulse_per_rotate;
ykuroda 0:ae0a960a4a2b 24 _gear_ratio = gear_ratio;
ykuroda 0:ae0a960a4a2b 25 _delta_t = delta_t;
ykuroda 0:ae0a960a4a2b 26 _dpos_sum = 0;
ykuroda 0:ae0a960a4a2b 27 }
ykuroda 0:ae0a960a4a2b 28
ykuroda 0:ae0a960a4a2b 29 void
ykuroda 0:ae0a960a4a2b 30 TB6569::setParam(float pulse_per_rotate, float gear_ratio, float delta_t)
ykuroda 0:ae0a960a4a2b 31 {
ykuroda 0:ae0a960a4a2b 32 _pulse_per_rotate = pulse_per_rotate;
ykuroda 0:ae0a960a4a2b 33 _gear_ratio = gear_ratio;
ykuroda 0:ae0a960a4a2b 34 _delta_t = delta_t;
ykuroda 0:ae0a960a4a2b 35 }
ykuroda 0:ae0a960a4a2b 36
ykuroda 0:ae0a960a4a2b 37 void
ykuroda 0:ae0a960a4a2b 38 TB6569::output(short int power)
ykuroda 0:ae0a960a4a2b 39 {
ykuroda 1:2dc0a85a05b1 40 float p = fabs((float)power); // abandan 100
ykuroda 0:ae0a960a4a2b 41 if(power == 0) { // stop
ykuroda 0:ae0a960a4a2b 42 _pwm = 0;
ykuroda 0:ae0a960a4a2b 43 _in1 = 0;
ykuroda 0:ae0a960a4a2b 44 _in2 = 0;
ykuroda 0:ae0a960a4a2b 45 } else
ykuroda 0:ae0a960a4a2b 46 if(power > 0) {
ykuroda 0:ae0a960a4a2b 47 _pwm = p;
ykuroda 0:ae0a960a4a2b 48 _in1 = 1;
ykuroda 0:ae0a960a4a2b 49 _in2 = 0;
ykuroda 0:ae0a960a4a2b 50 } else {
ykuroda 0:ae0a960a4a2b 51 _pwm = p;
ykuroda 0:ae0a960a4a2b 52 _in1 = 0;
ykuroda 0:ae0a960a4a2b 53 _in2 = 1;
ykuroda 0:ae0a960a4a2b 54 }
ykuroda 0:ae0a960a4a2b 55
ykuroda 0:ae0a960a4a2b 56 #if 0
ykuroda 0:ae0a960a4a2b 57 switch(_mode){
ykuroda 0:ae0a960a4a2b 58 case PWM:
ykuroda 0:ae0a960a4a2b 59 //output();
ykuroda 0:ae0a960a4a2b 60 break;
ykuroda 0:ae0a960a4a2b 61
ykuroda 0:ae0a960a4a2b 62 case VEL:
ykuroda 0:ae0a960a4a2b 63 break;
ykuroda 0:ae0a960a4a2b 64
ykuroda 0:ae0a960a4a2b 65 case SVO:
ykuroda 0:ae0a960a4a2b 66 break;
ykuroda 0:ae0a960a4a2b 67 }
ykuroda 0:ae0a960a4a2b 68 #endif
ykuroda 0:ae0a960a4a2b 69 }
ykuroda 0:ae0a960a4a2b 70
ykuroda 0:ae0a960a4a2b 71 void
ykuroda 0:ae0a960a4a2b 72 TB6569::output_break(void)
ykuroda 0:ae0a960a4a2b 73 {
ykuroda 0:ae0a960a4a2b 74 _pwm = 0;
ykuroda 0:ae0a960a4a2b 75 _in1 = 1;
ykuroda 0:ae0a960a4a2b 76 _in2 = 1;
ykuroda 0:ae0a960a4a2b 77 }
ykuroda 0:ae0a960a4a2b 78
ykuroda 0:ae0a960a4a2b 79
ykuroda 0:ae0a960a4a2b 80 void
ykuroda 0:ae0a960a4a2b 81 TB6569::output(void)
ykuroda 0:ae0a960a4a2b 82 {
ykuroda 0:ae0a960a4a2b 83 switch(_mode){
ykuroda 0:ae0a960a4a2b 84
ykuroda 0:ae0a960a4a2b 85 case VEL:
ykuroda 0:ae0a960a4a2b 86 _rs_d = _fv_d *2 / _wheel_diameter;
ykuroda 0:ae0a960a4a2b 87
ykuroda 0:ae0a960a4a2b 88 case ROT:
ykuroda 0:ae0a960a4a2b 89 _drs = _rs_d - _rs;
ykuroda 0:ae0a960a4a2b 90 _drs_sum = _drs + _drs_sum * _dc;
ykuroda 0:ae0a960a4a2b 91 _power = _kp * _drs - _kd * _rs + _ki * _drs_sum + _ff * _rs_d;
ykuroda 0:ae0a960a4a2b 92 break;
ykuroda 0:ae0a960a4a2b 93
ykuroda 0:ae0a960a4a2b 94 case SVO:
ykuroda 0:ae0a960a4a2b 95 _dpos = _rad_d - _rad;
ykuroda 0:ae0a960a4a2b 96 _dpos_sum = _dpos + _dpos_sum * _dc;
ykuroda 0:ae0a960a4a2b 97 _power = _kp * _dpos - _kd * _rad + _ki * _dpos_sum + _ff * _dpos;
ykuroda 0:ae0a960a4a2b 98 break;
ykuroda 0:ae0a960a4a2b 99
ykuroda 0:ae0a960a4a2b 100 }
ykuroda 0:ae0a960a4a2b 101
ykuroda 0:ae0a960a4a2b 102 // switch(_mode){
ykuroda 0:ae0a960a4a2b 103 // case PWM:
ykuroda 0:ae0a960a4a2b 104 if(_power >= 0) {
ykuroda 0:ae0a960a4a2b 105 _pwm = fabs((float)_power)/100;
ykuroda 0:ae0a960a4a2b 106 _in1 = 1;
ykuroda 0:ae0a960a4a2b 107 _in2 = 0;
ykuroda 0:ae0a960a4a2b 108 } else {
ykuroda 0:ae0a960a4a2b 109 _pwm = fabs((float)_power)/100;
ykuroda 0:ae0a960a4a2b 110 _in1 = 0;
ykuroda 0:ae0a960a4a2b 111 _in2 = 1;
ykuroda 0:ae0a960a4a2b 112 }
ykuroda 0:ae0a960a4a2b 113 // break;
ykuroda 0:ae0a960a4a2b 114 // }
ykuroda 0:ae0a960a4a2b 115 }
ykuroda 0:ae0a960a4a2b 116
ykuroda 0:ae0a960a4a2b 117 float
ykuroda 0:ae0a960a4a2b 118 TB6569::getRotSpeed(int _pulse)
ykuroda 0:ae0a960a4a2b 119 {
ykuroda 0:ae0a960a4a2b 120 float dp = _pulse - _enc_pulse;
ykuroda 0:ae0a960a4a2b 121 _enc_pulse = _pulse;
ykuroda 0:ae0a960a4a2b 122
ykuroda 0:ae0a960a4a2b 123 _rs = dp * 2 * PI / (_pulse_per_rotate * _gear_ratio * _delta_t); // rotational speed [rad/s]
ykuroda 0:ae0a960a4a2b 124 _fv = _rs * _wheel_diameter / 2;
ykuroda 0:ae0a960a4a2b 125 return _rs;
ykuroda 0:ae0a960a4a2b 126 }
ykuroda 0:ae0a960a4a2b 127
ykuroda 0:ae0a960a4a2b 128