DDSScontrol.cpp

00001 #include "mbed.h"
00002 #include "define.h"
00003 #include "DDSScontrol.h"
00004 #include "math.h"
00005 
00006 DDSS::DDSS(PinName _pwm_a,PinName _pwm_b,PinName _dir_a,PinName _dir_b) : pwm_a(_pwm_a),pwm_b(_pwm_b),dir_a(_dir_a),dir_b(_dir_b)//コンストラクタ ピン番号を引数にする
00007 {
00008     pwm_a.write(0.0);
00009     pwm_b.write(0.0);
00010     dir_a.write(0);
00011     dir_b.write(0);
00012 }
00013 
00014 void DDSS::setposi(double _Wp_x,double _Wp_y)//操舵点の座標を渡す
00015 {
00016     Wp_x = _Wp_x;
00017     Wp_y = _Wp_y;
00018 }
00019 
00020 void DDSS::control(double _refVx,double _refVy,double _refVz,double _stear_angle)//実際に制御を行う
00021 {
00022     refVx = _refVx;
00023     refVy = _refVy;
00024     refVz = _refVz;
00025     stear_angle = _stear_angle;
00026 
00027     V_x = refVx - Wp_y * refVz;//操舵点の速度決定
00028     V_y = refVy + Wp_x * refVz;
00029 
00030     omega_wheel = (V_x * cos(stear_angle) + V_y * sin(stear_angle)) / WHEEL_R;//操舵角速度
00031     omega_stear = (-V_x * sin(stear_angle) + V_y * cos(stear_angle)) / OFFSET;//走行角速度
00032 
00033     omega_a = GEAR_RATIO_TRANS * omega_wheel - GEAR_RATIO_TRANS * omega_stear;//モーターAの角速度
00034     omega_b = GEAR_RATIO_TRANS * omega_wheel + GEAR_RATIO_TRANS * omega_stear;//モーターBの角速度
00035 
00036     rpm_a = omega_a * 9.554;//角速度から回転数(rpm)に換算するところ
00037     rpm_b = omega_b * 9.554;
00038 
00039     MD_val_a = fabs(rpm_a) * 0.8 / MAX_RPM+0.1;//ESCONの指令値に変換する処理
00040     MD_val_b = fabs(rpm_b) * 0.8 / MAX_RPM+0.1;
00041 
00042     MD_val_a = MD_val_a > 0.9 ? MD_val_a = 0.9 : MD_val_a;//ESCONのPWM指令は10%から90%の間で有効なのでその範囲に収める処理
00043     MD_val_a = MD_val_a < 0.1 ? MD_val_a = 0.1 : MD_val_a;
00044     MD_val_b = MD_val_b > 0.9 ? MD_val_b = 0.9 : MD_val_b;
00045     MD_val_b = MD_val_b < 0.1 ? MD_val_b = 0.1 : MD_val_b;
00046     
00047     rpm_a > 0 ? dir_a.write(1):dir_a.write(0);//回転方向の指定
00048     rpm_b > 0 ? dir_b.write(1):dir_b.write(0);
00049     
00050     pwm_a.write(MD_val_a);//PWM出力処理
00051     pwm_b.write(MD_val_b);
00052 
00053 }