Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@17:446be2c278d1, 2019-09-24 (annotated)

Committer:
Tom0108
Date:
Tue Sep 24 04:28:34 2019 +0000
Revision:
17:446be2c278d1
Parent:
16:829b953d1ac1 
09241328 Diet incomplete

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