Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@13:48c4a4c95e77, 2019-09-15 (annotated)

Committer:
Tom0108
Date:
Sun Sep 15 11:43:13 2019 +0000
Revision:
13:48c4a4c95e77
Parent:
12:7e33e400a48d
Child:
14:e6d16095ffa8
Move arm manually

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