Yoji KURODA / TB6569

TB6569 - Toshiba's DC motor driver

TB6569.cpp@0:ae0a960a4a2b, 2012-12-05 (annotated)

Committer:
ykuroda
Date:
Wed Dec 05 04:26:49 2012 +0000
Revision:
0:ae0a960a4a2b
Child:
1:2dc0a85a05b1
[mbed] converted /Minerva2/TB6569

Who changed what in which revision?

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