Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@12:7e33e400a48d, 2019-09-08 (annotated)

Committer:
Tom0108
Date:
Sun Sep 08 09:57:22 2019 +0000
Revision:
12:7e33e400a48d
Parent:
11:c1a22e2e3534
Child:
13:48c4a4c95e77
09/08

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 8:b79d21c8178b 4 //ゾーン対応
Tom0108 12:7e33e400a48d 5 //#define _RED_ZONE_
Tom0108 8:b79d21c8178b 6 #ifdef _RED_ZONE_
Tom0108 8:b79d21c8178b 7 const int def_val=0;
Tom0108 8:b79d21c8178b 8 #else
Tom0108 8:b79d21c8178b 9 const int def_val=180;
Tom0108 8:b79d21c8178b 10 #endif
Tom0108 0:761a63c6d020 11
Tom0108 0:761a63c6d020 12 /* 関数宣言 ------------------------------------------------------------------*/
Tom0108 0:761a63c6d020 13
Tom0108 0:761a63c6d020 14 /* 変数宣言 ------------------------------------------------------------------*/
Tom0108 1:199c4a71da88 15
Tom0108 1:199c4a71da88 16 //コントローラーの状態を保存するため
Tom0108 1:199c4a71da88 17 int once=0;
Tom0108 1:199c4a71da88 18
Tom0108 12:7e33e400a48d 19 //kogo: かごをひっくり返す
Tom0108 10:7c73e1577742 20 //pull: 奥のリミットで引く
Tom0108 12:7e33e400a48d 21 //spull: 中央のリミットでつかむ(short_pull)
Tom0108 10:7c73e1577742 22 //grab: 掴む
Tom0108 8:b79d21c8178b 23 //back_f: 真ん中目で下がるとき
Tom0108 12:7e33e400a48d 24 //release_f: 全体のエアー開放用
Tom0108 12:7e33e400a48d 25 //unfold_f: タオル横展開用
Tom0108 12:7e33e400a48d 26 int kago=0, kago_f=0, pull=0, pull_f=0, grab=0, grab_f=0, spull=0, spull_f=0;
Tom0108 12:7e33e400a48d 27 int back_f=0, release_f=0, unfold_f=0, short_f=0;
Tom0108 3:1063aa133b62 28
Tom0108 12:7e33e400a48d 29 //上下動作を保持
Tom0108 12:7e33e400a48d 30 //[0]: 上
Tom0108 12:7e33e400a48d 31 //[1]: 下
Tom0108 12:7e33e400a48d 32 int keep_f[2]= {0};
Tom0108 3:1063aa133b62 33
Tom0108 6:040d001acb12 34 //yaw: 実際に使う角度
Tom0108 10:7c73e1577742 35 double yaw=def_val;
Tom0108 1:199c4a71da88 36
Tom0108 12:7e33e400a48d 37 //[0]: 今回の値
Tom0108 12:7e33e400a48d 38 //[1]: 前回の値
Tom0108 10:7c73e1577742 39 double rawyaw[2];
Tom0108 3:1063aa133b62 40
Tom0108 3:1063aa133b62 41 //Turn_val: 補正の目標値
Tom0108 11:c1a22e2e3534 42 double Turn_val=def_val;
Tom0108 3:1063aa133b62 43
Tom0108 3:1063aa133b62 44 //short_lim: 腕中央
Tom0108 3:1063aa133b62 45 //max_lim: 腕奥側
Tom0108 3:1063aa133b62 46 //short_lim: 腕手前側
Tom0108 3:1063aa133b62 47 int short_lim, max_lim, start_lim;
Tom0108 12:7e33e400a48d 48 int limit_up,limit_down; // リミットスイッチ
Tom0108 12:7e33e400a48d 49 int slide_start,slide_stop; //かごのリミット
Tom0108 1:199c4a71da88 50
Tom0108 6:040d001acb12 51 //足回りのpwm値
Tom0108 6:040d001acb12 52 int duty[4];
Tom0108 4:236e5c58e8ee 53
Tom0108 6:040d001acb12 54 // 足回り
Tom0108 6:040d001acb12 55 int lx,ly,rx;
Tom0108 0:761a63c6d020 56
Tom0108 0:761a63c6d020 57 /*----------------------------------- main -----------------------------------*/
Tom0108 0:761a63c6d020 58 int main()
Tom0108 0:761a63c6d020 59 {
Tom0108 11:c1a22e2e3534 60 //ジャイロリセット
Tom0108 10:7c73e1577742 61 bno.reset();
Tom0108 5:fcc79e507610 62
Tom0108 0:761a63c6d020 63 while(1) {
Tom0108 1:199c4a71da88 64 //自動系のタイマースタート
Tom0108 1:199c4a71da88 65 tim.start();
Tom0108 6:040d001acb12 66 drift_tim.start();
Tom0108 12:7e33e400a48d 67 keep_tim[0].start();
Tom0108 12:7e33e400a48d 68 keep_tim[1].start();
Tom0108 0:761a63c6d020 69
Tom0108 11:c1a22e2e3534 70 bno.setmode(OPERATION_MODE_IMUPLUS);
Tom0108 10:7c73e1577742 71 //角度の取得
Tom0108 10:7c73e1577742 72 bno.get_angles();
Tom0108 10:7c73e1577742 73 rawyaw[1]=rawyaw[0];
Tom0108 10:7c73e1577742 74 rawyaw[0]=bno.euler.yaw;
Tom0108 11:c1a22e2e3534 75
Tom0108 5:fcc79e507610 76 //180～-180をまたいだ時
Tom0108 10:7c73e1577742 77 if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360;
Tom0108 10:7c73e1577742 78 else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360;
Tom0108 11:c1a22e2e3534 79
Tom0108 10:7c73e1577742 80 yaw-=rawyaw[0]-rawyaw[1];
Tom0108 0:761a63c6d020 81
Tom0108 12:7e33e400a48d 82 //ゾーン対応
Tom0108 12:7e33e400a48d 83 #ifdef _RED_ZONE_
Tom0108 12:7e33e400a48d 84 PALETTE(RED);
Tom0108 12:7e33e400a48d 85 #else
Tom0108 12:7e33e400a48d 86 PALETTE(BLUE);
Tom0108 12:7e33e400a48d 87 #endif
Tom0108 12:7e33e400a48d 88
Tom0108 5:fcc79e507610 89 if(sbdbt.State_check()) {
Tom0108 12:7e33e400a48d 90 //リミット
Tom0108 5:fcc79e507610 91 for(int i=0; i<7; i++) i2c.In(In_Data,i);
Tom0108 6:040d001acb12 92
Tom0108 0:761a63c6d020 93 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 94 ly = ((64-sbdbt.LY)*100.0/64)*cos(yaw*M_PI/180)-((sbdbt.LX-64)*100.0/64)*sin(yaw*M_PI/180);
Tom0108 8:b79d21c8178b 95 rx = (sbdbt.RX - 64)*60/64;
Tom0108 8:b79d21c8178b 96
Tom0108 8:b79d21c8178b 97 //R1で停止
Tom0108 8:b79d21c8178b 98 if(sbdbt.R1==0) {
Tom0108 8:b79d21c8178b 99 //メカナム基本動作
Tom0108 8:b79d21c8178b 100 mecanum_Move(lx, ly, rx);
Tom0108 0:761a63c6d020 101
Tom0108 8:b79d21c8178b 102 //旋回している間タイマーをリセット
Tom0108 8:b79d21c8178b 103 if(sbdbt.RX != 64) drift_tim.reset();
Tom0108 8:b79d21c8178b 104
Tom0108 8:b79d21c8178b 105 //旋回して慣性で動いた後の角度に補正する
Tom0108 11:c1a22e2e3534 106 if(drift_tim.read_ms()<1000) Turn_val=yaw;
Tom0108 0:761a63c6d020 107
Tom0108 8:b79d21c8178b 108 //角度補正
Tom0108 8:b79d21c8178b 109 AngleCorrection(yaw, Turn_val);
Tom0108 8:b79d21c8178b 110
Tom0108 8:b79d21c8178b 111 /*---ハンガーかけるやつ----------------------------------------------*/
Tom0108 12:7e33e400a48d 112 //上方向
Tom0108 12:7e33e400a48d 113 if(!DOWNkey && ((UPkey || keep_f[0]) && limit_up == 0)) {
Tom0108 12:7e33e400a48d 114 //一定秒以上押されたら保持
Tom0108 12:7e33e400a48d 115 if(keep_tim[0].read_ms()>500) keep_f[0]=1;
Tom0108 12:7e33e400a48d 116 //下方向の保持を解除
Tom0108 12:7e33e400a48d 117 keep_f[1]=0;
Tom0108 12:7e33e400a48d 118 keep_tim[1].reset();
Tom0108 12:7e33e400a48d 119
Tom0108 12:7e33e400a48d 120 MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
Tom0108 12:7e33e400a48d 121 MD_SET_PWM(MD_Data, 5,100);
Tom0108 12:7e33e400a48d 122 }
Tom0108 12:7e33e400a48d 123 //下方向
Tom0108 12:7e33e400a48d 124 else if(!UPkey && ((DOWNkey || keep_f[1]) && limit_down == 0)) {
Tom0108 12:7e33e400a48d 125 if(keep_tim[1].read_ms()>500) keep_f[1]=1;
Tom0108 12:7e33e400a48d 126 keep_f[0]=0;
Tom0108 12:7e33e400a48d 127 keep_tim[0].reset();
Tom0108 12:7e33e400a48d 128
Tom0108 8:b79d21c8178b 129 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 8:b79d21c8178b 130 MD_SET_PWM(MD_Data, 5,100);
Tom0108 8:b79d21c8178b 131 } else {
Tom0108 12:7e33e400a48d 132 keep_f[0]=0;
Tom0108 12:7e33e400a48d 133 keep_tim[0].reset();
Tom0108 12:7e33e400a48d 134 keep_f[1]=0;
Tom0108 12:7e33e400a48d 135 keep_tim[1].reset();
Tom0108 12:7e33e400a48d 136
Tom0108 8:b79d21c8178b 137 MD_SET_PWM(MD_Data,5,0);
Tom0108 8:b79d21c8178b 138 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 8:b79d21c8178b 139 }
Tom0108 6:040d001acb12 140
Tom0108 8:b79d21c8178b 141 //タオルつかむ 四角
Tom0108 10:7c73e1577742 142 if(grab_f) {
Tom0108 10:7c73e1577742 143 //真ん中で止まる用
Tom0108 8:b79d21c8178b 144 short_f=1;
Tom0108 8:b79d21c8178b 145 switch(grab) {
Tom0108 8:b79d21c8178b 146 //腕を前に動かす
Tom0108 8:b79d21c8178b 147 case 0:
Tom0108 8:b79d21c8178b 148 //爪開く
Tom0108 8:b79d21c8178b 149 i2c.Out_Set(Out_Data,1,1);
Tom0108 0:761a63c6d020 150
Tom0108 10:7c73e1577742 151 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 152 MD_SET_PWM (MD_Data,4,60);
Tom0108 0:761a63c6d020 153
Tom0108 10:7c73e1577742 154 if(max_lim==1) {
Tom0108 10:7c73e1577742 155 grab++;
Tom0108 10:7c73e1577742 156 tim.reset();
Tom0108 8:b79d21c8178b 157 }
Tom0108 8:b79d21c8178b 158 break;
Tom0108 8:b79d21c8178b 159
Tom0108 8:b79d21c8178b 160 case 1:
Tom0108 8:b79d21c8178b 161 //腕停止
Tom0108 8:b79d21c8178b 162 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 163 MD_SET_PWM(MD_Data,4,0);
Tom0108 8:b79d21c8178b 164
Tom0108 8:b79d21c8178b 165 //腕おろす
Tom0108 8:b79d21c8178b 166 i2c.Out_Set(Out_Data,0,1);
Tom0108 9:64fcbcc182d9 167 if(tim.read_ms()>200) {
Tom0108 8:b79d21c8178b 168 grab++;
Tom0108 8:b79d21c8178b 169 tim.reset();
Tom0108 8:b79d21c8178b 170 }
Tom0108 8:b79d21c8178b 171 break;
Tom0108 0:761a63c6d020 172
Tom0108 8:b79d21c8178b 173 case 2:
Tom0108 8:b79d21c8178b 174 //爪閉じる
Tom0108 8:b79d21c8178b 175 i2c.Out_Set(Out_Data,1,0);
Tom0108 9:64fcbcc182d9 176 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 177 grab++;
Tom0108 8:b79d21c8178b 178 tim.reset();
Tom0108 8:b79d21c8178b 179 }
Tom0108 8:b79d21c8178b 180 break;
Tom0108 8:b79d21c8178b 181
Tom0108 8:b79d21c8178b 182 case 3:
Tom0108 8:b79d21c8178b 183 //腕上げる
Tom0108 8:b79d21c8178b 184 i2c.Out_Set(Out_Data,0,0);
Tom0108 9:64fcbcc182d9 185 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 186 grab++;
Tom0108 8:b79d21c8178b 187 tim.reset();
Tom0108 8:b79d21c8178b 188 }
Tom0108 8:b79d21c8178b 189 break;
Tom0108 8:b79d21c8178b 190
Tom0108 8:b79d21c8178b 191 //腕戻す
Tom0108 8:b79d21c8178b 192 case 4:
Tom0108 8:b79d21c8178b 193 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 9:64fcbcc182d9 194 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 195 if(start_lim==1) {
Tom0108 8:b79d21c8178b 196 grab_f=0;
Tom0108 8:b79d21c8178b 197 tim.reset();
Tom0108 8:b79d21c8178b 198 }
Tom0108 8:b79d21c8178b 199 break;
Tom0108 8:b79d21c8178b 200 }
Tom0108 8:b79d21c8178b 201 } //if(grab_f)
Tom0108 0:761a63c6d020 202
Tom0108 10:7c73e1577742 203 //シーツ用
Tom0108 10:7c73e1577742 204 else if(spull_f) {
Tom0108 10:7c73e1577742 205 short_f=1;
Tom0108 10:7c73e1577742 206 switch(spull) {
Tom0108 10:7c73e1577742 207 //腕を前に動かす
Tom0108 10:7c73e1577742 208 case 0:
Tom0108 10:7c73e1577742 209 //真ん中のリミットより前に腕があった場合にバックする
Tom0108 10:7c73e1577742 210 if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 10:7c73e1577742 211 else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 11:c1a22e2e3534 212 MD_SET_PWM (MD_Data,4,30);
Tom0108 10:7c73e1577742 213
Tom0108 10:7c73e1577742 214 if(short_lim==1) {
Tom0108 10:7c73e1577742 215 spull++;
Tom0108 10:7c73e1577742 216 tim.reset();
Tom0108 10:7c73e1577742 217 } else if(max_lim) back_f=1;
Tom0108 10:7c73e1577742 218 break;
Tom0108 11:c1a22e2e3534 219
Tom0108 11:c1a22e2e3534 220 case 1:
Tom0108 10:7c73e1577742 221 //腕停止
Tom0108 10:7c73e1577742 222 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 10:7c73e1577742 223 MD_SET_PWM(MD_Data,4,0);
Tom0108 10:7c73e1577742 224
Tom0108 10:7c73e1577742 225 //腕おろす
Tom0108 10:7c73e1577742 226 i2c.Out_Set(Out_Data,0,1);
Tom0108 10:7c73e1577742 227 if(tim.read_ms()>200) {
Tom0108 10:7c73e1577742 228 spull++;
Tom0108 10:7c73e1577742 229 tim.reset();
Tom0108 10:7c73e1577742 230 }
Tom0108 10:7c73e1577742 231 break;
Tom0108 10:7c73e1577742 232
Tom0108 10:7c73e1577742 233 case 2:
Tom0108 10:7c73e1577742 234 //爪閉じる
Tom0108 10:7c73e1577742 235 i2c.Out_Set(Out_Data,1,0);
Tom0108 10:7c73e1577742 236 if(tim.read_ms()>0) {
Tom0108 10:7c73e1577742 237 spull++;
Tom0108 10:7c73e1577742 238 tim.reset();
Tom0108 10:7c73e1577742 239 }
Tom0108 10:7c73e1577742 240 break;
Tom0108 10:7c73e1577742 241
Tom0108 10:7c73e1577742 242 case 3:
Tom0108 10:7c73e1577742 243 //腕上げる
Tom0108 10:7c73e1577742 244 i2c.Out_Set(Out_Data,0,0);
Tom0108 10:7c73e1577742 245 if(tim.read_ms()>0) {
Tom0108 10:7c73e1577742 246 spull++;
Tom0108 10:7c73e1577742 247 tim.reset();
Tom0108 10:7c73e1577742 248 }
Tom0108 10:7c73e1577742 249 break;
Tom0108 10:7c73e1577742 250
Tom0108 10:7c73e1577742 251 //腕戻す
Tom0108 10:7c73e1577742 252 case 4:
Tom0108 10:7c73e1577742 253 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 10:7c73e1577742 254 MD_SET_PWM (MD_Data,4,60);
Tom0108 10:7c73e1577742 255 if(start_lim==1) {
Tom0108 10:7c73e1577742 256 spull_f=0;
Tom0108 10:7c73e1577742 257 tim.reset();
Tom0108 10:7c73e1577742 258 }
Tom0108 10:7c73e1577742 259 break;
Tom0108 10:7c73e1577742 260
Tom0108 10:7c73e1577742 261 }
Tom0108 10:7c73e1577742 262 } //if(spull_f)
Tom0108 10:7c73e1577742 263
Tom0108 8:b79d21c8178b 264 //タオル引く 丸
Tom0108 8:b79d21c8178b 265 else if(pull_f) {
Tom0108 8:b79d21c8178b 266 short_f=1;
Tom0108 8:b79d21c8178b 267 switch(pull) {
Tom0108 8:b79d21c8178b 268 //腕を前に動かす
Tom0108 8:b79d21c8178b 269 case 0:
Tom0108 8:b79d21c8178b 270 //爪閉じる
Tom0108 8:b79d21c8178b 271 i2c.Out_Set(Out_Data,1,0);
Tom0108 8:b79d21c8178b 272
Tom0108 8:b79d21c8178b 273 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 274 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 275
Tom0108 8:b79d21c8178b 276 if(max_lim==1) {
Tom0108 8:b79d21c8178b 277 pull++;
Tom0108 8:b79d21c8178b 278 tim.reset();
Tom0108 8:b79d21c8178b 279 }
Tom0108 8:b79d21c8178b 280 break;
Tom0108 8:b79d21c8178b 281
Tom0108 8:b79d21c8178b 282 case 1:
Tom0108 8:b79d21c8178b 283 //腕停止
Tom0108 8:b79d21c8178b 284 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 285 MD_SET_PWM (MD_Data,4,0);
Tom0108 2:47954f05d32d 286
Tom0108 8:b79d21c8178b 287 //腕おろす
Tom0108 8:b79d21c8178b 288 i2c.Out_Set(Out_Data,0,1);
Tom0108 9:64fcbcc182d9 289 if(tim.read_ms()>100) {
Tom0108 8:b79d21c8178b 290 pull++;
Tom0108 8:b79d21c8178b 291 tim.reset();
Tom0108 8:b79d21c8178b 292 }
Tom0108 8:b79d21c8178b 293 break;
Tom0108 8:b79d21c8178b 294
Tom0108 9:64fcbcc182d9 295 //停止
Tom0108 8:b79d21c8178b 296 case 2:
Tom0108 9:64fcbcc182d9 297 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 298 pull++;
Tom0108 8:b79d21c8178b 299 tim.reset();
Tom0108 8:b79d21c8178b 300 }
Tom0108 8:b79d21c8178b 301 break;
Tom0108 0:761a63c6d020 302
Tom0108 8:b79d21c8178b 303 case 3:
Tom0108 8:b79d21c8178b 304 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 9:64fcbcc182d9 305 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 306 if(start_lim==1) {
Tom0108 8:b79d21c8178b 307 pull_f=0;
Tom0108 8:b79d21c8178b 308 tim.reset();
Tom0108 8:b79d21c8178b 309 }
Tom0108 8:b79d21c8178b 310 break;
Tom0108 8:b79d21c8178b 311 }
Tom0108 8:b79d21c8178b 312 } //if(pull_f)
Tom0108 8:b79d21c8178b 313
Tom0108 8:b79d21c8178b 314 //かごを倒す
Tom0108 8:b79d21c8178b 315 else if(kago_f) {
Tom0108 11:c1a22e2e3534 316 // PALETTE(MAGENTA);
Tom0108 8:b79d21c8178b 317 short_f=1;
Tom0108 8:b79d21c8178b 318 //腕の先を閉じておく
Tom0108 8:b79d21c8178b 319 i2c.Out_Set(Out_Data,1,0);
Tom0108 1:199c4a71da88 320
Tom0108 8:b79d21c8178b 321 switch(kago) {
Tom0108 8:b79d21c8178b 322 //腕を前に動かす
Tom0108 8:b79d21c8178b 323 case 0:
Tom0108 8:b79d21c8178b 324 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 325 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 326 if(max_lim==1) {
Tom0108 8:b79d21c8178b 327 kago++;
Tom0108 8:b79d21c8178b 328 tim.reset();
Tom0108 8:b79d21c8178b 329 }
Tom0108 8:b79d21c8178b 330 break;
Tom0108 8:b79d21c8178b 331
Tom0108 8:b79d21c8178b 332 case 1:
Tom0108 8:b79d21c8178b 333 //腕停止
Tom0108 8:b79d21c8178b 334 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 335 MD_SET_PWM (MD_Data,4,0);
Tom0108 8:b79d21c8178b 336
Tom0108 8:b79d21c8178b 337 //かご回転
Tom0108 8:b79d21c8178b 338 MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
Tom0108 9:64fcbcc182d9 339 MD_SET_PWM (MD_Data,6,100);
Tom0108 8:b79d21c8178b 340 if(slide_stop==1) {
Tom0108 8:b79d21c8178b 341 kago++;
Tom0108 8:b79d21c8178b 342 tim.reset();
Tom0108 8:b79d21c8178b 343 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 8:b79d21c8178b 344 MD_SET_PWM (MD_Data,6,0);
Tom0108 8:b79d21c8178b 345 }
Tom0108 8:b79d21c8178b 346 break;
Tom0108 3:1063aa133b62 347
Tom0108 8:b79d21c8178b 348 case 2:
Tom0108 8:b79d21c8178b 349 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 350 kago++;
Tom0108 8:b79d21c8178b 351 tim.reset();
Tom0108 8:b79d21c8178b 352 }
Tom0108 8:b79d21c8178b 353 break;
Tom0108 8:b79d21c8178b 354
Tom0108 8:b79d21c8178b 355 case 3:
Tom0108 8:b79d21c8178b 356 MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
Tom0108 9:64fcbcc182d9 357 MD_SET_PWM (MD_Data,6,100);
Tom0108 8:b79d21c8178b 358 if(slide_start==1) kago++;
Tom0108 8:b79d21c8178b 359 break;
Tom0108 3:1063aa133b62 360
Tom0108 8:b79d21c8178b 361 case 4:
Tom0108 8:b79d21c8178b 362 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 9:64fcbcc182d9 363 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 364 if(start_lim==1) {
Tom0108 8:b79d21c8178b 365 kago_f=0;
Tom0108 8:b79d21c8178b 366 tim.reset();
Tom0108 8:b79d21c8178b 367 }
Tom0108 8:b79d21c8178b 368 break;
Tom0108 8:b79d21c8178b 369 }
Tom0108 8:b79d21c8178b 370 } else {
Tom0108 8:b79d21c8178b 371 //爪開く
Tom0108 8:b79d21c8178b 372 i2c.Out_Set(Out_Data,1,1);
Tom0108 8:b79d21c8178b 373 //腕上げる
Tom0108 8:b79d21c8178b 374 i2c.Out_Set(Out_Data,0,0);
Tom0108 8:b79d21c8178b 375
Tom0108 8:b79d21c8178b 376 //腕真ん中で停止
Tom0108 8:b79d21c8178b 377 if(short_f) {
Tom0108 9:64fcbcc182d9 378 if(short_lim==1) {
Tom0108 9:64fcbcc182d9 379 short_f=0;
Tom0108 9:64fcbcc182d9 380 back_f=0;
Tom0108 9:64fcbcc182d9 381 }
Tom0108 9:64fcbcc182d9 382 //奥のリミットに当たったら逆回転
Tom0108 9:64fcbcc182d9 383 if(max_lim==1) back_f=1;
Tom0108 9:64fcbcc182d9 384 //手前のリミットに当たったら正回転
Tom0108 9:64fcbcc182d9 385 else if(start_lim==1) back_f=0;
Tom0108 10:7c73e1577742 386
Tom0108 9:64fcbcc182d9 387 if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 9:64fcbcc182d9 388 else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 389 MD_SET_PWM (MD_Data,4,30);
Tom0108 8:b79d21c8178b 390 } else {
Tom0108 2:47954f05d32d 391 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 2:47954f05d32d 392 MD_SET_PWM (MD_Data,4,0);
Tom0108 8:b79d21c8178b 393 }
Tom0108 8:b79d21c8178b 394 //かご停止
Tom0108 8:b79d21c8178b 395 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 8:b79d21c8178b 396 MD_SET_PWM (MD_Data,6,0);
Tom0108 1:199c4a71da88 397
Tom0108 8:b79d21c8178b 398 kago=0;
Tom0108 8:b79d21c8178b 399 pull=0;
Tom0108 8:b79d21c8178b 400 grab=0;
Tom0108 10:7c73e1577742 401 spull=0;
Tom0108 8:b79d21c8178b 402 tim.reset();
Tom0108 0:761a63c6d020 403 }
Tom0108 8:b79d21c8178b 404 }
Tom0108 8:b79d21c8178b 405 //非常停止
Tom0108 8:b79d21c8178b 406 else {
Tom0108 8:b79d21c8178b 407 for(int i=0; i<8; i++) {
Tom0108 8:b79d21c8178b 408 MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
Tom0108 8:b79d21c8178b 409 MD_SET_PWM (MD_Data,i,0);
Tom0108 8:b79d21c8178b 410 }
Tom0108 0:761a63c6d020 411 }
Tom0108 0:761a63c6d020 412
Tom0108 12:7e33e400a48d 413 /* --------------電磁弁系-------------------- */
Tom0108 12:7e33e400a48d 414 /*タオル掛け*/
Tom0108 12:7e33e400a48d 415 if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
Tom0108 12:7e33e400a48d 416 else i2c.Out_Set(Out_Data,2,0);
Tom0108 12:7e33e400a48d 417
Tom0108 12:7e33e400a48d 418 /*タオルを引っ張るやつ*/
Tom0108 12:7e33e400a48d 419 if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
Tom0108 12:7e33e400a48d 420 else i2c.Out_Set(Out_Data,4,0);
Tom0108 12:7e33e400a48d 421
Tom0108 12:7e33e400a48d 422 //タオル展開
Tom0108 12:7e33e400a48d 423 if(unfold_f) i2c.Out_Set(Out_Data,3,1);
Tom0108 12:7e33e400a48d 424 else i2c.Out_Set(Out_Data,3,0);
Tom0108 12:7e33e400a48d 425
Tom0108 12:7e33e400a48d 426 //エアー解放
Tom0108 12:7e33e400a48d 427 if(release_f) {
Tom0108 12:7e33e400a48d 428 PALETTE(YELLOW);
Tom0108 12:7e33e400a48d 429 i2c.Out_Set(Out_Data,5,1);
Tom0108 12:7e33e400a48d 430 } else i2c.Out_Set(Out_Data,5,0);
Tom0108 12:7e33e400a48d 431
Tom0108 3:1063aa133b62 432 //-------------- 出力 --------------//
Tom0108 3:1063aa133b62 433 //第二引数には使う最大の個数
Tom0108 12:7e33e400a48d 434 i2c.Out(Out_Data,9);
Tom0108 3:1063aa133b62 435
Tom0108 3:1063aa133b62 436 //モータ出力
Tom0108 3:1063aa133b62 437 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
Tom0108 3:1063aa133b62 438
Tom0108 3:1063aa133b62 439 /* ----------- りみっと --------------- */
Tom0108 3:1063aa133b62 440 limit_up = (In_Data+4)->in_data;
Tom0108 3:1063aa133b62 441 limit_down = (In_Data+5)->in_data;
Tom0108 3:1063aa133b62 442 max_lim = (In_Data+1)->in_data;
Tom0108 3:1063aa133b62 443 start_lim =(In_Data+2)->in_data;
Tom0108 3:1063aa133b62 444 short_lim = (In_Data+6)->in_data;
Tom0108 3:1063aa133b62 445 slide_start = (In_Data+3)->in_data;
Tom0108 3:1063aa133b62 446 slide_stop = (In_Data+0)->in_data;
Tom0108 3:1063aa133b62 447
Tom0108 3:1063aa133b62 448 //半自動のデバック
Tom0108 3:1063aa133b62 449 // pc.printf("pull %2d%2d ",pull,pull_f);
Tom0108 3:1063aa133b62 450 // pc.printf("grab %2d%2d ",grab,grab_f);
Tom0108 3:1063aa133b62 451 // pc.printf("kago %2d%2d ",kago,kago_f);
Tom0108 11:c1a22e2e3534 452 // pc.printf("%.1f %.1f ",rawyaw[0],rawyaw[1]);
Tom0108 11:c1a22e2e3534 453 // pc.printf("yaw:%.1f %.1f",yaw, bno.euler.yaw);
Tom0108 12:7e33e400a48d 454 // pc.printf("lx %d ly %d rx %d",lx, ly, rx);
Tom0108 12:7e33e400a48d 455 pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]);
Tom0108 3:1063aa133b62 456 pc.printf("\n\r");
Tom0108 3:1063aa133b62 457
Tom0108 3:1063aa133b62 458 /* ----------------ボタン系--------------- */
Tom0108 6:040d001acb12 459 //角度リセット
Tom0108 2:47954f05d32d 460 if(sbdbt.L1) {
Tom0108 8:b79d21c8178b 461 yaw=def_val;
Tom0108 8:b79d21c8178b 462 Turn_val=def_val;
Tom0108 2:47954f05d32d 463 }
Tom0108 3:1063aa133b62 464 //かごを倒す
Tom0108 6:040d001acb12 465 if(CROSS) {
Tom0108 1:199c4a71da88 466 if(once==0) {
Tom0108 3:1063aa133b62 467 kago_f++;
Tom0108 3:1063aa133b62 468 kago_f%=2;
Tom0108 1:199c4a71da88 469 once++;
Tom0108 1:199c4a71da88 470 }
Tom0108 3:1063aa133b62 471 }
Tom0108 3:1063aa133b62 472 //タオルを掴んで動かす
Tom0108 3:1063aa133b62 473 else if(SQUARE) {
Tom0108 1:199c4a71da88 474 if(once==0) {
Tom0108 3:1063aa133b62 475 grab_f++;
Tom0108 3:1063aa133b62 476 grab_f%=2;
Tom0108 1:199c4a71da88 477 once++;
Tom0108 1:199c4a71da88 478 }
Tom0108 4:236e5c58e8ee 479 } else if(TRIANGLE) {
Tom0108 4:236e5c58e8ee 480 if(once==0) {
Tom0108 8:b79d21c8178b 481 spull_f++;
Tom0108 8:b79d21c8178b 482 spull_f%=2;
Tom0108 4:236e5c58e8ee 483 once++;
Tom0108 4:236e5c58e8ee 484 }
Tom0108 3:1063aa133b62 485 }
Tom0108 3:1063aa133b62 486 //倒すを引いて動かす
Tom0108 3:1063aa133b62 487 else if(CIRCLE) {
Tom0108 1:199c4a71da88 488 if(once==0) {
Tom0108 3:1063aa133b62 489 pull_f++;
Tom0108 3:1063aa133b62 490 pull_f%=2;
Tom0108 3:1063aa133b62 491 once++;
Tom0108 3:1063aa133b62 492 }
Tom0108 3:1063aa133b62 493 }
Tom0108 3:1063aa133b62 494 //タオル展開用
Tom0108 3:1063aa133b62 495 else if(sbdbt.START) {
Tom0108 3:1063aa133b62 496 if(once==0) {
Tom0108 3:1063aa133b62 497 unfold_f++;
Tom0108 3:1063aa133b62 498 unfold_f%=2;
Tom0108 1:199c4a71da88 499 once++;
Tom0108 1:199c4a71da88 500 }
Tom0108 11:c1a22e2e3534 501 }
Tom0108 11:c1a22e2e3534 502 //タオル展開用
Tom0108 11:c1a22e2e3534 503 else if(sbdbt.SELECT) {
Tom0108 11:c1a22e2e3534 504 if(once==0) {
Tom0108 11:c1a22e2e3534 505 release_f++;
Tom0108 11:c1a22e2e3534 506 release_f%=2;
Tom0108 11:c1a22e2e3534 507 once++;
Tom0108 11:c1a22e2e3534 508 }
Tom0108 1:199c4a71da88 509 } else once=0;
Tom0108 1:199c4a71da88 510
Tom0108 1:199c4a71da88 511
Tom0108 0:761a63c6d020 512 } // if(sbdbt.State_check())
Tom0108 12:7e33e400a48d 513 else PALETTE(BLACK);
Tom0108 0:761a63c6d020 514 } // while(1)
Tom0108 0:761a63c6d020 515 } // int main()
Tom0108 0:761a63c6d020 516
Tom0108 0:761a63c6d020 517 /* メカナムの基本移動 */
Tom0108 6:040d001acb12 518 void mecanum_Move(int lx, int ly, int rx)
Tom0108 0:761a63c6d020 519 {
Tom0108 6:040d001acb12 520 duty[0]=lx+ly;
Tom0108 6:040d001acb12 521 duty[1]=-(-lx+ly);
Tom0108 6:040d001acb12 522 duty[2]=-lx+ly;
Tom0108 6:040d001acb12 523 duty[3]=-(lx+ly);
Tom0108 0:761a63c6d020 524
Tom0108 0:761a63c6d020 525 for(int i=0; i<4; i++) {
Tom0108 8:b79d21c8178b 526 //旋回
Tom0108 6:040d001acb12 527 duty[i]+=rx;
Tom0108 8:b79d21c8178b 528
Tom0108 8:b79d21c8178b 529 //制限
Tom0108 8:b79d21c8178b 530 if(duty[i]>=99) duty[i]=99;
Tom0108 8:b79d21c8178b 531 else if(duty[i]<=-99) duty[i]=-99;
Tom0108 6:040d001acb12 532 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 533 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 534 }
Tom0108 0:761a63c6d020 535 }
Tom0108 0:761a63c6d020 536
Tom0108 6:040d001acb12 537 void AngleCorrection(double n_angle, double t_angle)
Tom0108 0:761a63c6d020 538 {
Tom0108 6:040d001acb12 539 double dif=-(t_angle-n_angle);
Tom0108 6:040d001acb12 540
Tom0108 6:040d001acb12 541 if(dif>=30) dif=30;
Tom0108 6:040d001acb12 542 else if(dif<=-30) dif=-30;
Tom0108 6:040d001acb12 543
Tom0108 0:761a63c6d020 544 for(int i=0; i<4; i++) {
Tom0108 6:040d001acb12 545 duty[i]+=dif;
Tom0108 6:040d001acb12 546 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 547 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 548 }
Tom0108 0:761a63c6d020 549 }
Tom0108 0:761a63c6d020 550
Tom0108 0:761a63c6d020 551 /*操作法*/
Tom0108 0:761a63c6d020 552 /*
Tom0108 0:761a63c6d020 553 〇 ボタン == 回収（引きずる）
Tom0108 0:761a63c6d020 554 ? ボタン == 籠
Tom0108 0:761a63c6d020 555 △ ボタン == 回収（シーツ兼シャツ）
Tom0108 0:761a63c6d020 556 □ ボタン == 回収（タオル）
Tom0108 0:761a63c6d020 557 上 ボタン == ハンガーかけ機構 上昇
Tom0108 0:761a63c6d020 558 下 ボタン == ハンガーかけ機構 下降
Tom0108 0:761a63c6d020 559 L1 ボタン == リセット（足回り）
Tom0108 0:761a63c6d020 560 L2 ボタン == タオル掛け
Tom0108 6:040d001acb12 561 R2 ボタン == タオル真ん中掴む
Tom0108 0:761a63c6d020 562 select == ハンガー掛け 最大まで上昇
Tom0108 0:761a63c6d020 563 start == ハンガー展開
Tom0108 0:761a63c6d020 564 */