Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@11:c1a22e2e3534, 2019-09-04 (annotated)

Committer:
Tom0108
Date:
Wed Sep 04 05:31:40 2019 +0000
Revision:
11:c1a22e2e3534
Parent:
10:7c73e1577742
Child:
12:7e33e400a48d
Increase in solenoid valves for opening to the atmosphere

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