Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@8:b79d21c8178b, 2019-08-27 (annotated)

Committer:
Tom0108
Date:
Tue Aug 27 08:54:15 2019 +0000
Revision:
8:b79d21c8178b
Parent:
6:040d001acb12
Child:
9:64fcbcc182d9
The color changes according to the zone

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 8:b79d21c8178b 150 short_f=1;
Tom0108 8:b79d21c8178b 151 switch(grab) {
Tom0108 8:b79d21c8178b 152 //腕を前に動かす
Tom0108 8:b79d21c8178b 153 case 0:
Tom0108 8:b79d21c8178b 154 //爪開く
Tom0108 8:b79d21c8178b 155 i2c.Out_Set(Out_Data,1,1);
Tom0108 0:761a63c6d020 156
Tom0108 8:b79d21c8178b 157 if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 8:b79d21c8178b 158 else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 8:b79d21c8178b 159 MD_SET_PWM (MD_Data,4,40);
Tom0108 0:761a63c6d020 160
Tom0108 8:b79d21c8178b 161 if(spull_f) {
Tom0108 8:b79d21c8178b 162 if(short_lim==1) {
Tom0108 8:b79d21c8178b 163 grab++;
Tom0108 8:b79d21c8178b 164 back_f=0;
Tom0108 8:b79d21c8178b 165 tim.reset();
Tom0108 8:b79d21c8178b 166 } else if(max_lim) back_f=1;
Tom0108 8:b79d21c8178b 167 } else {
Tom0108 8:b79d21c8178b 168 if(max_lim==1) {
Tom0108 8:b79d21c8178b 169 grab++;
Tom0108 8:b79d21c8178b 170 back_f=0;
Tom0108 8:b79d21c8178b 171 tim.reset();
Tom0108 8:b79d21c8178b 172 }
Tom0108 8:b79d21c8178b 173 }
Tom0108 8:b79d21c8178b 174 break;
Tom0108 8:b79d21c8178b 175
Tom0108 8:b79d21c8178b 176 case 1:
Tom0108 8:b79d21c8178b 177 //腕停止
Tom0108 8:b79d21c8178b 178 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 179 MD_SET_PWM(MD_Data,4,0);
Tom0108 8:b79d21c8178b 180
Tom0108 8:b79d21c8178b 181 //腕おろす
Tom0108 8:b79d21c8178b 182 i2c.Out_Set(Out_Data,0,1);
Tom0108 8:b79d21c8178b 183 if(tim.read_ms()>300) {
Tom0108 8:b79d21c8178b 184 grab++;
Tom0108 8:b79d21c8178b 185 tim.reset();
Tom0108 8:b79d21c8178b 186 }
Tom0108 8:b79d21c8178b 187 break;
Tom0108 0:761a63c6d020 188
Tom0108 8:b79d21c8178b 189 case 2:
Tom0108 8:b79d21c8178b 190 //爪閉じる
Tom0108 8:b79d21c8178b 191 i2c.Out_Set(Out_Data,1,0);
Tom0108 8:b79d21c8178b 192 if(tim.read_ms()>100) {
Tom0108 8:b79d21c8178b 193 grab++;
Tom0108 8:b79d21c8178b 194 tim.reset();
Tom0108 8:b79d21c8178b 195 }
Tom0108 8:b79d21c8178b 196 break;
Tom0108 8:b79d21c8178b 197
Tom0108 8:b79d21c8178b 198 case 3:
Tom0108 8:b79d21c8178b 199 //腕上げる
Tom0108 8:b79d21c8178b 200 i2c.Out_Set(Out_Data,0,0);
Tom0108 8:b79d21c8178b 201 if(tim.read_ms()>300) {
Tom0108 8:b79d21c8178b 202 grab++;
Tom0108 8:b79d21c8178b 203 tim.reset();
Tom0108 8:b79d21c8178b 204 }
Tom0108 8:b79d21c8178b 205 break;
Tom0108 8:b79d21c8178b 206
Tom0108 8:b79d21c8178b 207 //腕戻す
Tom0108 8:b79d21c8178b 208 case 4:
Tom0108 8:b79d21c8178b 209 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 8:b79d21c8178b 210 MD_SET_PWM (MD_Data,4,40);
Tom0108 8:b79d21c8178b 211 if(start_lim==1) {
Tom0108 8:b79d21c8178b 212 grab_f=0;
Tom0108 8:b79d21c8178b 213 spull_f=0;
Tom0108 8:b79d21c8178b 214 tim.reset();
Tom0108 8:b79d21c8178b 215 }
Tom0108 8:b79d21c8178b 216 break;
Tom0108 8:b79d21c8178b 217 }
Tom0108 8:b79d21c8178b 218 } //if(grab_f)
Tom0108 0:761a63c6d020 219
Tom0108 8:b79d21c8178b 220 //タオル引く 丸
Tom0108 8:b79d21c8178b 221 else if(pull_f) {
Tom0108 8:b79d21c8178b 222 PALETTE(CYAN);
Tom0108 8:b79d21c8178b 223 short_f=1;
Tom0108 8:b79d21c8178b 224 switch(pull) {
Tom0108 8:b79d21c8178b 225 //腕を前に動かす
Tom0108 8:b79d21c8178b 226 case 0:
Tom0108 8:b79d21c8178b 227 //爪閉じる
Tom0108 8:b79d21c8178b 228 i2c.Out_Set(Out_Data,1,0);
Tom0108 8:b79d21c8178b 229
Tom0108 8:b79d21c8178b 230 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 8:b79d21c8178b 231 MD_SET_PWM (MD_Data,4,40);
Tom0108 8:b79d21c8178b 232
Tom0108 8:b79d21c8178b 233 if(max_lim==1) {
Tom0108 8:b79d21c8178b 234 pull++;
Tom0108 8:b79d21c8178b 235 back_f=0;
Tom0108 8:b79d21c8178b 236 tim.reset();
Tom0108 8:b79d21c8178b 237 }
Tom0108 8:b79d21c8178b 238 break;
Tom0108 8:b79d21c8178b 239
Tom0108 8:b79d21c8178b 240 case 1:
Tom0108 8:b79d21c8178b 241 //腕停止
Tom0108 8:b79d21c8178b 242 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 243 MD_SET_PWM (MD_Data,4,0);
Tom0108 2:47954f05d32d 244
Tom0108 8:b79d21c8178b 245 //腕おろす
Tom0108 8:b79d21c8178b 246 i2c.Out_Set(Out_Data,0,1);
Tom0108 8:b79d21c8178b 247 if(tim.read_ms()>300) {
Tom0108 8:b79d21c8178b 248 pull++;
Tom0108 8:b79d21c8178b 249 tim.reset();
Tom0108 8:b79d21c8178b 250 }
Tom0108 8:b79d21c8178b 251 break;
Tom0108 8:b79d21c8178b 252
Tom0108 8:b79d21c8178b 253 case 2:
Tom0108 8:b79d21c8178b 254 if(tim.read_ms()>300) {
Tom0108 8:b79d21c8178b 255 pull++;
Tom0108 8:b79d21c8178b 256 tim.reset();
Tom0108 8:b79d21c8178b 257 }
Tom0108 8:b79d21c8178b 258 break;
Tom0108 0:761a63c6d020 259
Tom0108 8:b79d21c8178b 260 case 3:
Tom0108 8:b79d21c8178b 261 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 8:b79d21c8178b 262 MD_SET_PWM (MD_Data,4,40);
Tom0108 8:b79d21c8178b 263 if(start_lim==1) {
Tom0108 8:b79d21c8178b 264 pull_f=0;
Tom0108 8:b79d21c8178b 265 tim.reset();
Tom0108 8:b79d21c8178b 266 }
Tom0108 8:b79d21c8178b 267 break;
Tom0108 8:b79d21c8178b 268 }
Tom0108 8:b79d21c8178b 269 } //if(pull_f)
Tom0108 8:b79d21c8178b 270
Tom0108 8:b79d21c8178b 271 //かごを倒す
Tom0108 8:b79d21c8178b 272 else if(kago_f) {
Tom0108 8:b79d21c8178b 273 PALETTE(MAGENTA);
Tom0108 8:b79d21c8178b 274 short_f=1;
Tom0108 8:b79d21c8178b 275 //腕の先を閉じておく
Tom0108 8:b79d21c8178b 276 i2c.Out_Set(Out_Data,1,0);
Tom0108 1:199c4a71da88 277
Tom0108 8:b79d21c8178b 278 switch(kago) {
Tom0108 8:b79d21c8178b 279 //腕を前に動かす
Tom0108 8:b79d21c8178b 280 case 0:
Tom0108 8:b79d21c8178b 281 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 8:b79d21c8178b 282 MD_SET_PWM (MD_Data,4,40);
Tom0108 8:b79d21c8178b 283 if(max_lim==1) {
Tom0108 8:b79d21c8178b 284 kago++;
Tom0108 8:b79d21c8178b 285 tim.reset();
Tom0108 8:b79d21c8178b 286 }
Tom0108 8:b79d21c8178b 287 break;
Tom0108 8:b79d21c8178b 288
Tom0108 8:b79d21c8178b 289 case 1:
Tom0108 8:b79d21c8178b 290 //腕停止
Tom0108 8:b79d21c8178b 291 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 292 MD_SET_PWM (MD_Data,4,0);
Tom0108 8:b79d21c8178b 293
Tom0108 8:b79d21c8178b 294 //かご回転
Tom0108 8:b79d21c8178b 295 MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
Tom0108 8:b79d21c8178b 296 MD_SET_PWM (MD_Data,6,80);
Tom0108 8:b79d21c8178b 297 if(slide_stop==1) {
Tom0108 8:b79d21c8178b 298 kago++;
Tom0108 8:b79d21c8178b 299 tim.reset();
Tom0108 8:b79d21c8178b 300 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 8:b79d21c8178b 301 MD_SET_PWM (MD_Data,6,0);
Tom0108 8:b79d21c8178b 302 }
Tom0108 8:b79d21c8178b 303 break;
Tom0108 3:1063aa133b62 304
Tom0108 8:b79d21c8178b 305 case 2:
Tom0108 8:b79d21c8178b 306 if(tim.read_ms()>0) {
Tom0108 8:b79d21c8178b 307 kago++;
Tom0108 8:b79d21c8178b 308 tim.reset();
Tom0108 8:b79d21c8178b 309 }
Tom0108 8:b79d21c8178b 310 break;
Tom0108 8:b79d21c8178b 311
Tom0108 8:b79d21c8178b 312 case 3:
Tom0108 8:b79d21c8178b 313 MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
Tom0108 8:b79d21c8178b 314 MD_SET_PWM (MD_Data,6,80);
Tom0108 8:b79d21c8178b 315 if(slide_start==1) kago++;
Tom0108 8:b79d21c8178b 316 break;
Tom0108 3:1063aa133b62 317
Tom0108 8:b79d21c8178b 318 case 4:
Tom0108 8:b79d21c8178b 319 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 8:b79d21c8178b 320 MD_SET_PWM (MD_Data,4,40);
Tom0108 8:b79d21c8178b 321 if(start_lim==1) {
Tom0108 8:b79d21c8178b 322 kago_f=0;
Tom0108 8:b79d21c8178b 323 tim.reset();
Tom0108 8:b79d21c8178b 324 }
Tom0108 8:b79d21c8178b 325 break;
Tom0108 8:b79d21c8178b 326 }
Tom0108 8:b79d21c8178b 327 } else {
Tom0108 8:b79d21c8178b 328 //爪開く
Tom0108 8:b79d21c8178b 329 i2c.Out_Set(Out_Data,1,1);
Tom0108 8:b79d21c8178b 330 //腕上げる
Tom0108 8:b79d21c8178b 331 i2c.Out_Set(Out_Data,0,0);
Tom0108 8:b79d21c8178b 332
Tom0108 8:b79d21c8178b 333 //腕真ん中で停止
Tom0108 8:b79d21c8178b 334 if(short_f) {
Tom0108 8:b79d21c8178b 335 if(short_lim==1) short_f=0;
Tom0108 0:761a63c6d020 336 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 8:b79d21c8178b 337 MD_SET_PWM (MD_Data,4,20);
Tom0108 8:b79d21c8178b 338 } else {
Tom0108 2:47954f05d32d 339 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 2:47954f05d32d 340 MD_SET_PWM (MD_Data,4,0);
Tom0108 8:b79d21c8178b 341 }
Tom0108 8:b79d21c8178b 342 //かご停止
Tom0108 8:b79d21c8178b 343 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 8:b79d21c8178b 344 MD_SET_PWM (MD_Data,6,0);
Tom0108 1:199c4a71da88 345
Tom0108 8:b79d21c8178b 346 kago=0;
Tom0108 8:b79d21c8178b 347 pull=0;
Tom0108 8:b79d21c8178b 348 grab=0;
Tom0108 0:761a63c6d020 349
Tom0108 8:b79d21c8178b 350 tim.reset();
Tom0108 0:761a63c6d020 351 }
Tom0108 8:b79d21c8178b 352 }
Tom0108 8:b79d21c8178b 353 //非常停止
Tom0108 8:b79d21c8178b 354 else {
Tom0108 8:b79d21c8178b 355 for(int i=0; i<8; i++) {
Tom0108 8:b79d21c8178b 356 MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
Tom0108 8:b79d21c8178b 357 MD_SET_PWM (MD_Data,i,0);
Tom0108 8:b79d21c8178b 358 }
Tom0108 0:761a63c6d020 359 }
Tom0108 0:761a63c6d020 360
Tom0108 3:1063aa133b62 361 //-------------- 出力 --------------//
Tom0108 3:1063aa133b62 362 //第二引数には使う最大の個数
Tom0108 8:b79d21c8178b 363 i2c.Out(Out_Data,8);
Tom0108 3:1063aa133b62 364
Tom0108 3:1063aa133b62 365 //モータ出力
Tom0108 3:1063aa133b62 366 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
Tom0108 3:1063aa133b62 367
Tom0108 3:1063aa133b62 368 /* ----------- りみっと --------------- */
Tom0108 3:1063aa133b62 369 limit_up = (In_Data+4)->in_data;
Tom0108 3:1063aa133b62 370 limit_down = (In_Data+5)->in_data;
Tom0108 3:1063aa133b62 371 max_lim = (In_Data+1)->in_data;
Tom0108 3:1063aa133b62 372 start_lim =(In_Data+2)->in_data;
Tom0108 3:1063aa133b62 373 short_lim = (In_Data+6)->in_data;
Tom0108 3:1063aa133b62 374 slide_start = (In_Data+3)->in_data;
Tom0108 3:1063aa133b62 375 slide_stop = (In_Data+0)->in_data;
Tom0108 3:1063aa133b62 376
Tom0108 3:1063aa133b62 377 //半自動のデバック
Tom0108 3:1063aa133b62 378 // pc.printf("pull %2d%2d ",pull,pull_f);
Tom0108 3:1063aa133b62 379 // pc.printf("grab %2d%2d ",grab,grab_f);
Tom0108 3:1063aa133b62 380 // pc.printf("kago %2d%2d ",kago,kago_f);
Tom0108 6:040d001acb12 381 // pc.printf("yaw:%.1f dif:%.1f",yaw,dif_val);
Tom0108 6:040d001acb12 382 pc.printf("0:%.1f 1:%.1f yaw:%.1f dif:%.1f",y_deg[0],y_deg[1],yaw,dif_val);
Tom0108 4:236e5c58e8ee 383 // pc.printf("%2d ",dir);
Tom0108 3:1063aa133b62 384 pc.printf("\n\r");
Tom0108 3:1063aa133b62 385
Tom0108 3:1063aa133b62 386
Tom0108 3:1063aa133b62 387 /* --------------電磁弁系-------------------- */
Tom0108 0:761a63c6d020 388 /*タオル掛け*/
Tom0108 0:761a63c6d020 389 if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
Tom0108 0:761a63c6d020 390 else i2c.Out_Set(Out_Data,2,0);
Tom0108 3:1063aa133b62 391
Tom0108 0:761a63c6d020 392 /*タオルを引っ張るやつ*/
Tom0108 1:199c4a71da88 393 if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
Tom0108 1:199c4a71da88 394 else i2c.Out_Set(Out_Data,4,0);
Tom0108 0:761a63c6d020 395
Tom0108 3:1063aa133b62 396 if(unfold_f)i2c.Out_Set(Out_Data,3,1);
Tom0108 3:1063aa133b62 397 else i2c.Out_Set(Out_Data,3,0);
Tom0108 0:761a63c6d020 398
Tom0108 0:761a63c6d020 399
Tom0108 3:1063aa133b62 400 /* ----------------ボタン系--------------- */
Tom0108 6:040d001acb12 401 //角度リセット
Tom0108 2:47954f05d32d 402 if(sbdbt.L1) {
Tom0108 8:b79d21c8178b 403 yaw=def_val;
Tom0108 8:b79d21c8178b 404 Turn_val=def_val;
Tom0108 2:47954f05d32d 405 }
Tom0108 3:1063aa133b62 406 //かごを倒す
Tom0108 6:040d001acb12 407 if(CROSS) {
Tom0108 1:199c4a71da88 408 if(once==0) {
Tom0108 3:1063aa133b62 409 kago_f++;
Tom0108 3:1063aa133b62 410 kago_f%=2;
Tom0108 1:199c4a71da88 411 once++;
Tom0108 1:199c4a71da88 412 }
Tom0108 3:1063aa133b62 413 }
Tom0108 3:1063aa133b62 414 //タオルを掴んで動かす
Tom0108 3:1063aa133b62 415 else if(SQUARE) {
Tom0108 1:199c4a71da88 416 if(once==0) {
Tom0108 3:1063aa133b62 417 grab_f++;
Tom0108 3:1063aa133b62 418 grab_f%=2;
Tom0108 1:199c4a71da88 419 once++;
Tom0108 1:199c4a71da88 420 }
Tom0108 4:236e5c58e8ee 421 } else if(TRIANGLE) {
Tom0108 4:236e5c58e8ee 422 if(once==0) {
Tom0108 8:b79d21c8178b 423 spull_f++;
Tom0108 8:b79d21c8178b 424 spull_f%=2;
Tom0108 4:236e5c58e8ee 425 once++;
Tom0108 4:236e5c58e8ee 426 }
Tom0108 3:1063aa133b62 427 }
Tom0108 3:1063aa133b62 428 //倒すを引いて動かす
Tom0108 3:1063aa133b62 429 else if(CIRCLE) {
Tom0108 1:199c4a71da88 430 if(once==0) {
Tom0108 3:1063aa133b62 431 pull_f++;
Tom0108 3:1063aa133b62 432 pull_f%=2;
Tom0108 3:1063aa133b62 433 once++;
Tom0108 3:1063aa133b62 434 }
Tom0108 3:1063aa133b62 435 }
Tom0108 3:1063aa133b62 436 //タオル展開用
Tom0108 3:1063aa133b62 437 else if(sbdbt.START) {
Tom0108 3:1063aa133b62 438 if(once==0) {
Tom0108 3:1063aa133b62 439 unfold_f++;
Tom0108 3:1063aa133b62 440 unfold_f%=2;
Tom0108 1:199c4a71da88 441 once++;
Tom0108 1:199c4a71da88 442 }
Tom0108 1:199c4a71da88 443 } else once=0;
Tom0108 1:199c4a71da88 444
Tom0108 1:199c4a71da88 445
Tom0108 0:761a63c6d020 446 } // if(sbdbt.State_check())
Tom0108 0:761a63c6d020 447 } // while(1)
Tom0108 0:761a63c6d020 448 } // int main()
Tom0108 0:761a63c6d020 449
Tom0108 0:761a63c6d020 450 /* メカナムの基本移動 */
Tom0108 6:040d001acb12 451 void mecanum_Move(int lx, int ly, int rx)
Tom0108 0:761a63c6d020 452 {
Tom0108 6:040d001acb12 453 duty[0]=lx+ly;
Tom0108 6:040d001acb12 454 duty[1]=-(-lx+ly);
Tom0108 6:040d001acb12 455 duty[2]=-lx+ly;
Tom0108 6:040d001acb12 456 duty[3]=-(lx+ly);
Tom0108 0:761a63c6d020 457
Tom0108 0:761a63c6d020 458 for(int i=0; i<4; i++) {
Tom0108 8:b79d21c8178b 459 //旋回
Tom0108 6:040d001acb12 460 duty[i]+=rx;
Tom0108 8:b79d21c8178b 461
Tom0108 8:b79d21c8178b 462 //制限
Tom0108 8:b79d21c8178b 463 if(duty[i]>=99) duty[i]=99;
Tom0108 8:b79d21c8178b 464 else if(duty[i]<=-99) duty[i]=-99;
Tom0108 6:040d001acb12 465 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 466 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 467 }
Tom0108 0:761a63c6d020 468 }
Tom0108 0:761a63c6d020 469
Tom0108 6:040d001acb12 470 void AngleCorrection(double n_angle, double t_angle)
Tom0108 0:761a63c6d020 471 {
Tom0108 6:040d001acb12 472 double dif=-(t_angle-n_angle);
Tom0108 6:040d001acb12 473
Tom0108 6:040d001acb12 474 if(dif>=30) dif=30;
Tom0108 6:040d001acb12 475 else if(dif<=-30) dif=-30;
Tom0108 6:040d001acb12 476
Tom0108 0:761a63c6d020 477 for(int i=0; i<4; i++) {
Tom0108 6:040d001acb12 478 duty[i]+=dif;
Tom0108 6:040d001acb12 479 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 480 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 481 }
Tom0108 6:040d001acb12 482
Tom0108 0:761a63c6d020 483 }
Tom0108 0:761a63c6d020 484
Tom0108 0:761a63c6d020 485 /*操作法*/
Tom0108 0:761a63c6d020 486 /*
Tom0108 0:761a63c6d020 487 〇 ボタン == 回収（引きずる）
Tom0108 0:761a63c6d020 488 ? ボタン == 籠
Tom0108 0:761a63c6d020 489 △ ボタン == 回収（シーツ兼シャツ）
Tom0108 0:761a63c6d020 490 □ ボタン == 回収（タオル）
Tom0108 0:761a63c6d020 491 上 ボタン == ハンガーかけ機構 上昇
Tom0108 0:761a63c6d020 492 下 ボタン == ハンガーかけ機構 下降
Tom0108 0:761a63c6d020 493 L1 ボタン == リセット（足回り）
Tom0108 0:761a63c6d020 494 L2 ボタン == タオル掛け
Tom0108 6:040d001acb12 495 R2 ボタン == タオル真ん中掴む
Tom0108 0:761a63c6d020 496 select == ハンガー掛け 最大まで上昇
Tom0108 0:761a63c6d020 497 start == ハンガー展開
Tom0108 0:761a63c6d020 498 */
Tom0108 0:761a63c6d020 499 /*その他注意点*/
Tom0108 0:761a63c6d020 500 /*
Tom0108 6:040d001acb12 501 動かすときはジャイロの値を安定させるためLEDが赤から緑になるまで待つ
Tom0108 0:761a63c6d020 502 メカナムは振動が大きいせいかナット等が外れやすいため、定期的に確認をする（特に足回り）
Tom0108 0:761a63c6d020 503 あまり急発進をさせない（たまに暴走）
Tom0108 0:761a63c6d020 504 暴走したときはコントローラーを動かしたら治るはず
Tom0108 0:761a63c6d020 505 コントローラー接続が切れたらSTMの黒い奴を押す（わからないときは近くの回路班へ）
Tom0108 0:761a63c6d020 506 */