Kobayashi Akihiro / ActiveCaster

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   ActiveCaster_ ActiveCaster_2

Platform.cpp@2:f206311600ee, 2022-01-28 (annotated)

Committer:
e5119053f6
Date:
Fri Jan 28 15:43:18 2022 +0000
Revision:
2:f206311600ee
Parent:
1:5a65fc20f8c2 
DDSS

Who changed what in which revision?

UserRevisionLine numberNew contents of line
e5119053f6 0:5e4f1e288e2a 1 // 4輪オムニやメカナム，双輪キャスタなどのプラットフォームごとに
e5119053f6 0:5e4f1e288e2a 2 // 各モータの指令速度の計算からRoboClawの制御，自己位置推定を行なうクラス
e5119053f6 0:5e4f1e288e2a 3 // Platform.h の DRIVE_UNIT の定義でプラットフォームを選択する
e5119053f6 0:5e4f1e288e2a 4 // 作成日：2019年12月30日
e5119053f6 0:5e4f1e288e2a 5 // 作成者：上野祐樹
e5119053f6 0:5e4f1e288e2a 6
e5119053f6 0:5e4f1e288e2a 7 //アクティブキャスタの処理を追加
e5119053f6 1:5a65fc20f8c2 8 //追加日:2022年01月26日
e5119053f6 0:5e4f1e288e2a 9 //編集者:小林亮紘
e5119053f6 0:5e4f1e288e2a 10
e5119053f6 0:5e4f1e288e2a 11 #include "mbed.h"
e5119053f6 0:5e4f1e288e2a 12 #include "Platform.h"
e5119053f6 1:5a65fc20f8c2 13 #include "AMT22.h"
e5119053f6 1:5a65fc20f8c2 14 #include "DDSScontrol.h"
e5119053f6 0:5e4f1e288e2a 15 //#include "RoboClaw.h"
e5119053f6 0:5e4f1e288e2a 16 //#include "AMT203VPeach.h"
e5119053f6 0:5e4f1e288e2a 17
e5119053f6 0:5e4f1e288e2a 18 //RoboClaw MD(&SERIAL_ROBOCLAW,1);
e5119053f6 0:5e4f1e288e2a 19
e5119053f6 0:5e4f1e288e2a 20 #if DRIVE_UNIT == PLATFORM_DUALWHEEL
e5119053f6 1:5a65fc20f8c2 21 AMT203V amt203(&SPI, PIN_CSB);
e5119053f6 0:5e4f1e288e2a 22 #elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
e5119053f6 1:5a65fc20f8c2 23 SPI spi(P10_14, P10_15, P10_12); // mosi, miso, sclk
e5119053f6 1:5a65fc20f8c2 24 AMT203V absenc_1(&spi, PIN_CSB_1);
e5119053f6 1:5a65fc20f8c2 25 AMT203V absenc_2(&spi, PIN_CSB_2);
e5119053f6 1:5a65fc20f8c2 26 AMT203V absenc_3(&spi, PIN_CSB_3);
e5119053f6 1:5a65fc20f8c2 27
e5119053f6 1:5a65fc20f8c2 28 DDSS unit1(PIN_PWM_1A,PIN_PWM_1B,PIN_DIR_1A,PIN_DIR_1B);//ピン宣言
e5119053f6 1:5a65fc20f8c2 29 DDSS unit2(PIN_PWM_2A,PIN_PWM_2B,PIN_DIR_2A,PIN_DIR_2B);//ピン宣言
e5119053f6 1:5a65fc20f8c2 30 DDSS unit3(PIN_PWM_3A,PIN_PWM_3B,PIN_DIR_3A,PIN_DIR_3B);//ピン宣言
e5119053f6 1:5a65fc20f8c2 31
e5119053f6 0:5e4f1e288e2a 32 #endif
e5119053f6 0:5e4f1e288e2a 33
e5119053f6 1:5a65fc20f8c2 34 Platform::Platform(int dir1, int dir2, int dir3, int dir4)
e5119053f6 1:5a65fc20f8c2 35 {
e5119053f6 0:5e4f1e288e2a 36 rotateDir[0] = (dir1 > 0) - (dir1 < 0); // 1より大きい数値だった場合に，1か-1にするための処理
e5119053f6 0:5e4f1e288e2a 37 rotateDir[1] = (dir2 > 0) - (dir2 < 0);
e5119053f6 0:5e4f1e288e2a 38 rotateDir[2] = (dir3 > 0) - (dir3 < 0);
e5119053f6 0:5e4f1e288e2a 39 rotateDir[3] = (dir4 > 0) - (dir4 < 0);
e5119053f6 0:5e4f1e288e2a 40
e5119053f6 0:5e4f1e288e2a 41 init_done = false;
e5119053f6 0:5e4f1e288e2a 42 }
e5119053f6 0:5e4f1e288e2a 43
e5119053f6 1:5a65fc20f8c2 44 Platform::Platform(int dir1, int dir2, int dir3)
e5119053f6 1:5a65fc20f8c2 45 {
e5119053f6 0:5e4f1e288e2a 46 rotateDir[0] = (dir1 > 0) - (dir1 < 0); // 1より大きい数値だった場合に，1か-1にするための処理
e5119053f6 0:5e4f1e288e2a 47 rotateDir[1] = (dir2 > 0) - (dir2 < 0);
e5119053f6 0:5e4f1e288e2a 48 rotateDir[2] = (dir3 > 0) - (dir3 < 0);
e5119053f6 0:5e4f1e288e2a 49 rotateDir[3] = 1;
e5119053f6 0:5e4f1e288e2a 50 init_done = false;
e5119053f6 0:5e4f1e288e2a 51 }
e5119053f6 0:5e4f1e288e2a 52
e5119053f6 1:5a65fc20f8c2 53 Platform::Platform()
e5119053f6 1:5a65fc20f8c2 54 {
e5119053f6 0:5e4f1e288e2a 55 rotateDir[0] = 1; // 指示がないときはすべて1
e5119053f6 0:5e4f1e288e2a 56 rotateDir[1] = 1;
e5119053f6 0:5e4f1e288e2a 57 rotateDir[2] = 1;
e5119053f6 0:5e4f1e288e2a 58 rotateDir[3] = 1;
e5119053f6 0:5e4f1e288e2a 59
e5119053f6 0:5e4f1e288e2a 60 init_done = false;
e5119053f6 0:5e4f1e288e2a 61 }
e5119053f6 0:5e4f1e288e2a 62
e5119053f6 0:5e4f1e288e2a 63 // 自己位置推定の初期化
e5119053f6 1:5a65fc20f8c2 64 void Platform::platformInit(coords initPosi)
e5119053f6 1:5a65fc20f8c2 65 {
e5119053f6 0:5e4f1e288e2a 66 #if DRIVE_UNIT == PLATFORM_DUALWHEEL
e5119053f6 0:5e4f1e288e2a 67 SPI.begin(); // ここでSPIをbeginしてあげないとちゃんと動かなかった
e5119053f6 0:5e4f1e288e2a 68 SPI.setClockDivider(SPI_CLOCK_DIV16); //SPI通信のクロックを1MHzに設定 beginの後に置かないと，処理が止まる
e5119053f6 0:5e4f1e288e2a 69 stateamt203 = amt203.init();
e5119053f6 0:5e4f1e288e2a 70 #elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
e5119053f6 1:5a65fc20f8c2 71 printf("AMT222 initialization %d %d %d\r\n", absenc_1.init(), absenc_2.init(), absenc_3.init());
e5119053f6 1:5a65fc20f8c2 72 unit1.setposi(W1_x,W1_y);
e5119053f6 1:5a65fc20f8c2 73 unit2.setposi(W2_x,W2_y);
e5119053f6 1:5a65fc20f8c2 74 unit3.setposi(W3_x,W3_y);
e5119053f6 0:5e4f1e288e2a 75 #endif
e5119053f6 0:5e4f1e288e2a 76
e5119053f6 0:5e4f1e288e2a 77 //MD.begin(115200);
e5119053f6 0:5e4f1e288e2a 78
e5119053f6 0:5e4f1e288e2a 79 Posi = initPosi;
e5119053f6 0:5e4f1e288e2a 80
e5119053f6 0:5e4f1e288e2a 81 preEncX = 0;
e5119053f6 0:5e4f1e288e2a 82 preEncY = 0;
e5119053f6 0:5e4f1e288e2a 83 pre_angle_rad = 0.0;//Posi.z;
e5119053f6 0:5e4f1e288e2a 84 init_done = true;
e5119053f6 0:5e4f1e288e2a 85 }
e5119053f6 0:5e4f1e288e2a 86
e5119053f6 0:5e4f1e288e2a 87 // 自己位置推定値(Posi)を外部からセット
e5119053f6 1:5a65fc20f8c2 88 void Platform::setPosi(coords tempPosi)
e5119053f6 1:5a65fc20f8c2 89 {
e5119053f6 0:5e4f1e288e2a 90 Posi = tempPosi;
e5119053f6 0:5e4f1e288e2a 91 }
e5119053f6 0:5e4f1e288e2a 92
e5119053f6 0:5e4f1e288e2a 93 // エンコーダのカウント値と，ジャイロセンサから取得した角度をもとに自己位置を計算する
e5119053f6 1:5a65fc20f8c2 94 coords Platform::getPosi(int encX, int encY, double angle_rad)
e5119053f6 1:5a65fc20f8c2 95 {
e5119053f6 1:5a65fc20f8c2 96 if(init_done) {
e5119053f6 0:5e4f1e288e2a 97 // ローカル座標系での変化量を計算(zは角度)
e5119053f6 0:5e4f1e288e2a 98 coords diff;
e5119053f6 0:5e4f1e288e2a 99
e5119053f6 0:5e4f1e288e2a 100 // エンコーダのカウント値から角度の変化量を計算する
e5119053f6 0:5e4f1e288e2a 101 double angX, angY;
e5119053f6 0:5e4f1e288e2a 102 angX = (double)( encX - preEncX ) * _2PI_RES4;
e5119053f6 0:5e4f1e288e2a 103 angY = (double)( encY - preEncY ) * _2PI_RES4;
e5119053f6 1:5a65fc20f8c2 104
e5119053f6 0:5e4f1e288e2a 105 double angle_diff;
e5119053f6 0:5e4f1e288e2a 106 angle_diff = angle_rad - pre_angle_rad; // 角度の変化量を計算
e5119053f6 0:5e4f1e288e2a 107 diff.z = angle_diff;
e5119053f6 0:5e4f1e288e2a 108 diff.x = RADIUS_X * angX; //RADIUS_X はX軸エンコーダの車輪半径
e5119053f6 0:5e4f1e288e2a 109 diff.y = RADIUS_Y * angY; //RADIUS_Y はY軸エンコーダの車輪半径
e5119053f6 0:5e4f1e288e2a 110
e5119053f6 0:5e4f1e288e2a 111 // グローバル座標系での変化量に変換し，これまでのデータに加算することで自己位置推定完了
e5119053f6 0:5e4f1e288e2a 112 Posi.z += diff.z;
e5119053f6 0:5e4f1e288e2a 113 Posi.x += diff.x * cos( Posi.z ) - diff.y * sin( Posi.z );
e5119053f6 0:5e4f1e288e2a 114 Posi.y += diff.x * sin( Posi.z ) + diff.y * cos( Posi.z );
e5119053f6 1:5a65fc20f8c2 115
e5119053f6 0:5e4f1e288e2a 116 // 1サンプル前のデータとして今回取得したデータを格納
e5119053f6 0:5e4f1e288e2a 117 preEncX = encX;
e5119053f6 0:5e4f1e288e2a 118 preEncY = encY;
e5119053f6 0:5e4f1e288e2a 119 pre_angle_rad = angle_rad;
e5119053f6 0:5e4f1e288e2a 120 }
e5119053f6 0:5e4f1e288e2a 121 return Posi;
e5119053f6 0:5e4f1e288e2a 122 }
e5119053f6 0:5e4f1e288e2a 123
e5119053f6 1:5a65fc20f8c2 124 void Platform::VelocityControl(coords refV)
e5119053f6 1:5a65fc20f8c2 125 {
e5119053f6 1:5a65fc20f8c2 126 if(init_done) {
e5119053f6 1:5a65fc20f8c2 127 #if DRIVE_UNIT == PLATFORM_OMNI3WHEEL
e5119053f6 1:5a65fc20f8c2 128 double refOmegaA, refOmegaB, refOmegaC;
e5119053f6 1:5a65fc20f8c2 129 // 車輪反時計方向が正
e5119053f6 1:5a65fc20f8c2 130 refOmegaA = (-refV.y - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
e5119053f6 1:5a65fc20f8c2 131 refOmegaB = ( refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
e5119053f6 1:5a65fc20f8c2 132 refOmegaC = (-refV.x*COS_PI_6 + refV.y*SIN_PI_6 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
e5119053f6 0:5e4f1e288e2a 133
e5119053f6 1:5a65fc20f8c2 134 // RoboClawの指令値に変換
e5119053f6 1:5a65fc20f8c2 135 double mdCmdA, mdCmdB, mdCmdC;
e5119053f6 1:5a65fc20f8c2 136 mdCmdA = refOmegaA * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 137 mdCmdB = refOmegaB * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 138 mdCmdC = refOmegaC * _2RES_PI;
e5119053f6 0:5e4f1e288e2a 139
e5119053f6 1:5a65fc20f8c2 140 // モータにcmdを送り，回す
e5119053f6 1:5a65fc20f8c2 141 //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]);// 右前
e5119053f6 1:5a65fc20f8c2 142 //MD.SpeedM2(ADR_MD1, (int)mdCmdB * rotateDir[1]);// 左前
e5119053f6 1:5a65fc20f8c2 143 //MD.SpeedM2(ADR_MD2, (int)mdCmdC * rotateDir[2]);// 右後
e5119053f6 1:5a65fc20f8c2 144 #elif DRIVE_UNIT == PLATFORM_OMNI4WHEEL
e5119053f6 1:5a65fc20f8c2 145 double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
e5119053f6 1:5a65fc20f8c2 146 // 車輪反時計方向が正
e5119053f6 1:5a65fc20f8c2 147 refOmegaA = ( refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO; // [rad/s]
e5119053f6 1:5a65fc20f8c2 148 refOmegaB = ( refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
e5119053f6 1:5a65fc20f8c2 149 refOmegaC = (-refV.x * COS_PI_4 + refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
e5119053f6 1:5a65fc20f8c2 150 refOmegaD = (-refV.x * COS_PI_4 - refV.y * SIN_PI_4 - refV.z * DIST2WHEEL) / WHEEL_R * GEARRATIO;
e5119053f6 0:5e4f1e288e2a 151
e5119053f6 1:5a65fc20f8c2 152 // RoboClawの指令値に変換
e5119053f6 1:5a65fc20f8c2 153 double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
e5119053f6 1:5a65fc20f8c2 154 mdCmdA = refOmegaA * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 155 mdCmdB = refOmegaB * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 156 mdCmdC = refOmegaC * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 157 mdCmdD = refOmegaD * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 158
e5119053f6 1:5a65fc20f8c2 159 //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]); // 左前 ①
e5119053f6 1:5a65fc20f8c2 160 //MD.SpeedM2(ADR_MD1, (int)mdCmdC * rotateDir[1]); // 左後 ②
e5119053f6 1:5a65fc20f8c2 161 //MD.SpeedM1(ADR_MD2, (int)mdCmdD * rotateDir[2]); // 右後 ③
e5119053f6 1:5a65fc20f8c2 162 //MD.SpeedM2(ADR_MD2, (int)mdCmdB * rotateDir[3]); // 右前 ④
e5119053f6 1:5a65fc20f8c2 163 #elif DRIVE_UNIT == PLATFORM_DUALWHEEL
e5119053f6 1:5a65fc20f8c2 164 // ターンテーブルの角度取得
e5119053f6 1:5a65fc20f8c2 165 double thetaDuEnc, thetaDu;
e5119053f6 1:5a65fc20f8c2 166 static double preThetaDuEnc = thetaDuEnc;
e5119053f6 1:5a65fc20f8c2 167 thetaDuEnc = amt203.getEncount();
e5119053f6 1:5a65fc20f8c2 168 if( thetaDuEnc == -1 ) {
e5119053f6 1:5a65fc20f8c2 169 thetaDuEnc = preThetaDuEnc; // -1はエラーなので，前の値を格納しておく
e5119053f6 1:5a65fc20f8c2 170 }
e5119053f6 1:5a65fc20f8c2 171 preThetaDuEnc = thetaDuEnc;
e5119053f6 1:5a65fc20f8c2 172 thetaDu = (double)thetaDuEnc*2*PI / TT_RES4; // 角度に変換
e5119053f6 0:5e4f1e288e2a 173
e5119053f6 1:5a65fc20f8c2 174 // 車輪やターンテーブルの指令速度を計算
e5119053f6 1:5a65fc20f8c2 175 double cosDu, sinDu, refOmegaR, refOmegaL, refOmegaT;
e5119053f6 1:5a65fc20f8c2 176 cosDu = cos(thetaDu);
e5119053f6 1:5a65fc20f8c2 177 sinDu = sin(thetaDu);
e5119053f6 1:5a65fc20f8c2 178 refOmegaR = ( ( cosDu - sinDu ) * refV.x + ( sinDu + cosDu ) * refV.y ) / RADIUS_R;// right
e5119053f6 1:5a65fc20f8c2 179 refOmegaL = ( ( cosDu + sinDu ) * refV.x + ( sinDu - cosDu ) * refV.y ) / RADIUS_L;// left
e5119053f6 1:5a65fc20f8c2 180 refOmegaT = ( - ( 2 * sinDu / W ) * refV.x + ( 2 * cosDu / W ) * refV.y - refV.z ) * GEARRATIO;// turntable
e5119053f6 1:5a65fc20f8c2 181
e5119053f6 1:5a65fc20f8c2 182 // RoboClawの指令値に変換
e5119053f6 1:5a65fc20f8c2 183 double mdCmdR, mdCmdL, mdCmdT;
e5119053f6 1:5a65fc20f8c2 184 mdCmdR = refOmegaR * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 185 mdCmdL = refOmegaL * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 186 mdCmdT = refOmegaT * _2RES_PI_T;
e5119053f6 0:5e4f1e288e2a 187
e5119053f6 1:5a65fc20f8c2 188 // モータにcmdを送り，回す
e5119053f6 1:5a65fc20f8c2 189 //MD.SpeedM1(ADR_MD1, -(int)mdCmdR);// 右車輪
e5119053f6 1:5a65fc20f8c2 190 //MD.SpeedM2(ADR_MD1, (int)mdCmdL);// 左車輪
e5119053f6 1:5a65fc20f8c2 191 //MD.SpeedM1(ADR_MD2, (int)mdCmdT);// ターンテーブル
e5119053f6 1:5a65fc20f8c2 192 #elif DRIVE_UNIT == PLATFORM_MECHANUM
e5119053f6 1:5a65fc20f8c2 193 double refOmegaA, refOmegaB, refOmegaC, refOmegaD;
e5119053f6 1:5a65fc20f8c2 194 // 車輪の軸まわりで見て，反時計方向が正
e5119053f6 1:5a65fc20f8c2 195 refOmegaA = ( refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左前
e5119053f6 1:5a65fc20f8c2 196 refOmegaB = ( refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 左後
e5119053f6 1:5a65fc20f8c2 197 refOmegaC = (-refV.x + refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右後
e5119053f6 1:5a65fc20f8c2 198 refOmegaD = (-refV.x - refV.y + refV.z * ( TREAD_2 + WHEELBASE_2 ) ) / WHEEL_R;// 右前
e5119053f6 0:5e4f1e288e2a 199
e5119053f6 1:5a65fc20f8c2 200 // RoboClawの指令値に変換
e5119053f6 1:5a65fc20f8c2 201 double mdCmdA, mdCmdB, mdCmdC, mdCmdD;
e5119053f6 1:5a65fc20f8c2 202 mdCmdA = refOmegaA * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 203 mdCmdB = refOmegaB * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 204 mdCmdC = refOmegaC * _2RES_PI;
e5119053f6 1:5a65fc20f8c2 205 mdCmdD = refOmegaD * _2RES_PI;
e5119053f6 0:5e4f1e288e2a 206
e5119053f6 1:5a65fc20f8c2 207 //MD.SpeedM1(ADR_MD1, (int)mdCmdA * rotateDir[0]);// 左前
e5119053f6 1:5a65fc20f8c2 208 //MD.SpeedM2(ADR_MD1, (int)mdCmdB * rotateDir[1]);// 左後
e5119053f6 1:5a65fc20f8c2 209 //MD.SpeedM1(ADR_MD2, (int)mdCmdC * rotateDir[2]);// 右後
e5119053f6 1:5a65fc20f8c2 210 //MD.SpeedM2(ADR_MD2, (int)mdCmdD * rotateDir[3]);// 右前
e5119053f6 1:5a65fc20f8c2 211
e5119053f6 1:5a65fc20f8c2 212 #elif DRIVE_UNIT == PLATFORM_ACTIVECASTER3WHEEL
e5119053f6 1:5a65fc20f8c2 213 double stear_1_rad = (double)absenc_1.getEncount() / ABS_ENCRES * PI * -2;
e5119053f6 1:5a65fc20f8c2 214 double stear_2_rad = (double)absenc_2.getEncount() / ABS_ENCRES * PI * -2;
e5119053f6 1:5a65fc20f8c2 215 double stear_3_rad = (double)absenc_3.getEncount() / ABS_ENCRES * PI * -2;
e5119053f6 1:5a65fc20f8c2 216
e5119053f6 1:5a65fc20f8c2 217 unit1.control(refV.x,refV.y,refV.z,stear_1_rad);
e5119053f6 1:5a65fc20f8c2 218 unit2.control(refV.x,refV.y,refV.z,stear_2_rad);
e5119053f6 1:5a65fc20f8c2 219 unit3.control(refV.x,refV.y,refV.z,stear_3_rad);
e5119053f6 1:5a65fc20f8c2 220
e5119053f6 1:5a65fc20f8c2 221
e5119053f6 1:5a65fc20f8c2 222 #endif
e5119053f6 0:5e4f1e288e2a 223 }
e5119053f6 0:5e4f1e288e2a 224 }