Platform.cpp

00001 // 4輪オムニやメカナム，双輪キャスタなどのプラットフォームごとに
00002 // 各モータの指令速度の計算からRoboClawの制御，自己位置推定を行なうクラス
00003 // Platform.h の DRIVE_UNIT の定義でプラットフォームを選択する
00004 // 作成日：2019年12月30日
00005 // 作成者：上野祐樹
00006 
00007 //アクティブキャスタの処理を追加
00008 //追加日:2022年01月26日
00009 //編集者:小林亮紘
00010 
00011 #include "mbed.h"
00012 #include "Platform.h"
00013 #include "AMT22.h"
00014 #include "DDSScontrol.h"
00015 //#include "RoboClaw.h"
00016 //#include "AMT203VPeach.h"
00017 
00018 //RoboClaw MD(&SERIAL_ROBOCLAW,1);
00019 
00020 #if DRIVE_UNIT == PLATFORM_DUALWHEEL
00021 AMT203V amt203(&SPI, PIN_CSB);
00022 #elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
00023     SPI spi(P10_14, P10_15, P10_12); // mosi, miso, sclk
00024     AMT203V absenc_1(&spi, PIN_CSB_1);
00025     AMT203V absenc_2(&spi, PIN_CSB_2);
00026     AMT203V absenc_3(&spi, PIN_CSB_3);
00027     
00028     DDSS unit1(PIN_PWM_1A,PIN_PWM_1B,PIN_DIR_1A,PIN_DIR_1B);//ピン宣言
00029     DDSS unit2(PIN_PWM_2A,PIN_PWM_2B,PIN_DIR_2A,PIN_DIR_2B);//ピン宣言
00030     DDSS unit3(PIN_PWM_3A,PIN_PWM_3B,PIN_DIR_3A,PIN_DIR_3B);//ピン宣言
00031 
00032 #endif
00033 
00034 Platform::Platform(int dir1, int dir2, int dir3, int dir4)
00035 {
00036     rotateDir[0] = (dir1 > 0) - (dir1 < 0); // 1より大きい数値だった場合に，1か-1にするための処理
00037     rotateDir[1] = (dir2 > 0) - (dir2 < 0);
00038     rotateDir[2] = (dir3 > 0) - (dir3 < 0);
00039     rotateDir[3] = (dir4 > 0) - (dir4 < 0);
00040 
00041     init_done = false;
00042 }
00043 
00044 Platform::Platform(int dir1, int dir2, int dir3)
00045 {
00046     rotateDir[0] = (dir1 > 0) - (dir1 < 0); // 1より大きい数値だった場合に，1か-1にするための処理
00047     rotateDir[1] = (dir2 > 0) - (dir2 < 0);
00048     rotateDir[2] = (dir3 > 0) - (dir3 < 0);
00049     rotateDir[3] = 1;
00050     init_done = false;
00051 }
00052 
00053 Platform::Platform()
00054 {
00055     rotateDir[0] = 1; // 指示がないときはすべて1
00056     rotateDir[1] = 1;
00057     rotateDir[2] = 1;
00058     rotateDir[3] = 1;
00059 
00060     init_done = false;
00061 }
00062 
00063 // 自己位置推定の初期化
00064 void Platform::platformInit(coords initPosi)
00065 {
00066 #if DRIVE_UNIT == PLATFORM_DUALWHEEL
00067     SPI.begin(); // ここでSPIをbeginしてあげないとちゃんと動かなかった
00068     SPI.setClockDivider(SPI_CLOCK_DIV16); //SPI通信のクロックを1MHzに設定 beginの後に置かないと，処理が止まる
00069     stateamt203 = amt203.init();
00070 #elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
00071     printf("AMT222 initialization %d %d %d\r\n", absenc_1.init(), absenc_2.init(), absenc_3.init());
00072     unit1.setposi(W1_x,W1_y);
00073     unit2.setposi(W2_x,W2_y);
00074     unit3.setposi(W3_x,W3_y);
00075 #endif
00076 
00077     //MD.begin(115200);
00078 
00079     Posi = initPosi;
00080 
00081     preEncX = 0;
00082     preEncY = 0;
00083     pre_angle_rad = 0.0;//Posi.z;
00084     init_done = true;
00085 }
00086 
00087 // 自己位置推定値(Posi)を外部からセット
00088 void Platform::setPosi(coords tempPosi)
00089 {
00090     Posi = tempPosi;
00091 }
00092 
00093 // エンコーダのカウント値と，ジャイロセンサから取得した角度をもとに自己位置を計算する
00094 coords Platform::getPosi(int encX, int encY, double angle_rad)
00095 {
00096     if(init_done) {
00097         // ローカル座標系での変化量を計算(zは角度)
00098         coords diff;
00099 
00100         // エンコーダのカウント値から角度の変化量を計算する
00101         double angX, angY;
00102         angX = (double)( encX - preEncX ) * _2PI_RES4;
00103         angY = (double)( encY - preEncY ) * _2PI_RES4;
00104 
00105         double angle_diff;
00106         angle_diff = angle_rad - pre_angle_rad; // 角度の変化量を計算
00107         diff.z = angle_diff;
00108         diff.x = RADIUS_X * angX; //RADIUS_X はX軸エンコーダの車輪半径
00109         diff.y = RADIUS_Y * angY; //RADIUS_Y はY軸エンコーダの車輪半径
00110 
00111         // グローバル座標系での変化量に変換し，これまでのデータに加算することで自己位置推定完了
00112         Posi.z += diff.z;
00113         Posi.x += diff.x * cos( Posi.z ) - diff.y * sin( Posi.z );
00114         Posi.y += diff.x * sin( Posi.z ) + diff.y * cos( Posi.z );
00115 
00116         // 1サンプル前のデータとして今回取得したデータを格納
00117         preEncX = encX;
00118         preEncY = encY;
00119         pre_angle_rad = angle_rad;
00120     }
00121     return Posi;
00122 }
00123 
00124 void Platform::VelocityControl(coords refV)
00125 {
00126     if(init_done) {
00127 #if DRIVE_UNIT == PLATFORM_OMNI3WHEEL
00128         double refOmegaA, refOmegaB, refOmegaC;
00129         // 車輪反時計方向が正
00130         refOmegaA = (-refV.y - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
00131         refOmegaB = ( refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
00132         refOmegaC = (-refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
00133 
00134         // RoboClawの指令値に変換
00135         double mdCmdA, mdCmdB, mdCmdC;
00136         mdCmdA = refOmegaA * _2RES_PI;
00137         mdCmdB = refOmegaB * _2RES_PI;
00138         mdCmdC = refOmegaC * _2RES_PI;
00139 
00140         // モータにcmdを送り，回す
00141         //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]);// 右前
00142         //MD.SpeedM2(ADR_MD1, (int)mdCmdB * rotateDir[1]);// 左前
00143         //MD.SpeedM2(ADR_MD2, (int)mdCmdC * rotateDir[2]);// 右後
00144 #elif DRIVE_UNIT == PLATFORM_OMNI4WHEEL
00145         double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
00146         // 車輪反時計方向が正
00147         refOmegaA = ( refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO; // [rad/s]
00148         refOmegaB = ( refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
00149         refOmegaC = (-refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
00150         refOmegaD = (-refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
00151 
00152         // RoboClawの指令値に変換
00153         double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
00154         mdCmdA = refOmegaA * _2RES_PI;
00155         mdCmdB = refOmegaB * _2RES_PI;
00156         mdCmdC = refOmegaC * _2RES_PI;
00157         mdCmdD = refOmegaD * _2RES_PI;
00158 
00159         //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]); // 左前 ①
00160         //MD.SpeedM2(ADR_MD1, (int)mdCmdC * rotateDir[1]); // 左後 ②
00161         //MD.SpeedM1(ADR_MD2, (int)mdCmdD * rotateDir[2]); // 右後 ③
00162         //MD.SpeedM2(ADR_MD2, (int)mdCmdB * rotateDir[3]); // 右前 ④
00163 #elif DRIVE_UNIT == PLATFORM_DUALWHEEL
00164         // ターンテーブルの角度取得
00165         double thetaDuEnc, thetaDu;
00166         static double  preThetaDuEnc = thetaDuEnc;
00167         thetaDuEnc = amt203.getEncount();
00168         if( thetaDuEnc == -1 ) {
00169             thetaDuEnc = preThetaDuEnc; // -1はエラーなので，前の値を格納しておく
00170         }
00171         preThetaDuEnc = thetaDuEnc;
00172         thetaDu = (double)thetaDuEnc*2*PI / TT_RES4;    // 角度に変換
00173 
00174         // 車輪やターンテーブルの指令速度を計算
00175         double cosDu, sinDu, refOmegaR, refOmegaL, refOmegaT;
00176         cosDu = cos(thetaDu);
00177         sinDu = sin(thetaDu);
00178         refOmegaR = ( ( cosDu - sinDu ) * refV.x + ( sinDu + cosDu ) * refV.y ) / RADIUS_R;// right
00179         refOmegaL = ( ( cosDu + sinDu ) * refV.x + ( sinDu - cosDu ) * refV.y ) / RADIUS_L;// left
00180         refOmegaT = ( - ( 2 * sinDu / W ) * refV.x + ( 2 * cosDu / W ) * refV.y - refV.z ) * GEARRATIO;// turntable
00181 
00182         // RoboClawの指令値に変換
00183         double mdCmdR, mdCmdL, mdCmdT;
00184         mdCmdR = refOmegaR * _2RES_PI;
00185         mdCmdL = refOmegaL * _2RES_PI;
00186         mdCmdT = refOmegaT * _2RES_PI_T;
00187 
00188         // モータにcmdを送り，回す
00189         //MD.SpeedM1(ADR_MD1, -(int)mdCmdR);// 右車輪
00190         //MD.SpeedM2(ADR_MD1,  (int)mdCmdL);// 左車輪
00191         //MD.SpeedM1(ADR_MD2,  (int)mdCmdT);// ターンテーブル
00192 #elif DRIVE_UNIT == PLATFORM_MECHANUM
00193         double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
00194         // 車輪の軸まわりで見て，反時計方向が正
00195         refOmegaA = ( refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左前
00196         refOmegaB = ( refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左後
00197         refOmegaC = (-refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右後
00198         refOmegaD = (-refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右前
00199 
00200         // RoboClawの指令値に変換
00201         double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
00202         mdCmdA = refOmegaA * _2RES_PI;
00203         mdCmdB = refOmegaB * _2RES_PI;
00204         mdCmdC = refOmegaC * _2RES_PI;
00205         mdCmdD = refOmegaD * _2RES_PI;
00206 
00207         //MD.SpeedM1(ADR_MD1,  (int)mdCmdA * rotateDir[0]);// 左前
00208         //MD.SpeedM2(ADR_MD1,  (int)mdCmdB * rotateDir[1]);// 左後
00209         //MD.SpeedM1(ADR_MD2,  (int)mdCmdC * rotateDir[2]);// 右後
00210         //MD.SpeedM2(ADR_MD2,  (int)mdCmdD * rotateDir[3]);// 右前
00211         
00212 #elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
00213         double stear_1_rad = (double)absenc_1.getEncount() / ABS_ENCRES * PI * -2;
00214         double stear_2_rad = (double)absenc_2.getEncount() / ABS_ENCRES * PI * -2;
00215         double stear_3_rad = (double)absenc_3.getEncount() / ABS_ENCRES * PI * -2;
00216         
00217         unit1.control(refV.x,refV.y,refV.z,stear_1_rad);
00218         unit2.control(refV.x,refV.y,refV.z,stear_2_rad);
00219         unit3.control(refV.x,refV.y,refV.z,stear_3_rad);
00220         
00221         
00222 #endif
00223     }
00224 }