Diff: TB6569.cpp

Revision:

0:ae0a960a4a2b

Child:

1:2dc0a85a05b1

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TB6569.cpp	Wed Dec 05 04:26:49 2012 +0000
@@ -0,0 +1,123 @@
+//
+//  motor.cpp
+//
+#include <mbed.h>
+#include "TB6569.h"
+
+TB6569::TB6569( PinName pwm, PinName in1, PinName in2,
+                float pulse_per_rotate, float gear_ratio, float delta_t)
+:   _pwm(pwm),
+    _in1(in1),
+    _in2(in2)
+{
+    _pwm = 0;
+    _in1 = 0;
+    _in2 = 0;
+
+    _mode = PWM;
+    _pulse_per_rotate = pulse_per_rotate;
+    _gear_ratio = gear_ratio;
+    _delta_t = delta_t;
+    _dpos_sum = 0;
+}
+
+void
+TB6569::setParam(float pulse_per_rotate, float gear_ratio, float delta_t)
+{
+    _pulse_per_rotate = pulse_per_rotate;
+    _gear_ratio = gear_ratio;
+    _delta_t = delta_t;
+}
+
+void
+TB6569::output(short int power)
+{
+    float p = fabs((float)power)/100;
+    if(power == 0) {   // stop
+        _pwm = 0;
+        _in1 = 0;
+        _in2 = 0;
+    } else
+    if(power > 0) {
+        _pwm = p;
+        _in1 = 1;
+        _in2 = 0;
+    } else {
+        _pwm = p;
+        _in1 = 0;
+        _in2 = 1;
+    }
+
+#if 0
+    switch(_mode){
+      case PWM:
+        //output();
+        break;
+        
+      case VEL:
+        break;
+        
+      case SVO:
+        break;
+    }
+#endif
+}
+
+void
+TB6569::output_break(void)
+{
+    _pwm = 0;
+    _in1 = 1;
+    _in2 = 1;
+}
+
+
+void
+TB6569::output(void)
+{
+    switch(_mode){
+
+      case VEL:
+        _rs_d = _fv_d *2 / _wheel_diameter;
+      
+      case ROT:
+        _drs = _rs_d - _rs;
+        _drs_sum = _drs + _drs_sum * _dc;
+        _power = _kp * _drs - _kd * _rs + _ki * _drs_sum + _ff * _rs_d;
+        break;
+        
+      case SVO:
+        _dpos = _rad_d - _rad;
+        _dpos_sum = _dpos + _dpos_sum * _dc;
+        _power = _kp * _dpos - _kd * _rad + _ki * _dpos_sum + _ff * _dpos;
+        break;
+        
+    }
+  
+//    switch(_mode){
+//      case PWM:
+        if(_power >= 0) {
+            _pwm = fabs((float)_power)/100;
+            _in1 = 1;
+            _in2 = 0;
+        } else {
+            _pwm = fabs((float)_power)/100;
+            _in1 = 0;
+            _in2 = 1;
+        }
+//        break;
+//    }
+}
+
+float
+TB6569::getRotSpeed(int _pulse)
+{
+    float dp = _pulse - _enc_pulse;
+    _enc_pulse = _pulse;
+
+    _rs = dp * 2 * PI / (_pulse_per_rotate * _gear_ratio * _delta_t);  // rotational speed [rad/s]
+    _fv = _rs * _wheel_diameter / 2;
+    return _rs;
+}
+
+