Diff: TB6569.cpp

Revision:

2:88e59101a275

Parent:

1:2dc0a85a05b1

Child:

3:0a18bc568fa4

--- a/TB6569.cpp	Sun Jun 23 10:16:16 2013 +0000
+++ b/TB6569.cpp	Sun Jun 23 13:57:48 2013 +0000
@@ -1,71 +1,58 @@
 //
-//  TB6569.h
+//  TB6569.cpp
 //
 //  ... Y.Kuroda
 //
-//  2012.2.1 ... Originally written by Y.Kuroda
+//  2012.02.01 ... Originally written by Y.Kuroda
+//  2013.06.23 ... Originally written by Y.Kuroda
 //
 //
 #include <mbed.h>
 #include "TB6569.h"
 
-TB6569::TB6569( PinName pwm, PinName in1, PinName in2,
-                float pulse_per_rotate, float gear_ratio, float delta_t)
+TB6569::TB6569( PinName pwm, PinName in1, PinName in2, PinName vref, PinName alert)
 :   _pwm(pwm),
     _in1(in1),
-    _in2(in2)
+    _in2(in2),
+    _vref(vref),
+    _alert(alert)
 {
-    _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;
+    output_stop();
+    current_control(1.0);   // default = not control current
 }
 
 void
-TB6569::setParam(float pulse_per_rotate, float gear_ratio, float delta_t)
+TB6569::current_control(float vref)   // Current Control [0 ... 1]
 {
-    _pulse_per_rotate = pulse_per_rotate;
-    _gear_ratio = gear_ratio;
-    _delta_t = delta_t;
+    _vref = vref;
 }
 
 void
-TB6569::output(short int power)
+TB6569::output(int power)   // Percent [-100 ... 100]
 {
-    float p = fabs((float)power);   // abandan 100
     if(power == 0) {   // stop
-        _pwm = 0;
-        _in1 = 0;
-        _in2 = 0;
-    } else
+        output_stop();
+    } else {
+        output((float)power/100);
+    }
+}
+
+void
+TB6569::output(float power) // PWM Duty [-1 ... 1]
+{
+    if(power> 1) power= 1;
+    else
+    if(power<-1) power=-1;
+
     if(power > 0) {
-        _pwm = p;
+        _pwm = power;
         _in1 = 1;
         _in2 = 0;
     } else {
-        _pwm = p;
+        _pwm = power;
         _in1 = 0;
         _in2 = 1;
     }
-
-#if 0
-    switch(_mode){
-      case PWM:
-        //output();
-        break;
-        
-      case VEL:
-        break;
-        
-      case SVO:
-        break;
-    }
-#endif
 }
 
 void
@@ -76,53 +63,16 @@
     _in2 = 1;
 }
 
+void
+TB6569::output_stop(void)
+{
+    _pwm = 0;
+    _in1 = 0;
+    _in2 = 0;
+}
 
 void
-TB6569::output(void)
+TB6569::reset(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;
-//    }
+    output_stop();
 }
-
-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;
-}
-
-