Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@6:040d001acb12, 2019-08-24 (annotated)

Committer:
Tom0108
Date:
Sat Aug 24 06:06:36 2019 +0000
Revision:
6:040d001acb12
Parent:
5:fcc79e507610
Child:
8:b79d21c8178b
Positive control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Tom0108 0:761a63c6d020 1 #include "main.h"
Tom0108 0:761a63c6d020 2
Tom0108 0:761a63c6d020 3 /* 型定義 --------------------------------------------------------------------*/
Tom0108 0:761a63c6d020 4
Tom0108 0:761a63c6d020 5 /* 関数宣言 ------------------------------------------------------------------*/
Tom0108 0:761a63c6d020 6
Tom0108 0:761a63c6d020 7 /* 変数宣言 ------------------------------------------------------------------*/
Tom0108 1:199c4a71da88 8 //かご回転用
Tom0108 3:1063aa133b62 9 int kago=0, kago_f=0;
Tom0108 1:199c4a71da88 10
Tom0108 1:199c4a71da88 11 //コントローラーの状態を保存するため
Tom0108 1:199c4a71da88 12 int once=0;
Tom0108 1:199c4a71da88 13
Tom0108 1:199c4a71da88 14 //タオル引く用
Tom0108 3:1063aa133b62 15 int pull=0, pull_f=0;
Tom0108 1:199c4a71da88 16
Tom0108 4:236e5c58e8ee 17 //タオル掴む用
Tom0108 4:236e5c58e8ee 18 //sgrab_f: 中央のリミットでつかむ
Tom0108 4:236e5c58e8ee 19 int grab=0, grab_f=0, sgrab_f=0;
Tom0108 3:1063aa133b62 20
Tom0108 4:236e5c58e8ee 21 //タオル横展開用
Tom0108 3:1063aa133b62 22 int unfold_f=0;
Tom0108 3:1063aa133b62 23
Tom0108 6:040d001acb12 24 //y_deg: ジャイロの生の値
Tom0108 6:040d001acb12 25 //yaw: 実際に使う角度
Tom0108 6:040d001acb12 26 double y_deg[2], yaw=0;;
Tom0108 1:199c4a71da88 27
Tom0108 6:040d001acb12 28 //ジャイロさんぷりんぐ用
Tom0108 5:fcc79e507610 29 double sum=0, ave=0;
Tom0108 5:fcc79e507610 30 int offset_cnt=0;
Tom0108 3:1063aa133b62 31
Tom0108 3:1063aa133b62 32 //Turn_val: 補正の目標値
Tom0108 3:1063aa133b62 33 //dif_val: 補正値との差
Tom0108 3:1063aa133b62 34 double Turn_val, dif_val;
Tom0108 3:1063aa133b62 35
Tom0108 3:1063aa133b62 36 //short_lim: 腕中央
Tom0108 3:1063aa133b62 37 //max_lim: 腕奥側
Tom0108 3:1063aa133b62 38 //short_lim: 腕手前側
Tom0108 3:1063aa133b62 39 int short_lim, max_lim, start_lim;
Tom0108 1:199c4a71da88 40
Tom0108 6:040d001acb12 41 //足回りのpwm値
Tom0108 6:040d001acb12 42 int duty[4];
Tom0108 4:236e5c58e8ee 43
Tom0108 6:040d001acb12 44 // 足回り
Tom0108 6:040d001acb12 45 int lx,ly,rx;
Tom0108 0:761a63c6d020 46
Tom0108 6:040d001acb12 47 int limit_up,limit_down; // リミットスイッチ
Tom0108 6:040d001acb12 48 int slide_start,slide_stop; //かごのリミット
Tom0108 0:761a63c6d020 49
Tom0108 0:761a63c6d020 50 int as=0;
Tom0108 0:761a63c6d020 51
Tom0108 0:761a63c6d020 52 /*----------------------------------- main -----------------------------------*/
Tom0108 0:761a63c6d020 53 int main()
Tom0108 0:761a63c6d020 54 {
Tom0108 0:761a63c6d020 55 //flipper1.attach(&flip1, 0.001); // 割り込み
Tom0108 5:fcc79e507610 56
Tom0108 0:761a63c6d020 57 while(1) {
Tom0108 1:199c4a71da88 58 //自動系のタイマースタート
Tom0108 1:199c4a71da88 59 tim.start();
Tom0108 6:040d001acb12 60 drift_tim.start();
Tom0108 0:761a63c6d020 61 mpu.loop();
Tom0108 0:761a63c6d020 62
Tom0108 6:040d001acb12 63 //ジャイロから値を取得
Tom0108 5:fcc79e507610 64 y_deg[1] = y_deg[0];
Tom0108 5:fcc79e507610 65 y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI;
Tom0108 6:040d001acb12 66
Tom0108 5:fcc79e507610 67 //180～-180をまたいだ時
Tom0108 5:fcc79e507610 68 if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360;
Tom0108 5:fcc79e507610 69 else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360;
Tom0108 0:761a63c6d020 70
Tom0108 5:fcc79e507610 71 if(sbdbt.State_check()) {
Tom0108 5:fcc79e507610 72 for(int i=0; i<7; i++) i2c.In(In_Data,i);
Tom0108 6:040d001acb12 73
Tom0108 5:fcc79e507610 74 //誤差をさんぷりんぐ
Tom0108 5:fcc79e507610 75 if(offset_cnt<1000) {
Tom0108 6:040d001acb12 76 PALETTE(RED);
Tom0108 5:fcc79e507610 77 if(offset_cnt!=0) {
Tom0108 5:fcc79e507610 78 sum+=(y_deg[0]-y_deg[1]);
Tom0108 5:fcc79e507610 79 ave=sum/offset_cnt;
Tom0108 5:fcc79e507610 80 }
Tom0108 5:fcc79e507610 81 offset_cnt++;
Tom0108 6:040d001acb12 82 } else {
Tom0108 6:040d001acb12 83 PALETTE(GREEN);
Tom0108 6:040d001acb12 84 yaw -= (y_deg[0]-y_deg[1])-ave;
Tom0108 6:040d001acb12 85 }
Tom0108 6:040d001acb12 86
Tom0108 6:040d001acb12 87 dif_val = yaw-Turn_val;
Tom0108 6:040d001acb12 88 //制限
Tom0108 6:040d001acb12 89 if(dif_val>=30) dif_val=30;
Tom0108 6:040d001acb12 90 else if(dif_val<=-30) dif_val=-30;
Tom0108 0:761a63c6d020 91
Tom0108 0:761a63c6d020 92 lx = ((64-sbdbt.LY)*100.0/64)*sin(yaw*M_PI/180)+((sbdbt.LX-64)*100.0/64)*cos(yaw*M_PI/180);
Tom0108 0:761a63c6d020 93 ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180);
Tom0108 6:040d001acb12 94 rx = (sbdbt.RX - 64)*30/64;
Tom0108 0:761a63c6d020 95
Tom0108 6:040d001acb12 96 //メカナム基本動作
Tom0108 6:040d001acb12 97 mecanum_Move(lx, ly, rx);
Tom0108 0:761a63c6d020 98
Tom0108 6:040d001acb12 99 //旋回している間タイマーをリセット
Tom0108 6:040d001acb12 100 if(sbdbt.RX != 64) drift_tim.reset();
Tom0108 3:1063aa133b62 101
Tom0108 6:040d001acb12 102 //旋回して慣性で動いた後の角度に補正する
Tom0108 6:040d001acb12 103 if(drift_tim.read_ms()<500) Turn_val=yaw;
Tom0108 6:040d001acb12 104
Tom0108 6:040d001acb12 105 //角度補正
Tom0108 6:040d001acb12 106 AngleCorrection(yaw, Turn_val);
Tom0108 0:761a63c6d020 107
Tom0108 3:1063aa133b62 108 /*---ハンガーかけるやつ----------------------------------------------*/
Tom0108 1:199c4a71da88 109 if(sbdbt.SELECT)as=1;//selectキー
Tom0108 0:761a63c6d020 110
Tom0108 0:761a63c6d020 111 if(as==1 && limit_up == 0) {
Tom0108 0:761a63c6d020 112 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 0:761a63c6d020 113 MD_SET_PWM(MD_Data, 5,50);
Tom0108 0:761a63c6d020 114 } else if(UPkey && limit_up == 0) {
Tom0108 0:761a63c6d020 115 as=0;
Tom0108 0:761a63c6d020 116 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 0:761a63c6d020 117 MD_SET_PWM(MD_Data, 5,50);
Tom0108 0:761a63c6d020 118 } else if(as==1 && limit_up == 1) {
Tom0108 0:761a63c6d020 119 as=0;
Tom0108 0:761a63c6d020 120 MD_SET_PWM(MD_Data,5,0);
Tom0108 0:761a63c6d020 121 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 0:761a63c6d020 122 } else if(DOWNkey && limit_down == 0) {
Tom0108 0:761a63c6d020 123 MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
Tom0108 0:761a63c6d020 124 MD_SET_PWM(MD_Data, 5,50);
Tom0108 0:761a63c6d020 125 } else {
Tom0108 0:761a63c6d020 126 MD_SET_PWM(MD_Data,5,0);
Tom0108 0:761a63c6d020 127 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 0:761a63c6d020 128 }
Tom0108 0:761a63c6d020 129
Tom0108 1:199c4a71da88 130 //タオルつかむ 四角
Tom0108 4:236e5c58e8ee 131 if(grab_f || sgrab_f) {
Tom0108 6:040d001acb12 132 PALETTE(BLUE);
Tom0108 1:199c4a71da88 133 switch(grab) {
Tom0108 1:199c4a71da88 134 //腕を前に動かす
Tom0108 1:199c4a71da88 135 case 0:
Tom0108 1:199c4a71da88 136 //爪開く
Tom0108 1:199c4a71da88 137 i2c.Out_Set(Out_Data,1,1);
Tom0108 1:199c4a71da88 138 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 4:236e5c58e8ee 139 MD_SET_PWM(MD_Data,4,40);
Tom0108 6:040d001acb12 140 //sgrab_fは途中のリミットで止まる
Tom0108 4:236e5c58e8ee 141 if((sgrab_f && short_lim) || max_lim==1) {
Tom0108 1:199c4a71da88 142 grab++;
Tom0108 1:199c4a71da88 143 tim.reset();
Tom0108 1:199c4a71da88 144 }
Tom0108 1:199c4a71da88 145 break;
Tom0108 0:761a63c6d020 146
Tom0108 1:199c4a71da88 147 case 1:
Tom0108 2:47954f05d32d 148 //腕停止
Tom0108 2:47954f05d32d 149 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 3:1063aa133b62 150 MD_SET_PWM(MD_Data,4,0);
Tom0108 2:47954f05d32d 151
Tom0108 1:199c4a71da88 152 //腕おろす
Tom0108 0:761a63c6d020 153 i2c.Out_Set(Out_Data,0,1);
Tom0108 3:1063aa133b62 154 if(tim.read_ms()>300) {
Tom0108 1:199c4a71da88 155 grab++;
Tom0108 1:199c4a71da88 156 tim.reset();
Tom0108 1:199c4a71da88 157 }
Tom0108 1:199c4a71da88 158 break;
Tom0108 0:761a63c6d020 159
Tom0108 1:199c4a71da88 160 case 2:
Tom0108 1:199c4a71da88 161 //爪閉じる
Tom0108 1:199c4a71da88 162 i2c.Out_Set(Out_Data,1,0);
Tom0108 3:1063aa133b62 163 if(tim.read_ms()>100) {
Tom0108 1:199c4a71da88 164 grab++;
Tom0108 0:761a63c6d020 165 tim.reset();
Tom0108 0:761a63c6d020 166 }
Tom0108 1:199c4a71da88 167 break;
Tom0108 1:199c4a71da88 168
Tom0108 1:199c4a71da88 169 case 3:
Tom0108 1:199c4a71da88 170 //腕上げる
Tom0108 1:199c4a71da88 171 i2c.Out_Set(Out_Data,0,0);
Tom0108 3:1063aa133b62 172 if(tim.read_ms()>300) {
Tom0108 3:1063aa133b62 173 grab++;
Tom0108 3:1063aa133b62 174 tim.reset();
Tom0108 3:1063aa133b62 175 }
Tom0108 3:1063aa133b62 176 break;
Tom0108 3:1063aa133b62 177
Tom0108 3:1063aa133b62 178 case 4:
Tom0108 1:199c4a71da88 179 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 3:1063aa133b62 180 MD_SET_PWM (MD_Data,4,60);
Tom0108 4:236e5c58e8ee 181 if(start_lim==1) {
Tom0108 4:236e5c58e8ee 182 grab_f=0;
Tom0108 4:236e5c58e8ee 183 sgrab_f=0;
Tom0108 4:236e5c58e8ee 184 }
Tom0108 1:199c4a71da88 185 break;
Tom0108 0:761a63c6d020 186 }
Tom0108 3:1063aa133b62 187 } //if(grab_f)
Tom0108 3:1063aa133b62 188
Tom0108 2:47954f05d32d 189 //タオル引く 丸
Tom0108 3:1063aa133b62 190 else if(pull_f) {
Tom0108 6:040d001acb12 191 PALETTE(CYAN);
Tom0108 1:199c4a71da88 192 switch(pull) {
Tom0108 1:199c4a71da88 193 //腕を前に動かす
Tom0108 1:199c4a71da88 194 case 0:
Tom0108 1:199c4a71da88 195 //爪閉じる
Tom0108 1:199c4a71da88 196 i2c.Out_Set(Out_Data,1,0);
Tom0108 0:761a63c6d020 197 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 3:1063aa133b62 198 MD_SET_PWM (MD_Data,4,60);
Tom0108 1:199c4a71da88 199 if(max_lim==1) {
Tom0108 1:199c4a71da88 200 pull++;
Tom0108 1:199c4a71da88 201 tim.reset();
Tom0108 1:199c4a71da88 202 }
Tom0108 1:199c4a71da88 203 break;
Tom0108 0:761a63c6d020 204
Tom0108 1:199c4a71da88 205 case 1:
Tom0108 2:47954f05d32d 206 //腕停止
Tom0108 2:47954f05d32d 207 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 2:47954f05d32d 208 MD_SET_PWM (MD_Data,4,0);
Tom0108 2:47954f05d32d 209
Tom0108 1:199c4a71da88 210 //腕おろす
Tom0108 0:761a63c6d020 211 i2c.Out_Set(Out_Data,0,1);
Tom0108 3:1063aa133b62 212 if(tim.read_ms()>300) {
Tom0108 1:199c4a71da88 213 pull++;
Tom0108 1:199c4a71da88 214 tim.reset();
Tom0108 0:761a63c6d020 215 }
Tom0108 1:199c4a71da88 216 break;
Tom0108 1:199c4a71da88 217
Tom0108 1:199c4a71da88 218 case 2:
Tom0108 3:1063aa133b62 219 if(tim.read_ms()>300) {
Tom0108 1:199c4a71da88 220 pull++;
Tom0108 0:761a63c6d020 221 tim.reset();
Tom0108 0:761a63c6d020 222 }
Tom0108 1:199c4a71da88 223 break;
Tom0108 1:199c4a71da88 224
Tom0108 1:199c4a71da88 225 case 3:
Tom0108 1:199c4a71da88 226 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 3:1063aa133b62 227 MD_SET_PWM (MD_Data,4,60);
Tom0108 3:1063aa133b62 228 if(start_lim==1) pull_f=0;
Tom0108 1:199c4a71da88 229 break;
Tom0108 0:761a63c6d020 230 }
Tom0108 3:1063aa133b62 231 } //if(pull_f)
Tom0108 3:1063aa133b62 232
Tom0108 3:1063aa133b62 233 //かごを倒す
Tom0108 3:1063aa133b62 234 else if(kago_f) {
Tom0108 6:040d001acb12 235 PALETTE(MAGENTA);
Tom0108 1:199c4a71da88 236 //腕の先を閉じておく
Tom0108 1:199c4a71da88 237 i2c.Out_Set(Out_Data,1,0);
Tom0108 0:761a63c6d020 238
Tom0108 1:199c4a71da88 239 switch(kago) {
Tom0108 1:199c4a71da88 240 //腕を前に動かす
Tom0108 1:199c4a71da88 241 case 0:
Tom0108 0:761a63c6d020 242 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 3:1063aa133b62 243 MD_SET_PWM (MD_Data,4,60);
Tom0108 2:47954f05d32d 244 if(max_lim==1) {
Tom0108 2:47954f05d32d 245 kago++;
Tom0108 2:47954f05d32d 246 tim.reset();
Tom0108 2:47954f05d32d 247 }
Tom0108 1:199c4a71da88 248 break;
Tom0108 1:199c4a71da88 249
Tom0108 1:199c4a71da88 250 case 1:
Tom0108 1:199c4a71da88 251 //腕停止
Tom0108 0:761a63c6d020 252 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 1:199c4a71da88 253 MD_SET_PWM (MD_Data,4,0);
Tom0108 0:761a63c6d020 254
Tom0108 1:199c4a71da88 255 //かご回転
Tom0108 1:199c4a71da88 256 MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
Tom0108 3:1063aa133b62 257 MD_SET_PWM (MD_Data,6,80);
Tom0108 2:47954f05d32d 258 if(slide_stop==1 || tim.read_ms()>3000) {
Tom0108 1:199c4a71da88 259 kago++;
Tom0108 2:47954f05d32d 260 tim.reset();
Tom0108 1:199c4a71da88 261 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 1:199c4a71da88 262 MD_SET_PWM (MD_Data,6,0);
Tom0108 1:199c4a71da88 263 }
Tom0108 1:199c4a71da88 264 break;
Tom0108 0:761a63c6d020 265
Tom0108 1:199c4a71da88 266 case 2:
Tom0108 3:1063aa133b62 267 if(tim.read_ms()>1000) {
Tom0108 2:47954f05d32d 268 kago++;
Tom0108 2:47954f05d32d 269 tim.reset();
Tom0108 2:47954f05d32d 270 }
Tom0108 1:199c4a71da88 271 break;
Tom0108 1:199c4a71da88 272
Tom0108 1:199c4a71da88 273 case 3:
Tom0108 1:199c4a71da88 274 MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
Tom0108 3:1063aa133b62 275 MD_SET_PWM (MD_Data,6,80);
Tom0108 2:47954f05d32d 276 if(slide_start==1 || tim.read_ms()>3000) kago++;
Tom0108 1:199c4a71da88 277 break;
Tom0108 1:199c4a71da88 278
Tom0108 1:199c4a71da88 279 case 4:
Tom0108 1:199c4a71da88 280 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 3:1063aa133b62 281 MD_SET_PWM (MD_Data,4,60);
Tom0108 3:1063aa133b62 282 if(start_lim==1) kago_f=0;
Tom0108 1:199c4a71da88 283 break;
Tom0108 0:761a63c6d020 284 }
Tom0108 0:761a63c6d020 285 } else {
Tom0108 3:1063aa133b62 286 //かごと腕を停止
Tom0108 0:761a63c6d020 287 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 1:199c4a71da88 288 MD_SET_PWM (MD_Data,4,0);
Tom0108 1:199c4a71da88 289 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 1:199c4a71da88 290 MD_SET_PWM (MD_Data,6,0);
Tom0108 3:1063aa133b62 291
Tom0108 1:199c4a71da88 292 //爪開く
Tom0108 2:47954f05d32d 293 i2c.Out_Set(Out_Data,1,1);
Tom0108 1:199c4a71da88 294 //腕上げる
Tom0108 2:47954f05d32d 295 i2c.Out_Set(Out_Data,0,0);
Tom0108 2:47954f05d32d 296
Tom0108 1:199c4a71da88 297 kago=0;
Tom0108 1:199c4a71da88 298 pull=0;
Tom0108 1:199c4a71da88 299 grab=0;
Tom0108 3:1063aa133b62 300
Tom0108 2:47954f05d32d 301 tim.reset();
Tom0108 0:761a63c6d020 302 }
Tom0108 0:761a63c6d020 303
Tom0108 3:1063aa133b62 304 //-------------- 出力 --------------//
Tom0108 3:1063aa133b62 305 //第二引数には使う最大の個数
Tom0108 3:1063aa133b62 306 i2c.Out(Out_Data,5);
Tom0108 3:1063aa133b62 307
Tom0108 3:1063aa133b62 308 //モータ出力
Tom0108 3:1063aa133b62 309 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
Tom0108 3:1063aa133b62 310
Tom0108 3:1063aa133b62 311 /* ----------- りみっと --------------- */
Tom0108 3:1063aa133b62 312 limit_up = (In_Data+4)->in_data;
Tom0108 3:1063aa133b62 313 limit_down = (In_Data+5)->in_data;
Tom0108 3:1063aa133b62 314 max_lim = (In_Data+1)->in_data;
Tom0108 3:1063aa133b62 315 start_lim =(In_Data+2)->in_data;
Tom0108 3:1063aa133b62 316 short_lim = (In_Data+6)->in_data;
Tom0108 3:1063aa133b62 317 slide_start = (In_Data+3)->in_data;
Tom0108 3:1063aa133b62 318 slide_stop = (In_Data+0)->in_data;
Tom0108 3:1063aa133b62 319
Tom0108 3:1063aa133b62 320 //半自動のデバック
Tom0108 3:1063aa133b62 321 // pc.printf("pull %2d%2d ",pull,pull_f);
Tom0108 3:1063aa133b62 322 // pc.printf("grab %2d%2d ",grab,grab_f);
Tom0108 3:1063aa133b62 323 // pc.printf("kago %2d%2d ",kago,kago_f);
Tom0108 6:040d001acb12 324 // pc.printf("yaw:%.1f dif:%.1f",yaw,dif_val);
Tom0108 6:040d001acb12 325 pc.printf("0:%.1f 1:%.1f yaw:%.1f dif:%.1f",y_deg[0],y_deg[1],yaw,dif_val);
Tom0108 4:236e5c58e8ee 326 // pc.printf("%2d ",dir);
Tom0108 3:1063aa133b62 327 pc.printf("\n\r");
Tom0108 3:1063aa133b62 328
Tom0108 3:1063aa133b62 329
Tom0108 3:1063aa133b62 330 /* --------------電磁弁系-------------------- */
Tom0108 0:761a63c6d020 331 /*タオル掛け*/
Tom0108 0:761a63c6d020 332 if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
Tom0108 0:761a63c6d020 333 else i2c.Out_Set(Out_Data,2,0);
Tom0108 3:1063aa133b62 334
Tom0108 0:761a63c6d020 335 /*タオルを引っ張るやつ*/
Tom0108 1:199c4a71da88 336 if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
Tom0108 1:199c4a71da88 337 else i2c.Out_Set(Out_Data,4,0);
Tom0108 0:761a63c6d020 338
Tom0108 3:1063aa133b62 339 if(unfold_f)i2c.Out_Set(Out_Data,3,1);
Tom0108 3:1063aa133b62 340 else i2c.Out_Set(Out_Data,3,0);
Tom0108 0:761a63c6d020 341
Tom0108 0:761a63c6d020 342
Tom0108 3:1063aa133b62 343 /* ----------------ボタン系--------------- */
Tom0108 6:040d001acb12 344 //角度リセット
Tom0108 2:47954f05d32d 345 if(sbdbt.L1) {
Tom0108 6:040d001acb12 346 yaw=0;
Tom0108 6:040d001acb12 347 Turn_val=0;
Tom0108 2:47954f05d32d 348 }
Tom0108 3:1063aa133b62 349 //かごを倒す
Tom0108 6:040d001acb12 350 if(CROSS) {
Tom0108 1:199c4a71da88 351 if(once==0) {
Tom0108 3:1063aa133b62 352 kago_f++;
Tom0108 3:1063aa133b62 353 kago_f%=2;
Tom0108 1:199c4a71da88 354 once++;
Tom0108 1:199c4a71da88 355 }
Tom0108 3:1063aa133b62 356 }
Tom0108 3:1063aa133b62 357 //タオルを掴んで動かす
Tom0108 3:1063aa133b62 358 else if(SQUARE) {
Tom0108 1:199c4a71da88 359 if(once==0) {
Tom0108 3:1063aa133b62 360 grab_f++;
Tom0108 3:1063aa133b62 361 grab_f%=2;
Tom0108 1:199c4a71da88 362 once++;
Tom0108 1:199c4a71da88 363 }
Tom0108 4:236e5c58e8ee 364 } else if(TRIANGLE) {
Tom0108 4:236e5c58e8ee 365 if(once==0) {
Tom0108 4:236e5c58e8ee 366 sgrab_f++;
Tom0108 4:236e5c58e8ee 367 sgrab_f%=2;
Tom0108 4:236e5c58e8ee 368 once++;
Tom0108 4:236e5c58e8ee 369 }
Tom0108 3:1063aa133b62 370 }
Tom0108 3:1063aa133b62 371 //倒すを引いて動かす
Tom0108 3:1063aa133b62 372 else if(CIRCLE) {
Tom0108 1:199c4a71da88 373 if(once==0) {
Tom0108 3:1063aa133b62 374 pull_f++;
Tom0108 3:1063aa133b62 375 pull_f%=2;
Tom0108 3:1063aa133b62 376 once++;
Tom0108 3:1063aa133b62 377 }
Tom0108 3:1063aa133b62 378 }
Tom0108 3:1063aa133b62 379 //タオル展開用
Tom0108 3:1063aa133b62 380 else if(sbdbt.START) {
Tom0108 3:1063aa133b62 381 if(once==0) {
Tom0108 3:1063aa133b62 382 unfold_f++;
Tom0108 3:1063aa133b62 383 unfold_f%=2;
Tom0108 1:199c4a71da88 384 once++;
Tom0108 1:199c4a71da88 385 }
Tom0108 1:199c4a71da88 386 } else once=0;
Tom0108 1:199c4a71da88 387
Tom0108 1:199c4a71da88 388
Tom0108 0:761a63c6d020 389 } // if(sbdbt.State_check())
Tom0108 0:761a63c6d020 390 } // while(1)
Tom0108 0:761a63c6d020 391 } // int main()
Tom0108 0:761a63c6d020 392
Tom0108 0:761a63c6d020 393 /* メカナムの基本移動 */
Tom0108 6:040d001acb12 394 void mecanum_Move(int lx, int ly, int rx)
Tom0108 0:761a63c6d020 395 {
Tom0108 6:040d001acb12 396 duty[0]=lx+ly;
Tom0108 6:040d001acb12 397 duty[1]=-(-lx+ly);
Tom0108 6:040d001acb12 398 duty[2]=-lx+ly;
Tom0108 6:040d001acb12 399 duty[3]=-(lx+ly);
Tom0108 0:761a63c6d020 400
Tom0108 0:761a63c6d020 401 for(int i=0; i<4; i++) {
Tom0108 6:040d001acb12 402 duty[i]+=rx;
Tom0108 6:040d001acb12 403 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 404 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 405 }
Tom0108 0:761a63c6d020 406 }
Tom0108 0:761a63c6d020 407
Tom0108 6:040d001acb12 408 void AngleCorrection(double n_angle, double t_angle)
Tom0108 0:761a63c6d020 409 {
Tom0108 6:040d001acb12 410 double dif=-(t_angle-n_angle);
Tom0108 6:040d001acb12 411
Tom0108 6:040d001acb12 412 if(dif>=30) dif=30;
Tom0108 6:040d001acb12 413 else if(dif<=-30) dif=-30;
Tom0108 6:040d001acb12 414
Tom0108 0:761a63c6d020 415 for(int i=0; i<4; i++) {
Tom0108 6:040d001acb12 416 duty[i]+=dif;
Tom0108 6:040d001acb12 417 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 418 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 419 }
Tom0108 6:040d001acb12 420
Tom0108 0:761a63c6d020 421 }
Tom0108 0:761a63c6d020 422
Tom0108 0:761a63c6d020 423 /*操作法*/
Tom0108 0:761a63c6d020 424 /*
Tom0108 0:761a63c6d020 425 〇 ボタン == 回収（引きずる）
Tom0108 0:761a63c6d020 426 ? ボタン == 籠
Tom0108 0:761a63c6d020 427 △ ボタン == 回収（シーツ兼シャツ）
Tom0108 0:761a63c6d020 428 □ ボタン == 回収（タオル）
Tom0108 0:761a63c6d020 429 上 ボタン == ハンガーかけ機構 上昇
Tom0108 0:761a63c6d020 430 下 ボタン == ハンガーかけ機構 下降
Tom0108 0:761a63c6d020 431 L1 ボタン == リセット（足回り）
Tom0108 0:761a63c6d020 432 L2 ボタン == タオル掛け
Tom0108 6:040d001acb12 433 R2 ボタン == タオル真ん中掴む
Tom0108 0:761a63c6d020 434 select == ハンガー掛け 最大まで上昇
Tom0108 0:761a63c6d020 435 start == ハンガー展開
Tom0108 0:761a63c6d020 436 */
Tom0108 0:761a63c6d020 437 /*その他注意点*/
Tom0108 0:761a63c6d020 438 /*
Tom0108 6:040d001acb12 439 動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ
Tom0108 0:761a63c6d020 440 メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
Tom0108 0:761a63c6d020 441 あまり急発進をさせない（たまに暴走）
Tom0108 0:761a63c6d020 442 暴走したときはコントローラーを動かしたら治るはず
Tom0108 0:761a63c6d020 443 コントローラー接続が切れたらSTMの黒い奴を押す（わからないときは近くの回路班へ）
Tom0108 0:761a63c6d020 444 */