Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@16:829b953d1ac1, 2019-09-23 (annotated)

Committer:
Tom0108
Date:
Mon Sep 23 08:45:17 2019 +0000
Revision:
16:829b953d1ac1
Parent:
15:68720ff6bbc9
Child:
17:446be2c278d1
09231745 on a diet

Who changed what in which revision?

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