Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@10:7c73e1577742, 2019-08-30 (annotated)

Committer:
Tom0108
Date:
Fri Aug 30 07:47:41 2019 +0000
Revision:
10:7c73e1577742
Parent:
9:64fcbcc182d9
Child:
11:c1a22e2e3534
Now you can use bno055

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