2020_TeamA / Mbed 2 deprecated Yoshida_MiniRobo

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.

Dependencies:   mbed YKNCT_Movement SBDBT BNO055 YKNCT_MD YKNCT_I2C

main.cpp@7:8bea84f72e64, 2020-03-16 (annotated)

Committer:
yoshidayuito
Date:
Mon Mar 16 04:25:24 2020 +0000
Revision:
7:8bea84f72e64
Parent:
6:fa9121fe4a53
Child:
8:367c7d6ef5a3
add Wall;Ver.yoshida

Who changed what in which revision?

UserRevisionLine numberNew contents of line
TakushimaYukimasa 0:9e851dc42cde 1 /**
TakushimaYukimasa 0:9e851dc42cde 2 ********************************************************************************
TakushimaYukimasa 0:9e851dc42cde 3 * @file main.c
TakushimaYukimasa 0:9e851dc42cde 4 * @author You
TakushimaYukimasa 0:9e851dc42cde 5 * @version V?.?.?
TakushimaYukimasa 0:9e851dc42cde 6 * @date Today
TakushimaYukimasa 0:9e851dc42cde 7 * @brief メインファイル
TakushimaYukimasa 0:9e851dc42cde 8 ******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 9 */
TakushimaYukimasa 0:9e851dc42cde 10
TakushimaYukimasa 0:9e851dc42cde 11 /* Includes ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 12 #include <main.h>
TakushimaYukimasa 0:9e851dc42cde 13
TakushimaYukimasa 0:9e851dc42cde 14 /* 型定義 --------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 15
TakushimaYukimasa 0:9e851dc42cde 16 /* ロボットの加速度 */
TakushimaYukimasa 0:9e851dc42cde 17 ROCATION NowAcc;
TakushimaYukimasa 0:9e851dc42cde 18 /* ロボットの座標 */
TakushimaYukimasa 0:9e851dc42cde 19 ROCATION NowLoc;
TakushimaYukimasa 0:9e851dc42cde 20
TakushimaYukimasa 0:9e851dc42cde 21 /* 定数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 22 /* マクロ定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 23 /* 関数プロトタイプ宣言 -------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 24
TakushimaYukimasa 0:9e851dc42cde 25 /* タイマ呼び出し用 */
TakushimaYukimasa 0:9e851dc42cde 26 void IT_CallBack(void);
TakushimaYukimasa 0:9e851dc42cde 27
TakushimaYukimasa 0:9e851dc42cde 28 /* 自己位置推定処理 */
TakushimaYukimasa 0:9e851dc42cde 29 void LocEstimate(void);
TakushimaYukimasa 0:9e851dc42cde 30
yoshidayuito 7:8bea84f72e64 31 /* 壁あて */
yoshidayuito 7:8bea84f72e64 32 void ToWall(void);
TakushimaYukimasa 0:9e851dc42cde 33
TakushimaYukimasa 0:9e851dc42cde 34 /* 変数定義 ------------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 35
TakushimaYukimasa 0:9e851dc42cde 36 /* 操作権 0…なし 1…手動 2…自動 */
TakushimaYukimasa 0:9e851dc42cde 37 int operate=0;
TakushimaYukimasa 0:9e851dc42cde 38
TakushimaYukimasa 0:9e851dc42cde 39 /* 自動シーケンス */
TakushimaYukimasa 0:9e851dc42cde 40 int auto_mode=0;
TakushimaYukimasa 0:9e851dc42cde 41
TakushimaYukimasa 0:9e851dc42cde 42 /* 直読みエンコーダ角度保存(degree) */
TakushimaYukimasa 0:9e851dc42cde 43 double EncoderDeg[EncoderMAX] = {0};
TakushimaYukimasa 0:9e851dc42cde 44
TakushimaYukimasa 0:9e851dc42cde 45 /* 足回り値保存変数 */
yoshidayuito 7:8bea84f72e64 46 int MovMotor[4]= {0};
TakushimaYukimasa 0:9e851dc42cde 47
TakushimaYukimasa 0:9e851dc42cde 48 /* 自動yaw補整目標角度 */
TakushimaYukimasa 0:9e851dc42cde 49 double TarTheta=0;
TakushimaYukimasa 0:9e851dc42cde 50
TakushimaYukimasa 0:9e851dc42cde 51 /* クラス定義 ----------------------------------------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 52
TakushimaYukimasa 0:9e851dc42cde 53 /* 割り込み用クラス */
TakushimaYukimasa 0:9e851dc42cde 54 Ticker flipper;
TakushimaYukimasa 0:9e851dc42cde 55
TakushimaYukimasa 0:9e851dc42cde 56 /* UART (Tx,Rx) */
TakushimaYukimasa 0:9e851dc42cde 57 Serial telemetry(USBTX, USBRX, 115200);
TakushimaYukimasa 0:9e851dc42cde 58
TakushimaYukimasa 0:9e851dc42cde 59 /* コントローラー */
TakushimaYukimasa 0:9e851dc42cde 60 SBDBT DS3(PA_0, PA_1, 9600);
TakushimaYukimasa 0:9e851dc42cde 61
TakushimaYukimasa 0:9e851dc42cde 62 /* オンボードLED */
TakushimaYukimasa 0:9e851dc42cde 63 DigitalOut led(LED2);
TakushimaYukimasa 0:9e851dc42cde 64
TakushimaYukimasa 0:9e851dc42cde 65 /* USERボタン */
TakushimaYukimasa 0:9e851dc42cde 66 DigitalIn UB(PC_13,PullDown);
TakushimaYukimasa 0:9e851dc42cde 67
TakushimaYukimasa 0:9e851dc42cde 68 /* エンコーダーピン CS */
TakushimaYukimasa 0:9e851dc42cde 69 DigitalOut CS[] = {PA_2,PA_3};
TakushimaYukimasa 0:9e851dc42cde 70 DigitalOut CL[] = {PA_4,PA_5};
TakushimaYukimasa 0:9e851dc42cde 71 DigitalIn DO[] = {PA_6,PA_7};
TakushimaYukimasa 0:9e851dc42cde 72
yoshidayuito 7:8bea84f72e64 73 /* 壁あてリミット */
yoshidayuito 7:8bea84f72e64 74 DigitalIn limL(PA_0);
yoshidayuito 7:8bea84f72e64 75 DigitalIn limR(PA_1);
yoshidayuito 7:8bea84f72e64 76
yoshidayuito 1:520233a969e2 77 /* ジャイロ用タイマー */
yoshidayuito 1:520233a969e2 78 Timer yawCnt;
yoshidayuito 1:520233a969e2 79
TakushimaYukimasa 0:9e851dc42cde 80 /* 足回り動作クラス定義 */
TakushimaYukimasa 0:9e851dc42cde 81 Move omuni(MovMotor,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 82
TakushimaYukimasa 0:9e851dc42cde 83 /* I2C MDのクラス定義 */
TakushimaYukimasa 0:9e851dc42cde 84 YKNCT_MD_I2C MD(PB_9,PB_8);
TakushimaYukimasa 0:9e851dc42cde 85
TakushimaYukimasa 0:9e851dc42cde 86 /* ジャイロのピン設定 */
TakushimaYukimasa 0:9e851dc42cde 87 BNO055 bno(PB_9, PB_8);
TakushimaYukimasa 0:9e851dc42cde 88
TakushimaYukimasa 0:9e851dc42cde 89
TakushimaYukimasa 0:9e851dc42cde 90 /*----------------------------------- main ----------------------------------*/
TakushimaYukimasa 0:9e851dc42cde 91 int main()
TakushimaYukimasa 0:9e851dc42cde 92 {
yoshidayuito 7:8bea84f72e64 93 telemetry.printf("\n\rMainStart");
TakushimaYukimasa 0:9e851dc42cde 94
yoshidayuito 7:8bea84f72e64 95 /* 割り込みの設定
yoshidayuito 7:8bea84f72e64 96 * IT_CallBack関数を0.1msで割り込み */
yoshidayuito 7:8bea84f72e64 97 flipper.attach_us(&IT_CallBack, 100);
TakushimaYukimasa 0:9e851dc42cde 98
yoshidayuito 7:8bea84f72e64 99 /* ジャイロの設定 */
yoshidayuito 7:8bea84f72e64 100 bno.setmode(OPERATION_MODE_IMUPLUS);
TakushimaYukimasa 0:9e851dc42cde 101
yoshidayuito 7:8bea84f72e64 102 /* I2CMDの設定 */
yoshidayuito 7:8bea84f72e64 103 MD.Init(0,MD_SMB);
yoshidayuito 7:8bea84f72e64 104 MD.Init(1,MD_SMB);
yoshidayuito 7:8bea84f72e64 105 MD.Init(2,MD_SMB);
yoshidayuito 7:8bea84f72e64 106 MD.Init(3,MD_SMB);
TakushimaYukimasa 0:9e851dc42cde 107
yoshidayuito 7:8bea84f72e64 108 telemetry.printf("\n\rMainLoopStart");
yoshidayuito 7:8bea84f72e64 109 /* メインループ --------------------------------------------------------------*/
yoshidayuito 7:8bea84f72e64 110 while(1) {
yoshidayuito 7:8bea84f72e64 111 /* オンボードLED点滅 */
yoshidayuito 7:8bea84f72e64 112 led=!led;
yoshidayuito 7:8bea84f72e64 113
yoshidayuito 7:8bea84f72e64 114 /* 表示改行 */
yoshidayuito 7:8bea84f72e64 115 telemetry.printf("\n\r");
TakushimaYukimasa 0:9e851dc42cde 116
yoshidayuito 7:8bea84f72e64 117 /* 自動処理関連テレメトリ */
yoshidayuito 7:8bea84f72e64 118 telemetry.printf("ope:%d ",operate);
yoshidayuito 7:8bea84f72e64 119 /* 座標テレメトリ */
yoshidayuito 7:8bea84f72e64 120 telemetry.printf("X:%4.0f Y:%4.0f T:%4.0f ",NowLoc.X,NowLoc.Y,NowLoc.theta);
TakushimaYukimasa 0:9e851dc42cde 121
yoshidayuito 7:8bea84f72e64 122 /* 自己位置推定更新 */
yoshidayuito 7:8bea84f72e64 123 LocEstimate();
TakushimaYukimasa 0:9e851dc42cde 124
yoshidayuito 7:8bea84f72e64 125 /* 壁あて実行 */
yoshidayuito 7:8bea84f72e64 126 ToWall();
TakushimaYukimasa 0:9e851dc42cde 127
yoshidayuito 7:8bea84f72e64 128 /* I2CMD実行 */
yoshidayuito 7:8bea84f72e64 129 MD.Exe();
TakushimaYukimasa 0:9e851dc42cde 130
yoshidayuito 7:8bea84f72e64 131 /* タイマースタート */
yoshidayuito 7:8bea84f72e64 132 yawCnt.start();
yoshidayuito 7:8bea84f72e64 133 /* タイマーリセット */
yoshidayuito 7:8bea84f72e64 134 yawCnt.reset();
yoshidayuito 7:8bea84f72e64 135
yoshidayuito 7:8bea84f72e64 136 /* 操縦権変更 ×停止 △手動 〇自動 */
yoshidayuito 7:8bea84f72e64 137 if(DS3.CROSS) operate=0;
yoshidayuito 7:8bea84f72e64 138 if(DS3.TRIANGLE) operate=1;
yoshidayuito 7:8bea84f72e64 139 if(DS3.CIRCLE) operate=2;
TakushimaYukimasa 0:9e851dc42cde 140
yoshidayuito 7:8bea84f72e64 141 /* 操縦権：なし 停止動作 */
yoshidayuito 7:8bea84f72e64 142 if(operate==0) {
yoshidayuito 7:8bea84f72e64 143 /* 足回り停止 */
yoshidayuito 7:8bea84f72e64 144 omuni.XmarkOmni_Move(0,0,0);
yoshidayuito 7:8bea84f72e64 145
yoshidayuito 7:8bea84f72e64 146 for(int i=0; i<4; i++)
yoshidayuito 7:8bea84f72e64 147 MD.Set(i,MovMotor[i]);
yoshidayuito 7:8bea84f72e64 148 }
yoshidayuito 7:8bea84f72e64 149 /* 操縦権：手動 */
yoshidayuito 7:8bea84f72e64 150 else if(operate==1) {
yoshidayuito 7:8bea84f72e64 151 /* 足回り手動動作 */
yoshidayuito 7:8bea84f72e64 152 int x_val = (double)(DS3.LX-64)*100/64;
yoshidayuito 7:8bea84f72e64 153 int y_val = (double)(64-DS3.LY)*100/64;
yoshidayuito 7:8bea84f72e64 154 int r_val = (double)(DS3.RX-64)*100/64;
yoshidayuito 7:8bea84f72e64 155
yoshidayuito 7:8bea84f72e64 156 /* タイマーをリセット */
yoshidayuito 7:8bea84f72e64 157 if(DS3.RX!=64) yawCnt.reset();
yoshidayuito 7:8bea84f72e64 158 /* 目標角度再設定 */
yoshidayuito 7:8bea84f72e64 159 if(yawCnt.read_ms()<1000) TarTheta=NowLoc.theta;
yoshidayuito 7:8bea84f72e64 160 /* r_val補正 */
yoshidayuito 7:8bea84f72e64 161 r_val+=(TarTheta-NowLoc.theta)*CONST_CORRECTION_YAW;
yoshidayuito 7:8bea84f72e64 162
yoshidayuito 7:8bea84f72e64 163 omuni.XmarkOmni_Move(x_val,y_val,r_val);
yoshidayuito 7:8bea84f72e64 164
yoshidayuito 7:8bea84f72e64 165 for(int i=0; i<4; i++)
yoshidayuito 7:8bea84f72e64 166 MD.Set(i,MovMotor[i]);
yoshidayuito 7:8bea84f72e64 167 }
yoshidayuito 7:8bea84f72e64 168 /* 操縦権：自動 */
yoshidayuito 7:8bea84f72e64 169 else if(operate==2) {
yoshidayuito 7:8bea84f72e64 170 switch(auto_mode) {
yoshidayuito 7:8bea84f72e64 171 /* スタート待機処理 */
yoshidayuito 7:8bea84f72e64 172 case 0:
yoshidayuito 7:8bea84f72e64 173 /* オンボードSWで次のステップに */
yoshidayuito 7:8bea84f72e64 174 if(UB) auto_mode++;
yoshidayuito 7:8bea84f72e64 175 break;
yoshidayuito 7:8bea84f72e64 176
yoshidayuito 7:8bea84f72e64 177 /* 〇〇の処理 */
yoshidayuito 7:8bea84f72e64 178 case 1:
yoshidayuito 7:8bea84f72e64 179 /* 〇〇の時次のステップに */
yoshidayuito 7:8bea84f72e64 180 if(1) auto_mode++;
yoshidayuito 7:8bea84f72e64 181 break;
yoshidayuito 7:8bea84f72e64 182
yoshidayuito 7:8bea84f72e64 183 /* 終了処理 */
yoshidayuito 7:8bea84f72e64 184 default:
yoshidayuito 7:8bea84f72e64 185 auto_mode=0;
yoshidayuito 7:8bea84f72e64 186 operate=0;
yoshidayuito 7:8bea84f72e64 187 break;
yoshidayuito 7:8bea84f72e64 188 }
yoshidayuito 7:8bea84f72e64 189 }
yoshidayuito 7:8bea84f72e64 190
TakushimaYukimasa 0:9e851dc42cde 191 }
TakushimaYukimasa 0:9e851dc42cde 192 }
TakushimaYukimasa 0:9e851dc42cde 193
TakushimaYukimasa 0:9e851dc42cde 194
TakushimaYukimasa 0:9e851dc42cde 195 /*******************************************************************************
TakushimaYukimasa 0:9e851dc42cde 196 * @概要 自己位置推定関数
TakushimaYukimasa 0:9e851dc42cde 197 * @引数 なし
TakushimaYukimasa 0:9e851dc42cde 198 * @返り値 なし
TakushimaYukimasa 0:9e851dc42cde 199 *******************************************************************************/
yoshidayuito 7:8bea84f72e64 200 void LocEstimate(void)
yoshidayuito 7:8bea84f72e64 201 {
yoshidayuito 7:8bea84f72e64 202 static double GyroDeg[2]= {0};
yoshidayuito 7:8bea84f72e64 203 static double EncDeg[2][2]= {0};
yoshidayuito 7:8bea84f72e64 204 static double disp[3]= {0};
TakushimaYukimasa 0:9e851dc42cde 205
yoshidayuito 7:8bea84f72e64 206 /* ジャイロの値取得 */
yoshidayuito 7:8bea84f72e64 207 bno.get_angles();
yoshidayuito 7:8bea84f72e64 208 GyroDeg[1]=GyroDeg[0];
yoshidayuito 7:8bea84f72e64 209 GyroDeg[0]=bno.euler.yaw;
yoshidayuito 7:8bea84f72e64 210 if(GyroDeg[0]!=0) {
yoshidayuito 7:8bea84f72e64 211 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshidayuito 7:8bea84f72e64 212 if(GyroDeg[1]<90 && GyroDeg[0]>270) GyroDeg[1]+=360;
yoshidayuito 7:8bea84f72e64 213 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshidayuito 7:8bea84f72e64 214 else if(GyroDeg[1]>270 && GyroDeg[0]<90) GyroDeg[1]-=360;
yoshidayuito 7:8bea84f72e64 215 /* 差を求める*/
yoshidayuito 7:8bea84f72e64 216 disp[2]=GyroDeg[1]-GyroDeg[0];
yoshidayuito 7:8bea84f72e64 217 }
yoshidayuito 7:8bea84f72e64 218 /* Enc2つの差分求める */
yoshidayuito 7:8bea84f72e64 219 for(int i=0; i<2; i++) {
yoshidayuito 7:8bea84f72e64 220 EncDeg[i][1]=EncDeg[i][0];
yoshidayuito 7:8bea84f72e64 221 EncDeg[i][0]=EncoderDeg[i];
yoshidayuito 7:8bea84f72e64 222 disp[i]=EncDeg[i][1]-EncDeg[i][0];
yoshidayuito 7:8bea84f72e64 223 }
yoshidayuito 7:8bea84f72e64 224 /* 差分を加速度として保存 */
yoshidayuito 7:8bea84f72e64 225 NowAcc.theta = disp[2];
yoshidayuito 7:8bea84f72e64 226 NowAcc.X = disp[0] * cos(NowLoc.theta) + disp[1] * sin(NowLoc.theta)+200*disp[2];
yoshidayuito 7:8bea84f72e64 227 NowAcc.Y = -disp[0] * sin(NowLoc.theta) + disp[1] * cos(NowLoc.theta)+200*disp[2];
yoshidayuito 7:8bea84f72e64 228 /* 差分を累積して現在位置を保存 */
yoshidayuito 7:8bea84f72e64 229 NowLoc.X += NowAcc.X;
yoshidayuito 7:8bea84f72e64 230 NowLoc.Y += NowAcc.Y;
yoshidayuito 7:8bea84f72e64 231 NowLoc.theta += NowAcc.theta;
TakushimaYukimasa 0:9e851dc42cde 232 }
TakushimaYukimasa 0:9e851dc42cde 233
TakushimaYukimasa 0:9e851dc42cde 234
TakushimaYukimasa 0:9e851dc42cde 235
TakushimaYukimasa 0:9e851dc42cde 236
TakushimaYukimasa 0:9e851dc42cde 237 /* 割り込み(100us) *************************************************************/
TakushimaYukimasa 0:9e851dc42cde 238 void IT_CallBack(void)
TakushimaYukimasa 0:9e851dc42cde 239 {
yoshidayuito 7:8bea84f72e64 240 static int cnt = 0;
yoshidayuito 7:8bea84f72e64 241 static int data[EncoderMAX] = {0};
yoshidayuito 7:8bea84f72e64 242 static double EncDeg[EncoderMAX][2] = {0};
TakushimaYukimasa 0:9e851dc42cde 243
yoshidayuito 7:8bea84f72e64 244 for(int i=0; i<EncoderMAX; i++)
yoshidayuito 7:8bea84f72e64 245 switch(cnt) {
yoshidayuito 7:8bea84f72e64 246 /* 最初の処理 */
yoshidayuito 7:8bea84f72e64 247 case 0:
yoshidayuito 7:8bea84f72e64 248 data[i] = 0;
yoshidayuito 7:8bea84f72e64 249 CS[i] = 0;
yoshidayuito 7:8bea84f72e64 250 CL[i] = 1;
yoshidayuito 7:8bea84f72e64 251 break;
yoshidayuito 7:8bea84f72e64 252 /* 最後の処理 */
yoshidayuito 7:8bea84f72e64 253 case 25:
yoshidayuito 7:8bea84f72e64 254 CS[i]=1;
yoshidayuito 7:8bea84f72e64 255 /* 前回の値更新 今回の値更新(エンコーダの値(0~4096)を角度(0~360)に) */
yoshidayuito 7:8bea84f72e64 256 EncDeg[i][1] = EncDeg[i][0];
yoshidayuito 7:8bea84f72e64 257 EncDeg[i][0] = (double)data[i] * 360.0 / 4096;
yoshidayuito 7:8bea84f72e64 258 /* 359→0を跨いだ時，前回の値を0から逆回転で負の値で表記 */
yoshidayuito 7:8bea84f72e64 259 if ((270 <= EncDeg[i][1]) && (EncDeg[i][0] < 90))
yoshidayuito 7:8bea84f72e64 260 EncDeg[i][1] -= 360;
yoshidayuito 7:8bea84f72e64 261 /* 0→359を跨いだ時，前回の値を360以上の値で表記 */
yoshidayuito 7:8bea84f72e64 262 else if ((EncDeg[i][1] < 90) && (270 <= EncDeg[i][0]))
yoshidayuito 7:8bea84f72e64 263 EncDeg[i][1] += 360;
yoshidayuito 7:8bea84f72e64 264 /* 差を求める*/
yoshidayuito 7:8bea84f72e64 265 EncoderDeg[i] += EncDeg[i][0] - EncDeg[i][1];
yoshidayuito 7:8bea84f72e64 266 break;
yoshidayuito 7:8bea84f72e64 267 /* 通常の処理 */
yoshidayuito 7:8bea84f72e64 268 default:
yoshidayuito 7:8bea84f72e64 269 CL[i]=!CL[i];
yoshidayuito 7:8bea84f72e64 270 /* 最初でも最後でもなく奇数回で最初以外の時読み取り処理 */
yoshidayuito 7:8bea84f72e64 271 if(cnt != 1 && cnt % 2) {
yoshidayuito 7:8bea84f72e64 272 data[i] |= (DO[i]==1);
yoshidayuito 7:8bea84f72e64 273 data[i] = data[i] << 1;
yoshidayuito 7:8bea84f72e64 274 }
yoshidayuito 7:8bea84f72e64 275 break;
yoshidayuito 7:8bea84f72e64 276 }
yoshidayuito 7:8bea84f72e64 277 cnt++;
yoshidayuito 7:8bea84f72e64 278 cnt%=26;
yoshidayuito 7:8bea84f72e64 279 }
yoshidayuito 7:8bea84f72e64 280
yoshidayuito 7:8bea84f72e64 281
yoshidayuito 7:8bea84f72e64 282
yoshidayuito 7:8bea84f72e64 283
yoshidayuito 7:8bea84f72e64 284 /*******************************************************************************
yoshidayuito 7:8bea84f72e64 285 * @概要 壁あて関数
yoshidayuito 7:8bea84f72e64 286 * @引数 なし
yoshidayuito 7:8bea84f72e64 287 * @返り値 なし
yoshidayuito 7:8bea84f72e64 288 *******************************************************************************/
yoshidayuito 7:8bea84f72e64 289 void ToWall(void)
yoshidayuito 7:8bea84f72e64 290 {
yoshidayuito 7:8bea84f72e64 291 if(limL==0)
yoshidayuito 7:8bea84f72e64 292 for(int i=0; i<2; i++) MovMotor[i]-=10;
yoshidayuito 7:8bea84f72e64 293 else for(int i=0; i<2; i++) MovMotor[i]+=0;
yoshidayuito 7:8bea84f72e64 294 if(limR==0)
yoshidayuito 7:8bea84f72e64 295 for(int i=2; i<4; i++) MovMotor[i]+=10;
yoshidayuito 7:8bea84f72e64 296 else for(int i=2; i<4; i++) MovMotor[i]+=0;
TakushimaYukimasa 0:9e851dc42cde 297 }