Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@9:64fcbcc182d9, 2019-08-27 (annotated)

Committer:
Tom0108
Date:
Tue Aug 27 10:59:38 2019 +0000
Revision:
9:64fcbcc182d9
Parent:
8:b79d21c8178b
Child:
10:7c73e1577742
08/27 Speed gain change

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 1:199c4a71da88 21 //タオル引く用
Tom0108 3:1063aa133b62 22 int pull=0, pull_f=0;
Tom0108 1:199c4a71da88 23
Tom0108 4:236e5c58e8ee 24 //タオル掴む用
Tom0108 8:b79d21c8178b 25 //spull_f: 中央のリミットでつかむ
Tom0108 8:b79d21c8178b 26 //back_f: 真ん中目で下がるとき
Tom0108 8:b79d21c8178b 27 int grab=0, grab_f=0, spull_f=0;
Tom0108 8:b79d21c8178b 28 int back_f=0;
Tom0108 3:1063aa133b62 29
Tom0108 4:236e5c58e8ee 30 //タオル横展開用
Tom0108 3:1063aa133b62 31 int unfold_f=0;
Tom0108 3:1063aa133b62 32
Tom0108 6:040d001acb12 33 //y_deg: ジャイロの生の値
Tom0108 6:040d001acb12 34 //yaw: 実際に使う角度
Tom0108 8:b79d21c8178b 35 double y_deg[2], yaw=def_val;
Tom0108 1:199c4a71da88 36
Tom0108 6:040d001acb12 37 //ジャイロさんぷりんぐ用
Tom0108 5:fcc79e507610 38 double sum=0, ave=0;
Tom0108 5:fcc79e507610 39 int offset_cnt=0;
Tom0108 3:1063aa133b62 40
Tom0108 3:1063aa133b62 41 //Turn_val: 補正の目標値
Tom0108 3:1063aa133b62 42 //dif_val: 補正値との差
Tom0108 8:b79d21c8178b 43 double Turn_val=def_val, dif_val;
Tom0108 3:1063aa133b62 44
Tom0108 3:1063aa133b62 45 //short_lim: 腕中央
Tom0108 3:1063aa133b62 46 //max_lim: 腕奥側
Tom0108 3:1063aa133b62 47 //short_lim: 腕手前側
Tom0108 3:1063aa133b62 48 int short_lim, max_lim, start_lim;
Tom0108 8:b79d21c8178b 49 int short_f=0;
Tom0108 1:199c4a71da88 50
Tom0108 6:040d001acb12 51 //足回りのpwm値
Tom0108 6:040d001acb12 52 int duty[4];
Tom0108 4:236e5c58e8ee 53
Tom0108 6:040d001acb12 54 // 足回り
Tom0108 6:040d001acb12 55 int lx,ly,rx;
Tom0108 0:761a63c6d020 56
Tom0108 6:040d001acb12 57 int limit_up,limit_down; // リミットスイッチ
Tom0108 6:040d001acb12 58 int slide_start,slide_stop; //かごのリミット
Tom0108 0:761a63c6d020 59
Tom0108 0:761a63c6d020 60 int as=0;
Tom0108 0:761a63c6d020 61
Tom0108 0:761a63c6d020 62 /*----------------------------------- main -----------------------------------*/
Tom0108 0:761a63c6d020 63 int main()
Tom0108 0:761a63c6d020 64 {
Tom0108 0:761a63c6d020 65 //flipper1.attach(&flip1, 0.001); // 割り込み
Tom0108 5:fcc79e507610 66
Tom0108 0:761a63c6d020 67 while(1) {
Tom0108 1:199c4a71da88 68 //自動系のタイマースタート
Tom0108 1:199c4a71da88 69 tim.start();
Tom0108 6:040d001acb12 70 drift_tim.start();
Tom0108 0:761a63c6d020 71 mpu.loop();
Tom0108 0:761a63c6d020 72
Tom0108 6:040d001acb12 73 //ジャイロから値を取得
Tom0108 5:fcc79e507610 74 y_deg[1] = y_deg[0];
Tom0108 5:fcc79e507610 75 y_deg[0] = (double)mpu.ypr[0] * 180 / M_PI;
Tom0108 6:040d001acb12 76
Tom0108 5:fcc79e507610 77 //180～-180をまたいだ時
Tom0108 5:fcc79e507610 78 if(y_deg[1]<-90 && y_deg[0]>90) y_deg[1]+=360;
Tom0108 5:fcc79e507610 79 else if(y_deg[1]>90 && y_deg[0]<-90) y_deg[1]-=360;
Tom0108 0:761a63c6d020 80
Tom0108 5:fcc79e507610 81 if(sbdbt.State_check()) {
Tom0108 5:fcc79e507610 82 for(int i=0; i<7; i++) i2c.In(In_Data,i);
Tom0108 6:040d001acb12 83
Tom0108 5:fcc79e507610 84 //誤差をさんぷりんぐ
Tom0108 5:fcc79e507610 85 if(offset_cnt<1000) {
Tom0108 8:b79d21c8178b 86 PALETTE(YELLOW);
Tom0108 5:fcc79e507610 87 if(offset_cnt!=0) {
Tom0108 5:fcc79e507610 88 sum+=(y_deg[0]-y_deg[1]);
Tom0108 5:fcc79e507610 89 ave=sum/offset_cnt;
Tom0108 5:fcc79e507610 90 }
Tom0108 5:fcc79e507610 91 offset_cnt++;
Tom0108 6:040d001acb12 92 } else {
Tom0108 8:b79d21c8178b 93 //ゾーン対応
Tom0108 8:b79d21c8178b 94 #ifdef _RED_ZONE_
Tom0108 8:b79d21c8178b 95 PALETTE(RED);
Tom0108 8:b79d21c8178b 96 #else
Tom0108 8:b79d21c8178b 97 PALETTE(BLUE);
Tom0108 8:b79d21c8178b 98 #endif
Tom0108 6:040d001acb12 99 yaw -= (y_deg[0]-y_deg[1])-ave;
Tom0108 6:040d001acb12 100 }
Tom0108 6:040d001acb12 101
Tom0108 6:040d001acb12 102 dif_val = yaw-Turn_val;
Tom0108 6:040d001acb12 103 //制限
Tom0108 6:040d001acb12 104 if(dif_val>=30) dif_val=30;
Tom0108 6:040d001acb12 105 else if(dif_val<=-30) dif_val=-30;
Tom0108 0:761a63c6d020 106
Tom0108 0:761a63c6d020 107 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 108 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 109 rx = (sbdbt.RX - 64)*60/64;
Tom0108 8:b79d21c8178b 110
Tom0108 8:b79d21c8178b 111 //R1で停止
Tom0108 8:b79d21c8178b 112 if(sbdbt.R1==0) {
Tom0108 8:b79d21c8178b 113 //メカナム基本動作
Tom0108 8:b79d21c8178b 114 mecanum_Move(lx, ly, rx);
Tom0108 0:761a63c6d020 115
Tom0108 8:b79d21c8178b 116 //旋回している間タイマーをリセット
Tom0108 8:b79d21c8178b 117 if(sbdbt.RX != 64) drift_tim.reset();
Tom0108 8:b79d21c8178b 118
Tom0108 8:b79d21c8178b 119 //旋回して慣性で動いた後の角度に補正する
Tom0108 8:b79d21c8178b 120 if(drift_tim.read_ms()<500) Turn_val=yaw;
Tom0108 0:761a63c6d020 121
Tom0108 8:b79d21c8178b 122 //角度補正
Tom0108 8:b79d21c8178b 123 AngleCorrection(yaw, Turn_val);
Tom0108 8:b79d21c8178b 124
Tom0108 8:b79d21c8178b 125 /*---ハンガーかけるやつ----------------------------------------------*/
Tom0108 8:b79d21c8178b 126 if(sbdbt.SELECT)as=1;//selectキー
Tom0108 3:1063aa133b62 127
Tom0108 8:b79d21c8178b 128 if(as==1 && limit_up == 0) {
Tom0108 8:b79d21c8178b 129 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 8:b79d21c8178b 130 MD_SET_PWM(MD_Data, 5,100);
Tom0108 8:b79d21c8178b 131 } else if(UPkey && limit_up == 0) {
Tom0108 8:b79d21c8178b 132 as=0;
Tom0108 8:b79d21c8178b 133 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 8:b79d21c8178b 134 MD_SET_PWM(MD_Data, 5,100);
Tom0108 8:b79d21c8178b 135 } else if(as==1 && limit_up == 1) {
Tom0108 8:b79d21c8178b 136 as=0;
Tom0108 8:b79d21c8178b 137 MD_SET_PWM(MD_Data,5,0);
Tom0108 8:b79d21c8178b 138 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 8:b79d21c8178b 139 } else if(DOWNkey && limit_down == 0) {
Tom0108 8:b79d21c8178b 140 MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
Tom0108 8:b79d21c8178b 141 MD_SET_PWM(MD_Data, 5,100);
Tom0108 8:b79d21c8178b 142 } else {
Tom0108 8:b79d21c8178b 143 MD_SET_PWM(MD_Data,5,0);
Tom0108 8:b79d21c8178b 144 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 8:b79d21c8178b 145 }
Tom0108 6:040d001acb12 146
Tom0108 8:b79d21c8178b 147 //タオルつかむ 四角
Tom0108 8:b79d21c8178b 148 if(grab_f || spull_f) {
Tom0108 8:b79d21c8178b 149 PALETTE(GREEN);
Tom0108 9:64fcbcc182d9 150
Tom0108 9:64fcbcc182d9 151 //真ん中で止まる用s
Tom0108 8:b79d21c8178b 152 short_f=1;
Tom0108 8:b79d21c8178b 153 switch(grab) {
Tom0108 8:b79d21c8178b 154 //腕を前に動かす
Tom0108 8:b79d21c8178b 155 case 0:
Tom0108 8:b79d21c8178b 156 //爪開く
Tom0108 8:b79d21c8178b 157 i2c.Out_Set(Out_Data,1,1);
Tom0108 0:761a63c6d020 158
Tom0108 8:b79d21c8178b 159 if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 8:b79d21c8178b 160 else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 161 MD_SET_PWM (MD_Data,4,60);
Tom0108 0:761a63c6d020 162
Tom0108 8:b79d21c8178b 163 if(spull_f) {
Tom0108 8:b79d21c8178b 164 if(short_lim==1) {
Tom0108 8:b79d21c8178b 165 grab++;
Tom0108 8:b79d21c8178b 166 back_f=0;
Tom0108 8:b79d21c8178b 167 tim.reset();
Tom0108 8:b79d21c8178b 168 } else if(max_lim) back_f=1;
Tom0108 8:b79d21c8178b 169 } else {
Tom0108 8:b79d21c8178b 170 if(max_lim==1) {
Tom0108 8:b79d21c8178b 171 grab++;
Tom0108 8:b79d21c8178b 172 back_f=0;
Tom0108 8:b79d21c8178b 173 tim.reset();
Tom0108 8:b79d21c8178b 174 }
Tom0108 8:b79d21c8178b 175 }
Tom0108 8:b79d21c8178b 176 break;
Tom0108 8:b79d21c8178b 177
Tom0108 8:b79d21c8178b 178 case 1:
Tom0108 8:b79d21c8178b 179 //腕停止
Tom0108 8:b79d21c8178b 180 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 181 MD_SET_PWM(MD_Data,4,0);
Tom0108 8:b79d21c8178b 182
Tom0108 8:b79d21c8178b 183 //腕おろす
Tom0108 8:b79d21c8178b 184 i2c.Out_Set(Out_Data,0,1);
Tom0108 9:64fcbcc182d9 185 if(tim.read_ms()>200) {
Tom0108 8:b79d21c8178b 186 grab++;
Tom0108 8:b79d21c8178b 187 tim.reset();
Tom0108 8:b79d21c8178b 188 }
Tom0108 8:b79d21c8178b 189 break;
Tom0108 0:761a63c6d020 190
Tom0108 8:b79d21c8178b 191 case 2:
Tom0108 8:b79d21c8178b 192 //爪閉じる
Tom0108 8:b79d21c8178b 193 i2c.Out_Set(Out_Data,1,0);
Tom0108 9:64fcbcc182d9 194 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 195 grab++;
Tom0108 8:b79d21c8178b 196 tim.reset();
Tom0108 8:b79d21c8178b 197 }
Tom0108 8:b79d21c8178b 198 break;
Tom0108 8:b79d21c8178b 199
Tom0108 8:b79d21c8178b 200 case 3:
Tom0108 8:b79d21c8178b 201 //腕上げる
Tom0108 8:b79d21c8178b 202 i2c.Out_Set(Out_Data,0,0);
Tom0108 9:64fcbcc182d9 203 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 204 grab++;
Tom0108 8:b79d21c8178b 205 tim.reset();
Tom0108 8:b79d21c8178b 206 }
Tom0108 8:b79d21c8178b 207 break;
Tom0108 8:b79d21c8178b 208
Tom0108 8:b79d21c8178b 209 //腕戻す
Tom0108 8:b79d21c8178b 210 case 4:
Tom0108 8:b79d21c8178b 211 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 9:64fcbcc182d9 212 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 213 if(start_lim==1) {
Tom0108 8:b79d21c8178b 214 grab_f=0;
Tom0108 8:b79d21c8178b 215 spull_f=0;
Tom0108 8:b79d21c8178b 216 tim.reset();
Tom0108 8:b79d21c8178b 217 }
Tom0108 8:b79d21c8178b 218 break;
Tom0108 8:b79d21c8178b 219 }
Tom0108 8:b79d21c8178b 220 } //if(grab_f)
Tom0108 0:761a63c6d020 221
Tom0108 8:b79d21c8178b 222 //タオル引く 丸
Tom0108 8:b79d21c8178b 223 else if(pull_f) {
Tom0108 8:b79d21c8178b 224 PALETTE(CYAN);
Tom0108 8:b79d21c8178b 225 short_f=1;
Tom0108 8:b79d21c8178b 226 switch(pull) {
Tom0108 8:b79d21c8178b 227 //腕を前に動かす
Tom0108 8:b79d21c8178b 228 case 0:
Tom0108 8:b79d21c8178b 229 //爪閉じる
Tom0108 8:b79d21c8178b 230 i2c.Out_Set(Out_Data,1,0);
Tom0108 8:b79d21c8178b 231
Tom0108 8:b79d21c8178b 232 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 233 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 234
Tom0108 8:b79d21c8178b 235 if(max_lim==1) {
Tom0108 8:b79d21c8178b 236 pull++;
Tom0108 8:b79d21c8178b 237 back_f=0;
Tom0108 8:b79d21c8178b 238 tim.reset();
Tom0108 8:b79d21c8178b 239 }
Tom0108 8:b79d21c8178b 240 break;
Tom0108 8:b79d21c8178b 241
Tom0108 8:b79d21c8178b 242 case 1:
Tom0108 8:b79d21c8178b 243 //腕停止
Tom0108 8:b79d21c8178b 244 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 245 MD_SET_PWM (MD_Data,4,0);
Tom0108 2:47954f05d32d 246
Tom0108 8:b79d21c8178b 247 //腕おろす
Tom0108 8:b79d21c8178b 248 i2c.Out_Set(Out_Data,0,1);
Tom0108 9:64fcbcc182d9 249 if(tim.read_ms()>100) {
Tom0108 8:b79d21c8178b 250 pull++;
Tom0108 8:b79d21c8178b 251 tim.reset();
Tom0108 8:b79d21c8178b 252 }
Tom0108 8:b79d21c8178b 253 break;
Tom0108 8:b79d21c8178b 254
Tom0108 9:64fcbcc182d9 255 //停止
Tom0108 8:b79d21c8178b 256 case 2:
Tom0108 9:64fcbcc182d9 257 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 258 pull++;
Tom0108 8:b79d21c8178b 259 tim.reset();
Tom0108 8:b79d21c8178b 260 }
Tom0108 8:b79d21c8178b 261 break;
Tom0108 0:761a63c6d020 262
Tom0108 8:b79d21c8178b 263 case 3:
Tom0108 8:b79d21c8178b 264 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 9:64fcbcc182d9 265 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 266 if(start_lim==1) {
Tom0108 8:b79d21c8178b 267 pull_f=0;
Tom0108 8:b79d21c8178b 268 tim.reset();
Tom0108 8:b79d21c8178b 269 }
Tom0108 8:b79d21c8178b 270 break;
Tom0108 8:b79d21c8178b 271 }
Tom0108 8:b79d21c8178b 272 } //if(pull_f)
Tom0108 8:b79d21c8178b 273
Tom0108 8:b79d21c8178b 274 //かごを倒す
Tom0108 8:b79d21c8178b 275 else if(kago_f) {
Tom0108 8:b79d21c8178b 276 PALETTE(MAGENTA);
Tom0108 8:b79d21c8178b 277 short_f=1;
Tom0108 8:b79d21c8178b 278 //腕の先を閉じておく
Tom0108 8:b79d21c8178b 279 i2c.Out_Set(Out_Data,1,0);
Tom0108 1:199c4a71da88 280
Tom0108 8:b79d21c8178b 281 switch(kago) {
Tom0108 8:b79d21c8178b 282 //腕を前に動かす
Tom0108 8:b79d21c8178b 283 case 0:
Tom0108 8:b79d21c8178b 284 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 285 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 286 if(max_lim==1) {
Tom0108 8:b79d21c8178b 287 kago++;
Tom0108 8:b79d21c8178b 288 tim.reset();
Tom0108 8:b79d21c8178b 289 }
Tom0108 8:b79d21c8178b 290 break;
Tom0108 8:b79d21c8178b 291
Tom0108 8:b79d21c8178b 292 case 1:
Tom0108 8:b79d21c8178b 293 //腕停止
Tom0108 8:b79d21c8178b 294 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 295 MD_SET_PWM (MD_Data,4,0);
Tom0108 8:b79d21c8178b 296
Tom0108 8:b79d21c8178b 297 //かご回転
Tom0108 8:b79d21c8178b 298 MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
Tom0108 9:64fcbcc182d9 299 MD_SET_PWM (MD_Data,6,100);
Tom0108 8:b79d21c8178b 300 if(slide_stop==1) {
Tom0108 8:b79d21c8178b 301 kago++;
Tom0108 8:b79d21c8178b 302 tim.reset();
Tom0108 8:b79d21c8178b 303 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 8:b79d21c8178b 304 MD_SET_PWM (MD_Data,6,0);
Tom0108 8:b79d21c8178b 305 }
Tom0108 8:b79d21c8178b 306 break;
Tom0108 3:1063aa133b62 307
Tom0108 8:b79d21c8178b 308 case 2:
Tom0108 8:b79d21c8178b 309 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 310 kago++;
Tom0108 8:b79d21c8178b 311 tim.reset();
Tom0108 8:b79d21c8178b 312 }
Tom0108 8:b79d21c8178b 313 break;
Tom0108 8:b79d21c8178b 314
Tom0108 8:b79d21c8178b 315 case 3:
Tom0108 8:b79d21c8178b 316 MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
Tom0108 9:64fcbcc182d9 317 MD_SET_PWM (MD_Data,6,100);
Tom0108 8:b79d21c8178b 318 if(slide_start==1) kago++;
Tom0108 8:b79d21c8178b 319 break;
Tom0108 3:1063aa133b62 320
Tom0108 8:b79d21c8178b 321 case 4:
Tom0108 8:b79d21c8178b 322 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 9:64fcbcc182d9 323 MD_SET_PWM (MD_Data,4,60);
Tom0108 8:b79d21c8178b 324 if(start_lim==1) {
Tom0108 8:b79d21c8178b 325 kago_f=0;
Tom0108 8:b79d21c8178b 326 tim.reset();
Tom0108 8:b79d21c8178b 327 }
Tom0108 8:b79d21c8178b 328 break;
Tom0108 8:b79d21c8178b 329 }
Tom0108 8:b79d21c8178b 330 } else {
Tom0108 8:b79d21c8178b 331 //爪開く
Tom0108 8:b79d21c8178b 332 i2c.Out_Set(Out_Data,1,1);
Tom0108 8:b79d21c8178b 333 //腕上げる
Tom0108 8:b79d21c8178b 334 i2c.Out_Set(Out_Data,0,0);
Tom0108 8:b79d21c8178b 335
Tom0108 8:b79d21c8178b 336 //腕真ん中で停止
Tom0108 8:b79d21c8178b 337 if(short_f) {
Tom0108 9:64fcbcc182d9 338 if(short_lim==1) {
Tom0108 9:64fcbcc182d9 339 short_f=0;
Tom0108 9:64fcbcc182d9 340 back_f=0;
Tom0108 9:64fcbcc182d9 341 }
Tom0108 9:64fcbcc182d9 342 //奥のリミットに当たったら逆回転
Tom0108 9:64fcbcc182d9 343 if(max_lim==1) back_f=1;
Tom0108 9:64fcbcc182d9 344 //手前のリミットに当たったら正回転
Tom0108 9:64fcbcc182d9 345 else if(start_lim==1) back_f=0;
Tom0108 9:64fcbcc182d9 346
Tom0108 9:64fcbcc182d9 347 if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 9:64fcbcc182d9 348 else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 349 MD_SET_PWM (MD_Data,4,30);
Tom0108 8:b79d21c8178b 350 } else {
Tom0108 2:47954f05d32d 351 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 2:47954f05d32d 352 MD_SET_PWM (MD_Data,4,0);
Tom0108 8:b79d21c8178b 353 }
Tom0108 8:b79d21c8178b 354 //かご停止
Tom0108 8:b79d21c8178b 355 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 8:b79d21c8178b 356 MD_SET_PWM (MD_Data,6,0);
Tom0108 1:199c4a71da88 357
Tom0108 8:b79d21c8178b 358 kago=0;
Tom0108 8:b79d21c8178b 359 pull=0;
Tom0108 8:b79d21c8178b 360 grab=0;
Tom0108 0:761a63c6d020 361
Tom0108 8:b79d21c8178b 362 tim.reset();
Tom0108 0:761a63c6d020 363 }
Tom0108 8:b79d21c8178b 364 }
Tom0108 8:b79d21c8178b 365 //非常停止
Tom0108 8:b79d21c8178b 366 else {
Tom0108 8:b79d21c8178b 367 for(int i=0; i<8; i++) {
Tom0108 8:b79d21c8178b 368 MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
Tom0108 8:b79d21c8178b 369 MD_SET_PWM (MD_Data,i,0);
Tom0108 8:b79d21c8178b 370 }
Tom0108 0:761a63c6d020 371 }
Tom0108 0:761a63c6d020 372
Tom0108 3:1063aa133b62 373 //-------------- 出力 --------------//
Tom0108 3:1063aa133b62 374 //第二引数には使う最大の個数
Tom0108 8:b79d21c8178b 375 i2c.Out(Out_Data,8);
Tom0108 3:1063aa133b62 376
Tom0108 3:1063aa133b62 377 //モータ出力
Tom0108 3:1063aa133b62 378 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
Tom0108 3:1063aa133b62 379
Tom0108 3:1063aa133b62 380 /* ----------- りみっと --------------- */
Tom0108 3:1063aa133b62 381 limit_up = (In_Data+4)->in_data;
Tom0108 3:1063aa133b62 382 limit_down = (In_Data+5)->in_data;
Tom0108 3:1063aa133b62 383 max_lim = (In_Data+1)->in_data;
Tom0108 3:1063aa133b62 384 start_lim =(In_Data+2)->in_data;
Tom0108 3:1063aa133b62 385 short_lim = (In_Data+6)->in_data;
Tom0108 3:1063aa133b62 386 slide_start = (In_Data+3)->in_data;
Tom0108 3:1063aa133b62 387 slide_stop = (In_Data+0)->in_data;
Tom0108 3:1063aa133b62 388
Tom0108 3:1063aa133b62 389 //半自動のデバック
Tom0108 3:1063aa133b62 390 // pc.printf("pull %2d%2d ",pull,pull_f);
Tom0108 3:1063aa133b62 391 // pc.printf("grab %2d%2d ",grab,grab_f);
Tom0108 3:1063aa133b62 392 // pc.printf("kago %2d%2d ",kago,kago_f);
Tom0108 6:040d001acb12 393 // pc.printf("yaw:%.1f dif:%.1f",yaw,dif_val);
Tom0108 6:040d001acb12 394 pc.printf("0:%.1f 1:%.1f yaw:%.1f dif:%.1f",y_deg[0],y_deg[1],yaw,dif_val);
Tom0108 4:236e5c58e8ee 395 // pc.printf("%2d ",dir);
Tom0108 3:1063aa133b62 396 pc.printf("\n\r");
Tom0108 3:1063aa133b62 397
Tom0108 3:1063aa133b62 398
Tom0108 3:1063aa133b62 399 /* --------------電磁弁系-------------------- */
Tom0108 0:761a63c6d020 400 /*タオル掛け*/
Tom0108 0:761a63c6d020 401 if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
Tom0108 0:761a63c6d020 402 else i2c.Out_Set(Out_Data,2,0);
Tom0108 3:1063aa133b62 403
Tom0108 0:761a63c6d020 404 /*タオルを引っ張るやつ*/
Tom0108 1:199c4a71da88 405 if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
Tom0108 1:199c4a71da88 406 else i2c.Out_Set(Out_Data,4,0);
Tom0108 0:761a63c6d020 407
Tom0108 3:1063aa133b62 408 if(unfold_f)i2c.Out_Set(Out_Data,3,1);
Tom0108 3:1063aa133b62 409 else i2c.Out_Set(Out_Data,3,0);
Tom0108 0:761a63c6d020 410
Tom0108 0:761a63c6d020 411
Tom0108 3:1063aa133b62 412 /* ----------------ボタン系--------------- */
Tom0108 6:040d001acb12 413 //角度リセット
Tom0108 2:47954f05d32d 414 if(sbdbt.L1) {
Tom0108 8:b79d21c8178b 415 yaw=def_val;
Tom0108 8:b79d21c8178b 416 Turn_val=def_val;
Tom0108 2:47954f05d32d 417 }
Tom0108 3:1063aa133b62 418 //かごを倒す
Tom0108 6:040d001acb12 419 if(CROSS) {
Tom0108 1:199c4a71da88 420 if(once==0) {
Tom0108 3:1063aa133b62 421 kago_f++;
Tom0108 3:1063aa133b62 422 kago_f%=2;
Tom0108 1:199c4a71da88 423 once++;
Tom0108 1:199c4a71da88 424 }
Tom0108 3:1063aa133b62 425 }
Tom0108 3:1063aa133b62 426 //タオルを掴んで動かす
Tom0108 3:1063aa133b62 427 else if(SQUARE) {
Tom0108 1:199c4a71da88 428 if(once==0) {
Tom0108 3:1063aa133b62 429 grab_f++;
Tom0108 3:1063aa133b62 430 grab_f%=2;
Tom0108 1:199c4a71da88 431 once++;
Tom0108 1:199c4a71da88 432 }
Tom0108 4:236e5c58e8ee 433 } else if(TRIANGLE) {
Tom0108 4:236e5c58e8ee 434 if(once==0) {
Tom0108 8:b79d21c8178b 435 spull_f++;
Tom0108 8:b79d21c8178b 436 spull_f%=2;
Tom0108 4:236e5c58e8ee 437 once++;
Tom0108 4:236e5c58e8ee 438 }
Tom0108 3:1063aa133b62 439 }
Tom0108 3:1063aa133b62 440 //倒すを引いて動かす
Tom0108 3:1063aa133b62 441 else if(CIRCLE) {
Tom0108 1:199c4a71da88 442 if(once==0) {
Tom0108 3:1063aa133b62 443 pull_f++;
Tom0108 3:1063aa133b62 444 pull_f%=2;
Tom0108 3:1063aa133b62 445 once++;
Tom0108 3:1063aa133b62 446 }
Tom0108 3:1063aa133b62 447 }
Tom0108 3:1063aa133b62 448 //タオル展開用
Tom0108 3:1063aa133b62 449 else if(sbdbt.START) {
Tom0108 3:1063aa133b62 450 if(once==0) {
Tom0108 3:1063aa133b62 451 unfold_f++;
Tom0108 3:1063aa133b62 452 unfold_f%=2;
Tom0108 1:199c4a71da88 453 once++;
Tom0108 1:199c4a71da88 454 }
Tom0108 1:199c4a71da88 455 } else once=0;
Tom0108 1:199c4a71da88 456
Tom0108 1:199c4a71da88 457
Tom0108 0:761a63c6d020 458 } // if(sbdbt.State_check())
Tom0108 0:761a63c6d020 459 } // while(1)
Tom0108 0:761a63c6d020 460 } // int main()
Tom0108 0:761a63c6d020 461
Tom0108 0:761a63c6d020 462 /* メカナムの基本移動 */
Tom0108 6:040d001acb12 463 void mecanum_Move(int lx, int ly, int rx)
Tom0108 0:761a63c6d020 464 {
Tom0108 6:040d001acb12 465 duty[0]=lx+ly;
Tom0108 6:040d001acb12 466 duty[1]=-(-lx+ly);
Tom0108 6:040d001acb12 467 duty[2]=-lx+ly;
Tom0108 6:040d001acb12 468 duty[3]=-(lx+ly);
Tom0108 0:761a63c6d020 469
Tom0108 0:761a63c6d020 470 for(int i=0; i<4; i++) {
Tom0108 8:b79d21c8178b 471 //旋回
Tom0108 6:040d001acb12 472 duty[i]+=rx;
Tom0108 8:b79d21c8178b 473
Tom0108 8:b79d21c8178b 474 //制限
Tom0108 8:b79d21c8178b 475 if(duty[i]>=99) duty[i]=99;
Tom0108 8:b79d21c8178b 476 else if(duty[i]<=-99) duty[i]=-99;
Tom0108 6:040d001acb12 477 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 478 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 479 }
Tom0108 0:761a63c6d020 480 }
Tom0108 0:761a63c6d020 481
Tom0108 6:040d001acb12 482 void AngleCorrection(double n_angle, double t_angle)
Tom0108 0:761a63c6d020 483 {
Tom0108 6:040d001acb12 484 double dif=-(t_angle-n_angle);
Tom0108 6:040d001acb12 485
Tom0108 6:040d001acb12 486 if(dif>=30) dif=30;
Tom0108 6:040d001acb12 487 else if(dif<=-30) dif=-30;
Tom0108 6:040d001acb12 488
Tom0108 0:761a63c6d020 489 for(int i=0; i<4; i++) {
Tom0108 6:040d001acb12 490 duty[i]+=dif;
Tom0108 6:040d001acb12 491 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 492 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 493 }
Tom0108 6:040d001acb12 494
Tom0108 0:761a63c6d020 495 }
Tom0108 0:761a63c6d020 496
Tom0108 0:761a63c6d020 497 /*操作法*/
Tom0108 0:761a63c6d020 498 /*
Tom0108 0:761a63c6d020 499 〇 ボタン == 回収（引きずる）
Tom0108 0:761a63c6d020 500 ? ボタン == 籠
Tom0108 0:761a63c6d020 501 △ ボタン == 回収（シーツ兼シャツ）
Tom0108 0:761a63c6d020 502 □ ボタン == 回収（タオル）
Tom0108 0:761a63c6d020 503 上 ボタン == ハンガーかけ機構 上昇
Tom0108 0:761a63c6d020 504 下 ボタン == ハンガーかけ機構 下降
Tom0108 0:761a63c6d020 505 L1 ボタン == リセット（足回り）
Tom0108 0:761a63c6d020 506 L2 ボタン == タオル掛け
Tom0108 6:040d001acb12 507 R2 ボタン == タオル真ん中掴む
Tom0108 0:761a63c6d020 508 select == ハンガー掛け 最大まで上昇
Tom0108 0:761a63c6d020 509 start == ハンガー展開
Tom0108 0:761a63c6d020 510 */
Tom0108 0:761a63c6d020 511 /*その他注意点*/
Tom0108 0:761a63c6d020 512 /*
Tom0108 6:040d001acb12 513 動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ
Tom0108 0:761a63c6d020 514 メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
Tom0108 0:761a63c6d020 515 あまり急発進をさせない（たまに暴走）
Tom0108 0:761a63c6d020 516 暴走したときはコントローラーを動かしたら治るはず
Tom0108 0:761a63c6d020 517 コントローラー接続が切れたらSTMの黒い奴を押す（わからないときは近くの回路班へ）
Tom0108 0:761a63c6d020 518 */