ManualControl.cpp

00001 // ゲームコントローラのジョイスティックデータから，ローパスフィルタを用いて指令速度を生成するクラス
00002 // 
00003 // 作成日：2019年12月30日
00004 // 作成者：上野祐樹
00005 
00006 #include "ManualControl.h"
00007 #include "Filter.h"
00008 
00009 Filter velX_filter(INT_TIME);
00010 Filter velY_filter(INT_TIME);
00011 Filter velZ_filter(INT_TIME);
00012 
00013 ManualControl::ManualControl()
00014 {
00015     anglePIDEnable = false;
00016 }
00017 
00018 int ManualControl::init()
00019 {
00020   // コンストラクタでローパスフィルタの初期化ができなかったので修正 
00021   velX_filter.setLowPassPara(MANUAL_LOWPASS_T, 0.0);//ローパスフィルタのTと初期値を設定
00022   velY_filter.setLowPassPara(MANUAL_LOWPASS_T, 0.0);//ローパスフィルタのTと初期値を設定
00023   velZ_filter.setLowPassPara(MANUAL_LOWPASS_T, 0.0);//ローパスフィルタのTと初期値を設定
00024   return 0;
00025 }
00026 
00027 coords ManualControl::getRefVel(unsigned int JoyX, unsigned int JoyY, unsigned int JoyZ)
00028 {
00029     int joyraw;
00030     coords rawV, refV;
00031 
00032     // ジョイスティックから指令速度を計算する
00033     joyraw = (int)JoyX - 127; 
00034     if(abs(joyraw) >= JOY_DEADBAND){
00035       if(joyraw >= 0){
00036         joyraw -= JOY_DEADBAND;
00037       }else{
00038         joyraw += JOY_DEADBAND;
00039       }
00040       rawV.x = (joyraw)/(127.0 - (double)JOY_DEADBAND) * JOY_MAXVEL;
00041     }else{
00042       rawV.x = 0.0;
00043     }
00044     refV.x = velX_filter.LowPassFilter(rawV.x);
00045 
00046     joyraw = (int)JoyY - 127; 
00047     if(abs(joyraw) >= JOY_DEADBAND){
00048       if(joyraw >= 0){
00049         joyraw -= JOY_DEADBAND;
00050       }else{
00051         joyraw += JOY_DEADBAND;
00052       }
00053       rawV.y = (joyraw)/(127.0 - (double)JOY_DEADBAND) * JOY_MAXVEL;
00054     }else{
00055       rawV.y = 0.0;
00056     }
00057     refV.y = velY_filter.LowPassFilter(rawV.y);
00058 
00059     joyraw = (int)JoyZ - 127; 
00060     if(abs(joyraw) >= JOY_DEADBAND){
00061       if(joyraw >= 0){
00062         joyraw -= JOY_DEADBAND;
00063       }else{
00064         joyraw += JOY_DEADBAND;
00065       }
00066       rawV.z = (joyraw)/(127.0 - (double)JOY_DEADBAND) * JOY_MAXANGVEL;
00067     }else{
00068       rawV.z = 0.0;
00069     }
00070     refV.z = velZ_filter.LowPassFilter(rawV.z);
00071 
00072     return refV;
00073 }