Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@15:68720ff6bbc9, 2019-09-23 (annotated)

Committer:
Tom0108
Date:
Mon Sep 23 06:10:11 2019 +0000
Revision:
15:68720ff6bbc9
Parent:
14:e6d16095ffa8
Child:
16:829b953d1ac1
09/23

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 14:e6d16095ffa8 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 14:e6d16095ffa8 71 yaw=def_val;
Tom0108 14:e6d16095ffa8 72
Tom0108 14:e6d16095ffa8 73 //補正角度
Tom0108 14:e6d16095ffa8 74 Turn_val=def_val;
Tom0108 14:e6d16095ffa8 75
Tom0108 14:e6d16095ffa8 76 sbdbt.LX=64;
Tom0108 14:e6d16095ffa8 77 sbdbt.LY=64;
Tom0108 14:e6d16095ffa8 78 sbdbt.RX=64;
Tom0108 14:e6d16095ffa8 79 sbdbt.RY=64;
Tom0108 11:c1a22e2e3534 80 //ジャイロリセット
Tom0108 10:7c73e1577742 81 bno.reset();
Tom0108 5:fcc79e507610 82
Tom0108 0:761a63c6d020 83 while(1) {
Tom0108 1:199c4a71da88 84 //自動系のタイマースタート
Tom0108 1:199c4a71da88 85 tim.start();
Tom0108 6:040d001acb12 86 drift_tim.start();
Tom0108 12:7e33e400a48d 87 keep_tim[0].start();
Tom0108 12:7e33e400a48d 88 keep_tim[1].start();
Tom0108 0:761a63c6d020 89
Tom0108 11:c1a22e2e3534 90 bno.setmode(OPERATION_MODE_IMUPLUS);
Tom0108 10:7c73e1577742 91 //角度の取得
Tom0108 10:7c73e1577742 92 bno.get_angles();
Tom0108 10:7c73e1577742 93 rawyaw[1]=rawyaw[0];
Tom0108 10:7c73e1577742 94 rawyaw[0]=bno.euler.yaw;
Tom0108 11:c1a22e2e3534 95
Tom0108 5:fcc79e507610 96 //180～-180をまたいだ時
Tom0108 10:7c73e1577742 97 if(rawyaw[1]<90 && rawyaw[0]>270) rawyaw[1]+=360;
Tom0108 10:7c73e1577742 98 else if(rawyaw[1]>270 && rawyaw[0]<90) rawyaw[1]-=360;
Tom0108 11:c1a22e2e3534 99
Tom0108 10:7c73e1577742 100 yaw-=rawyaw[0]-rawyaw[1];
Tom0108 14:e6d16095ffa8 101 yaw=fmod(yaw, 360.0);
Tom0108 0:761a63c6d020 102
Tom0108 12:7e33e400a48d 103 //ゾーン対応
Tom0108 14:e6d16095ffa8 104 if(zone==0) PALETTE(RED);
Tom0108 14:e6d16095ffa8 105 else PALETTE(BLUE);
Tom0108 14:e6d16095ffa8 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 14:e6d16095ffa8 114 //R1で停止
Tom0108 14:e6d16095ffa8 115 if(1) {
Tom0108 14:e6d16095ffa8 116 //メカナム基本動作
Tom0108 14:e6d16095ffa8 117 mecanum_Move(lx, ly, rx);
Tom0108 14:e6d16095ffa8 118
Tom0108 14:e6d16095ffa8 119 //旋回している間タイマーをリセット
Tom0108 14:e6d16095ffa8 120 if(sbdbt.RX != 64) drift_tim.reset();
Tom0108 14:e6d16095ffa8 121
Tom0108 14:e6d16095ffa8 122 //旋回して慣性で動いた後の角度に補正する
Tom0108 14:e6d16095ffa8 123 if(drift_tim.read_ms()<1000) Turn_val=yaw;
Tom0108 14:e6d16095ffa8 124
Tom0108 14:e6d16095ffa8 125 //角度補正
Tom0108 14:e6d16095ffa8 126 AngleCorrection(yaw, Turn_val);
Tom0108 14:e6d16095ffa8 127
Tom0108 14:e6d16095ffa8 128 /*---ハンガーかけるやつ----------------------------------------------*/
Tom0108 14:e6d16095ffa8 129 //上方向
Tom0108 14:e6d16095ffa8 130 if(!DOWNkey && ((UPkey || keep_f[0]) && limit_up == 0)) {
Tom0108 14:e6d16095ffa8 131 //一定秒以上押されたら保持
Tom0108 14:e6d16095ffa8 132 if(keep_tim[0].read_ms()>500) keep_f[0]=1;
Tom0108 14:e6d16095ffa8 133 //下方向の保持を解除
Tom0108 14:e6d16095ffa8 134 keep_f[1]=0;
Tom0108 14:e6d16095ffa8 135 keep_tim[1].reset();
Tom0108 12:7e33e400a48d 136
Tom0108 14:e6d16095ffa8 137 MD_SET_DRIVE(MD_Data, 5,MD_FORWARD);
Tom0108 14:e6d16095ffa8 138 MD_SET_PWM(MD_Data, 5,100);
Tom0108 14:e6d16095ffa8 139 }
Tom0108 14:e6d16095ffa8 140 //下方向
Tom0108 14:e6d16095ffa8 141 else if(!UPkey && ((DOWNkey || keep_f[1]) && limit_down == 0)) {
Tom0108 14:e6d16095ffa8 142 if(keep_tim[1].read_ms()>500) keep_f[1]=1;
Tom0108 14:e6d16095ffa8 143 keep_f[0]=0;
Tom0108 14:e6d16095ffa8 144 keep_tim[0].reset();
Tom0108 14:e6d16095ffa8 145
Tom0108 14:e6d16095ffa8 146 MD_SET_DRIVE(MD_Data, 5,MD_REVERSE);
Tom0108 14:e6d16095ffa8 147 MD_SET_PWM(MD_Data, 5,100);
Tom0108 14:e6d16095ffa8 148 } else {
Tom0108 14:e6d16095ffa8 149 keep_f[0]=0;
Tom0108 14:e6d16095ffa8 150 keep_tim[0].reset();
Tom0108 14:e6d16095ffa8 151 keep_f[1]=0;
Tom0108 14:e6d16095ffa8 152 keep_tim[1].reset();
Tom0108 14:e6d16095ffa8 153
Tom0108 14:e6d16095ffa8 154 MD_SET_PWM(MD_Data,5,0);
Tom0108 14:e6d16095ffa8 155 MD_SET_DRIVE(MD_Data, 5,MD_BRAKE);
Tom0108 14:e6d16095ffa8 156 }
Tom0108 6:040d001acb12 157
Tom0108 14:e6d16095ffa8 158 //タオルつかむ 四角
Tom0108 14:e6d16095ffa8 159 if(grab_f) {
Tom0108 14:e6d16095ffa8 160 //真ん中で止まる用
Tom0108 14:e6d16095ffa8 161 short_f=1;
Tom0108 14:e6d16095ffa8 162 switch(grab) {
Tom0108 14:e6d16095ffa8 163 //腕を前に動かす
Tom0108 14:e6d16095ffa8 164 case 0:
Tom0108 14:e6d16095ffa8 165 //爪開く
Tom0108 14:e6d16095ffa8 166 i2c.Out_Set(Out_Data,1,1);
Tom0108 14:e6d16095ffa8 167 //腕上げる
Tom0108 14:e6d16095ffa8 168 i2c.Out_Set(Out_Data,0,0);
Tom0108 14:e6d16095ffa8 169
Tom0108 14:e6d16095ffa8 170 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 14:e6d16095ffa8 171 MD_SET_PWM (MD_Data,4,60);
Tom0108 14:e6d16095ffa8 172
Tom0108 14:e6d16095ffa8 173 if(max_lim==1) {
Tom0108 14:e6d16095ffa8 174 grab++;
Tom0108 14:e6d16095ffa8 175 tim.reset();
Tom0108 14:e6d16095ffa8 176 }
Tom0108 14:e6d16095ffa8 177 break;
Tom0108 8:b79d21c8178b 178
Tom0108 14:e6d16095ffa8 179 case 1:
Tom0108 14:e6d16095ffa8 180 //腕停止
Tom0108 14:e6d16095ffa8 181 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 14:e6d16095ffa8 182 MD_SET_PWM(MD_Data,4,0);
Tom0108 14:e6d16095ffa8 183
Tom0108 14:e6d16095ffa8 184 //腕おろす
Tom0108 14:e6d16095ffa8 185 i2c.Out_Set(Out_Data,0,1);
Tom0108 14:e6d16095ffa8 186 if(tim.read_ms()>200) {
Tom0108 14:e6d16095ffa8 187 grab++;
Tom0108 14:e6d16095ffa8 188 tim.reset();
Tom0108 14:e6d16095ffa8 189 }
Tom0108 14:e6d16095ffa8 190 break;
Tom0108 14:e6d16095ffa8 191
Tom0108 14:e6d16095ffa8 192 case 2:
Tom0108 14:e6d16095ffa8 193 //爪閉じる
Tom0108 14:e6d16095ffa8 194 i2c.Out_Set(Out_Data,1,0);
Tom0108 14:e6d16095ffa8 195 if(tim.read_ms()>0) {
Tom0108 14:e6d16095ffa8 196 grab++;
Tom0108 14:e6d16095ffa8 197 tim.reset();
Tom0108 14:e6d16095ffa8 198 }
Tom0108 14:e6d16095ffa8 199 break;
Tom0108 14:e6d16095ffa8 200
Tom0108 14:e6d16095ffa8 201 case 3:
Tom0108 14:e6d16095ffa8 202 //腕上げる
Tom0108 14:e6d16095ffa8 203 i2c.Out_Set(Out_Data,0,0);
Tom0108 14:e6d16095ffa8 204 if(tim.read_ms()>0) {
Tom0108 14:e6d16095ffa8 205 grab++;
Tom0108 14:e6d16095ffa8 206 tim.reset();
Tom0108 14:e6d16095ffa8 207 }
Tom0108 14:e6d16095ffa8 208 break;
Tom0108 0:761a63c6d020 209
Tom0108 14:e6d16095ffa8 210 //腕戻す
Tom0108 14:e6d16095ffa8 211 case 4:
Tom0108 14:e6d16095ffa8 212 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 14:e6d16095ffa8 213 MD_SET_PWM (MD_Data,4,60);
Tom0108 14:e6d16095ffa8 214 if(start_lim==1) {
Tom0108 14:e6d16095ffa8 215 grab_f=0;
Tom0108 14:e6d16095ffa8 216 tim.reset();
Tom0108 14:e6d16095ffa8 217 }
Tom0108 14:e6d16095ffa8 218 break;
Tom0108 14:e6d16095ffa8 219 }
Tom0108 14:e6d16095ffa8 220 } //if(grab_f)
Tom0108 14:e6d16095ffa8 221
Tom0108 14:e6d16095ffa8 222 //シーツ用
Tom0108 14:e6d16095ffa8 223 else if(spull_f) {
Tom0108 14:e6d16095ffa8 224 short_f=1;
Tom0108 14:e6d16095ffa8 225 switch(spull) {
Tom0108 14:e6d16095ffa8 226 //腕を前に動かす
Tom0108 14:e6d16095ffa8 227 case 0:
Tom0108 14:e6d16095ffa8 228 //爪閉じる
Tom0108 14:e6d16095ffa8 229 i2c.Out_Set(Out_Data,1,0);
Tom0108 14:e6d16095ffa8 230 //腕上げる
Tom0108 14:e6d16095ffa8 231 i2c.Out_Set(Out_Data,0,0);
Tom0108 8:b79d21c8178b 232
Tom0108 14:e6d16095ffa8 233 //真ん中のリミットより前に腕があった場合にバックする
Tom0108 14:e6d16095ffa8 234 if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 14:e6d16095ffa8 235 else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 14:e6d16095ffa8 236 MD_SET_PWM (MD_Data,4,30);
Tom0108 14:e6d16095ffa8 237
Tom0108 14:e6d16095ffa8 238 if(short_lim==1) {
Tom0108 14:e6d16095ffa8 239 spull++;
Tom0108 14:e6d16095ffa8 240 tim.reset();
Tom0108 14:e6d16095ffa8 241 } else if(max_lim) back_f=1;
Tom0108 14:e6d16095ffa8 242 break;
Tom0108 0:761a63c6d020 243
Tom0108 14:e6d16095ffa8 244 case 1:
Tom0108 14:e6d16095ffa8 245 //腕停止
Tom0108 14:e6d16095ffa8 246 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 14:e6d16095ffa8 247 MD_SET_PWM(MD_Data,4,0);
Tom0108 14:e6d16095ffa8 248
Tom0108 14:e6d16095ffa8 249 //腕おろす
Tom0108 14:e6d16095ffa8 250 i2c.Out_Set(Out_Data,0,1);
Tom0108 14:e6d16095ffa8 251 if(tim.read_ms()>200) {
Tom0108 14:e6d16095ffa8 252 spull++;
Tom0108 14:e6d16095ffa8 253 tim.reset();
Tom0108 14:e6d16095ffa8 254 }
Tom0108 14:e6d16095ffa8 255 break;
Tom0108 8:b79d21c8178b 256
Tom0108 14:e6d16095ffa8 257 case 2:
Tom0108 14:e6d16095ffa8 258 //爪閉じる
Tom0108 14:e6d16095ffa8 259 i2c.Out_Set(Out_Data,1,0);
Tom0108 14:e6d16095ffa8 260 if(tim.read_ms()>0) {
Tom0108 14:e6d16095ffa8 261 spull++;
Tom0108 14:e6d16095ffa8 262 tim.reset();
Tom0108 14:e6d16095ffa8 263 }
Tom0108 14:e6d16095ffa8 264 break;
Tom0108 12:7e33e400a48d 265
Tom0108 14:e6d16095ffa8 266 case 3:
Tom0108 14:e6d16095ffa8 267 //腕上げる
Tom0108 14:e6d16095ffa8 268 // i2c.Out_Set(Out_Data,0,0);
Tom0108 14:e6d16095ffa8 269 if(tim.read_ms()>0) {
Tom0108 14:e6d16095ffa8 270 spull++;
Tom0108 14:e6d16095ffa8 271 tim.reset();
Tom0108 14:e6d16095ffa8 272 }
Tom0108 14:e6d16095ffa8 273 break;
Tom0108 14:e6d16095ffa8 274
Tom0108 14:e6d16095ffa8 275 //腕戻す
Tom0108 14:e6d16095ffa8 276 case 4:
Tom0108 14:e6d16095ffa8 277 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 14:e6d16095ffa8 278 MD_SET_PWM (MD_Data,4,60);
Tom0108 14:e6d16095ffa8 279 if(start_lim==1) {
Tom0108 14:e6d16095ffa8 280 spull_f=0;
Tom0108 14:e6d16095ffa8 281 tim.reset();
Tom0108 14:e6d16095ffa8 282 }
Tom0108 14:e6d16095ffa8 283 break;
Tom0108 14:e6d16095ffa8 284
Tom0108 12:7e33e400a48d 285 }
Tom0108 14:e6d16095ffa8 286 } //if(spull_f)
Tom0108 14:e6d16095ffa8 287
Tom0108 14:e6d16095ffa8 288 //タオル引く 丸
Tom0108 14:e6d16095ffa8 289 else if(pull_f) {
Tom0108 14:e6d16095ffa8 290 short_f=1;
Tom0108 14:e6d16095ffa8 291 switch(pull) {
Tom0108 14:e6d16095ffa8 292 //腕を前に動かす
Tom0108 14:e6d16095ffa8 293 case 0:
Tom0108 14:e6d16095ffa8 294 //爪閉じる
Tom0108 14:e6d16095ffa8 295 i2c.Out_Set(Out_Data,1,0);
Tom0108 14:e6d16095ffa8 296 //腕上げる
Tom0108 14:e6d16095ffa8 297 i2c.Out_Set(Out_Data,0,0);
Tom0108 14:e6d16095ffa8 298
Tom0108 14:e6d16095ffa8 299 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 14:e6d16095ffa8 300 MD_SET_PWM (MD_Data,4,60);
Tom0108 14:e6d16095ffa8 301
Tom0108 14:e6d16095ffa8 302 if(max_lim==1) {
Tom0108 14:e6d16095ffa8 303 pull++;
Tom0108 14:e6d16095ffa8 304 tim.reset();
Tom0108 14:e6d16095ffa8 305 }
Tom0108 14:e6d16095ffa8 306 break;
Tom0108 14:e6d16095ffa8 307
Tom0108 14:e6d16095ffa8 308 case 1:
Tom0108 14:e6d16095ffa8 309 //腕停止
Tom0108 14:e6d16095ffa8 310 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 14:e6d16095ffa8 311 MD_SET_PWM (MD_Data,4,0);
Tom0108 14:e6d16095ffa8 312
Tom0108 14:e6d16095ffa8 313 //腕おろす
Tom0108 14:e6d16095ffa8 314 i2c.Out_Set(Out_Data,0,1);
Tom0108 14:e6d16095ffa8 315 if(tim.read_ms()>100) {
Tom0108 14:e6d16095ffa8 316 pull++;
Tom0108 14:e6d16095ffa8 317 tim.reset();
Tom0108 14:e6d16095ffa8 318 }
Tom0108 14:e6d16095ffa8 319 break;
Tom0108 14:e6d16095ffa8 320
Tom0108 14:e6d16095ffa8 321 //停止
Tom0108 14:e6d16095ffa8 322 case 2:
Tom0108 14:e6d16095ffa8 323 if(tim.read_ms()>0) {
Tom0108 14:e6d16095ffa8 324 pull++;
Tom0108 14:e6d16095ffa8 325 tim.reset();
Tom0108 14:e6d16095ffa8 326 }
Tom0108 14:e6d16095ffa8 327 break;
Tom0108 14:e6d16095ffa8 328
Tom0108 14:e6d16095ffa8 329 case 3:
Tom0108 14:e6d16095ffa8 330 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 14:e6d16095ffa8 331 MD_SET_PWM (MD_Data,4,60);
Tom0108 14:e6d16095ffa8 332 if(start_lim==1) {
Tom0108 14:e6d16095ffa8 333 pull_f=0;
Tom0108 14:e6d16095ffa8 334 tim.reset();
Tom0108 14:e6d16095ffa8 335 }
Tom0108 14:e6d16095ffa8 336 break;
Tom0108 14:e6d16095ffa8 337 }
Tom0108 14:e6d16095ffa8 338 } //if(pull_f)
Tom0108 14:e6d16095ffa8 339
Tom0108 14:e6d16095ffa8 340 //かごを倒す
Tom0108 14:e6d16095ffa8 341 else if(kago_f) {
Tom0108 14:e6d16095ffa8 342 // PALETTE(MAGENTA);
Tom0108 14:e6d16095ffa8 343 short_f=1;
Tom0108 12:7e33e400a48d 344
Tom0108 14:e6d16095ffa8 345 switch(kago) {
Tom0108 14:e6d16095ffa8 346 //腕を前に動かす
Tom0108 14:e6d16095ffa8 347 case 0:
Tom0108 14:e6d16095ffa8 348 //爪閉じる
Tom0108 14:e6d16095ffa8 349 i2c.Out_Set(Out_Data,1,0);
Tom0108 14:e6d16095ffa8 350 //腕上げる
Tom0108 14:e6d16095ffa8 351 i2c.Out_Set(Out_Data,0,0);
Tom0108 14:e6d16095ffa8 352
Tom0108 14:e6d16095ffa8 353 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 14:e6d16095ffa8 354 MD_SET_PWM (MD_Data,4,60);
Tom0108 14:e6d16095ffa8 355 if(max_lim==1) {
Tom0108 14:e6d16095ffa8 356 kago++;
Tom0108 14:e6d16095ffa8 357 tim.reset();
Tom0108 14:e6d16095ffa8 358 }
Tom0108 14:e6d16095ffa8 359 break;
Tom0108 14:e6d16095ffa8 360
Tom0108 14:e6d16095ffa8 361 case 1:
Tom0108 14:e6d16095ffa8 362 //腕停止
Tom0108 14:e6d16095ffa8 363 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 14:e6d16095ffa8 364 MD_SET_PWM (MD_Data,4,0);
Tom0108 14:e6d16095ffa8 365
Tom0108 14:e6d16095ffa8 366 //かご回転
Tom0108 14:e6d16095ffa8 367 MD_SET_DRIVE(MD_Data,6,MD_FORWARD);
Tom0108 14:e6d16095ffa8 368 MD_SET_PWM (MD_Data,6,100);
Tom0108 14:e6d16095ffa8 369 if(slide_stop==1) {
Tom0108 14:e6d16095ffa8 370 kago++;
Tom0108 14:e6d16095ffa8 371 tim.reset();
Tom0108 14:e6d16095ffa8 372 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 14:e6d16095ffa8 373 MD_SET_PWM (MD_Data,6,0);
Tom0108 14:e6d16095ffa8 374 }
Tom0108 14:e6d16095ffa8 375 break;
Tom0108 12:7e33e400a48d 376
Tom0108 14:e6d16095ffa8 377 case 2:
Tom0108 14:e6d16095ffa8 378 if(tim.read_ms()>0) {
Tom0108 14:e6d16095ffa8 379 kago++;
Tom0108 14:e6d16095ffa8 380 tim.reset();
Tom0108 14:e6d16095ffa8 381 }
Tom0108 14:e6d16095ffa8 382 break;
Tom0108 14:e6d16095ffa8 383
Tom0108 14:e6d16095ffa8 384 case 3:
Tom0108 14:e6d16095ffa8 385 MD_SET_DRIVE(MD_Data,6,MD_REVERSE);
Tom0108 14:e6d16095ffa8 386 MD_SET_PWM (MD_Data,6,100);
Tom0108 14:e6d16095ffa8 387 if(slide_start==1) kago++;
Tom0108 14:e6d16095ffa8 388 break;
Tom0108 14:e6d16095ffa8 389
Tom0108 14:e6d16095ffa8 390 case 4:
Tom0108 14:e6d16095ffa8 391 MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 14:e6d16095ffa8 392 MD_SET_PWM (MD_Data,4,60);
Tom0108 14:e6d16095ffa8 393 if(start_lim==1) {
Tom0108 14:e6d16095ffa8 394 kago_f=0;
Tom0108 14:e6d16095ffa8 395 tim.reset();
Tom0108 14:e6d16095ffa8 396 }
Tom0108 14:e6d16095ffa8 397 break;
Tom0108 8:b79d21c8178b 398 }
Tom0108 14:e6d16095ffa8 399 } else {
Tom0108 14:e6d16095ffa8 400 if(sbdbt.R1) {
Tom0108 14:e6d16095ffa8 401 //腕おろす
Tom0108 14:e6d16095ffa8 402 i2c.Out_Set(Out_Data,0,1);
Tom0108 14:e6d16095ffa8 403 //爪閉じる
Tom0108 14:e6d16095ffa8 404 i2c.Out_Set(Out_Data,1,0);
Tom0108 14:e6d16095ffa8 405 }
Tom0108 14:e6d16095ffa8 406 //腕あげる
Tom0108 14:e6d16095ffa8 407 else i2c.Out_Set(Out_Data,0,0);
Tom0108 6:040d001acb12 408
Tom0108 14:e6d16095ffa8 409 if(disturb_f) {
Tom0108 10:7c73e1577742 410 //真ん中で止まる用
Tom0108 8:b79d21c8178b 411 short_f=1;
Tom0108 14:e6d16095ffa8 412 switch(disturb) {
Tom0108 8:b79d21c8178b 413 //腕を前に動かす
Tom0108 8:b79d21c8178b 414 case 0:
Tom0108 8:b79d21c8178b 415 //爪開く
Tom0108 8:b79d21c8178b 416 i2c.Out_Set(Out_Data,1,1);
Tom0108 13:48c4a4c95e77 417 //腕上げる
Tom0108 13:48c4a4c95e77 418 i2c.Out_Set(Out_Data,0,0);
Tom0108 0:761a63c6d020 419
Tom0108 10:7c73e1577742 420 MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 9:64fcbcc182d9 421 MD_SET_PWM (MD_Data,4,60);
Tom0108 0:761a63c6d020 422
Tom0108 10:7c73e1577742 423 if(max_lim==1) {
Tom0108 14:e6d16095ffa8 424 disturb++;
Tom0108 10:7c73e1577742 425 tim.reset();
Tom0108 8:b79d21c8178b 426 }
Tom0108 8:b79d21c8178b 427 break;
Tom0108 8:b79d21c8178b 428
Tom0108 8:b79d21c8178b 429 case 1:
Tom0108 8:b79d21c8178b 430 //腕停止
Tom0108 8:b79d21c8178b 431 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 8:b79d21c8178b 432 MD_SET_PWM(MD_Data,4,0);
Tom0108 8:b79d21c8178b 433
Tom0108 8:b79d21c8178b 434 //腕おろす
Tom0108 8:b79d21c8178b 435 i2c.Out_Set(Out_Data,0,1);
Tom0108 9:64fcbcc182d9 436 if(tim.read_ms()>200) {
Tom0108 14:e6d16095ffa8 437 disturb++;
Tom0108 10:7c73e1577742 438 tim.reset();
Tom0108 10:7c73e1577742 439 }
Tom0108 10:7c73e1577742 440 break;
Tom0108 10:7c73e1577742 441
Tom0108 10:7c73e1577742 442 case 2:
Tom0108 10:7c73e1577742 443 //爪閉じる
Tom0108 10:7c73e1577742 444 i2c.Out_Set(Out_Data,1,0);
Tom0108 10:7c73e1577742 445 if(tim.read_ms()>0) {
Tom0108 14:e6d16095ffa8 446 disturb++;
Tom0108 10:7c73e1577742 447 tim.reset();
Tom0108 10:7c73e1577742 448 }
Tom0108 10:7c73e1577742 449 break;
Tom0108 10:7c73e1577742 450
Tom0108 10:7c73e1577742 451 case 3:
Tom0108 10:7c73e1577742 452 //腕上げる
Tom0108 13:48c4a4c95e77 453 i2c.Out_Set(Out_Data,0,0);
Tom0108 8:b79d21c8178b 454
Tom0108 14:e6d16095ffa8 455 //赤ゾーン
Tom0108 14:e6d16095ffa8 456 if( yaw<=90 || 270<yaw ) {
Tom0108 14:e6d16095ffa8 457 MD_SET_PWM(MD_Data,4,20);
Tom0108 14:e6d16095ffa8 458 if(RIGHTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 14:e6d16095ffa8 459 else if(LEFTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 14:e6d16095ffa8 460 else {
Tom0108 14:e6d16095ffa8 461 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 14:e6d16095ffa8 462 MD_SET_PWM (MD_Data,4,0);
Tom0108 14:e6d16095ffa8 463 }
Tom0108 8:b79d21c8178b 464 }
Tom0108 14:e6d16095ffa8 465 //青ゾーン
Tom0108 14:e6d16095ffa8 466 else if( 90<yaw && yaw<=270 ) {
Tom0108 14:e6d16095ffa8 467 MD_SET_PWM(MD_Data,4,20);
Tom0108 14:e6d16095ffa8 468 if(LEFTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 14:e6d16095ffa8 469 else if(RIGHTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 14:e6d16095ffa8 470 else {
Tom0108 14:e6d16095ffa8 471 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 14:e6d16095ffa8 472 MD_SET_PWM (MD_Data,4,0);
Tom0108 14:e6d16095ffa8 473 }
Tom0108 8:b79d21c8178b 474 }
Tom0108 8:b79d21c8178b 475 }
Tom0108 14:e6d16095ffa8 476 } //if(disturb_f)
Tom0108 1:199c4a71da88 477
Tom0108 14:e6d16095ffa8 478 //腕真ん中で停止
Tom0108 14:e6d16095ffa8 479 else if(short_f) {
Tom0108 14:e6d16095ffa8 480 //爪開く
Tom0108 14:e6d16095ffa8 481 i2c.Out_Set(Out_Data,1,1);
Tom0108 15:68720ff6bbc9 482
Tom0108 14:e6d16095ffa8 483 if(short_lim==1) {
Tom0108 14:e6d16095ffa8 484 short_f=0;
Tom0108 14:e6d16095ffa8 485 back_f=0;
Tom0108 14:e6d16095ffa8 486 }
Tom0108 14:e6d16095ffa8 487 //奥のリミットに当たったら逆回転
Tom0108 14:e6d16095ffa8 488 if(max_lim==1) back_f=1;
Tom0108 14:e6d16095ffa8 489 //手前のリミットに当たったら正回転
Tom0108 14:e6d16095ffa8 490 else if(start_lim==1) back_f=0;
Tom0108 8:b79d21c8178b 491
Tom0108 14:e6d16095ffa8 492 if(back_f) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 14:e6d16095ffa8 493 else MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 14:e6d16095ffa8 494 MD_SET_PWM (MD_Data,4,30);
Tom0108 8:b79d21c8178b 495 } else {
Tom0108 8:b79d21c8178b 496 //爪開く
Tom0108 8:b79d21c8178b 497 i2c.Out_Set(Out_Data,1,1);
Tom0108 8:b79d21c8178b 498
Tom0108 13:48c4a4c95e77 499 //赤ゾーン
Tom0108 14:e6d16095ffa8 500 if( yaw<=90 || 270<yaw ) {
Tom0108 13:48c4a4c95e77 501 MD_SET_PWM(MD_Data,4,20);
Tom0108 13:48c4a4c95e77 502 if(RIGHTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 13:48c4a4c95e77 503 else if(LEFTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 13:48c4a4c95e77 504 else {
Tom0108 13:48c4a4c95e77 505 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 13:48c4a4c95e77 506 MD_SET_PWM (MD_Data,4,0);
Tom0108 13:48c4a4c95e77 507 }
Tom0108 13:48c4a4c95e77 508 }
Tom0108 13:48c4a4c95e77 509 //青ゾーン
Tom0108 14:e6d16095ffa8 510 else if( 90<yaw && yaw<=270 ) {
Tom0108 13:48c4a4c95e77 511 MD_SET_PWM(MD_Data,4,20);
Tom0108 13:48c4a4c95e77 512 if(LEFTkey && max_lim==0) MD_SET_DRIVE(MD_Data,4,MD_REVERSE);
Tom0108 13:48c4a4c95e77 513 else if(RIGHTkey && start_lim==0) MD_SET_DRIVE(MD_Data,4,MD_FORWARD);
Tom0108 13:48c4a4c95e77 514 else {
Tom0108 13:48c4a4c95e77 515 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 13:48c4a4c95e77 516 MD_SET_PWM (MD_Data,4,0);
Tom0108 13:48c4a4c95e77 517 }
Tom0108 13:48c4a4c95e77 518 }
Tom0108 13:48c4a4c95e77 519
Tom0108 13:48c4a4c95e77 520 //止まる
Tom0108 13:48c4a4c95e77 521 else {
Tom0108 2:47954f05d32d 522 MD_SET_DRIVE(MD_Data,4,MD_BRAKE);
Tom0108 2:47954f05d32d 523 MD_SET_PWM (MD_Data,4,0);
Tom0108 8:b79d21c8178b 524 }
Tom0108 8:b79d21c8178b 525 //かご停止
Tom0108 8:b79d21c8178b 526 MD_SET_DRIVE(MD_Data,6,MD_BRAKE);
Tom0108 8:b79d21c8178b 527 MD_SET_PWM (MD_Data,6,0);
Tom0108 1:199c4a71da88 528
Tom0108 8:b79d21c8178b 529 kago=0;
Tom0108 8:b79d21c8178b 530 pull=0;
Tom0108 8:b79d21c8178b 531 grab=0;
Tom0108 10:7c73e1577742 532 spull=0;
Tom0108 8:b79d21c8178b 533 }
Tom0108 0:761a63c6d020 534 }
Tom0108 14:e6d16095ffa8 535 }
Tom0108 14:e6d16095ffa8 536 //非常停止
Tom0108 14:e6d16095ffa8 537 else {
Tom0108 14:e6d16095ffa8 538 for(int i=0; i<8; i++) {
Tom0108 14:e6d16095ffa8 539 MD_SET_DRIVE(MD_Data,i,MD_BRAKE);
Tom0108 14:e6d16095ffa8 540 MD_SET_PWM (MD_Data,i,0);
Tom0108 2:47954f05d32d 541 }
Tom0108 14:e6d16095ffa8 542 }
Tom0108 14:e6d16095ffa8 543
Tom0108 14:e6d16095ffa8 544 /* --------------電磁弁系-------------------- */
Tom0108 14:e6d16095ffa8 545 /*タオル掛け*/
Tom0108 14:e6d16095ffa8 546 if(sbdbt.L2)i2c.Out_Set(Out_Data,2,1);
Tom0108 14:e6d16095ffa8 547 else i2c.Out_Set(Out_Data,2,0);
Tom0108 14:e6d16095ffa8 548
Tom0108 14:e6d16095ffa8 549 /*タオルを引っ張るやつ*/
Tom0108 14:e6d16095ffa8 550 if(sbdbt.R2)i2c.Out_Set(Out_Data,4,1);
Tom0108 14:e6d16095ffa8 551 else i2c.Out_Set(Out_Data,4,0);
Tom0108 14:e6d16095ffa8 552
Tom0108 14:e6d16095ffa8 553 //タオル展開
Tom0108 14:e6d16095ffa8 554 if(unfold_f) i2c.Out_Set(Out_Data,3,1);
Tom0108 14:e6d16095ffa8 555 else i2c.Out_Set(Out_Data,3,0);
Tom0108 14:e6d16095ffa8 556
Tom0108 14:e6d16095ffa8 557 //エアー解放
Tom0108 14:e6d16095ffa8 558 if(release_f) {
Tom0108 14:e6d16095ffa8 559 PALETTE(YELLOW);
Tom0108 14:e6d16095ffa8 560 i2c.Out_Set(Out_Data,5,1);
Tom0108 14:e6d16095ffa8 561 } else i2c.Out_Set(Out_Data,5,0);
Tom0108 14:e6d16095ffa8 562
Tom0108 14:e6d16095ffa8 563 //-------------- 出力 --------------//
Tom0108 14:e6d16095ffa8 564 //第二引数には使う最大の個数
Tom0108 14:e6d16095ffa8 565 i2c.Out(Out_Data,9);
Tom0108 14:e6d16095ffa8 566
Tom0108 14:e6d16095ffa8 567 //モータ出力
Tom0108 14:e6d16095ffa8 568 for(int i=0; i<8; i++) i2c.MD_I2C(MD_Data,i);
Tom0108 14:e6d16095ffa8 569
Tom0108 14:e6d16095ffa8 570 /* ----------- りみっと --------------- */
Tom0108 14:e6d16095ffa8 571 limit_up = (In_Data+4)->in_data;
Tom0108 14:e6d16095ffa8 572 limit_down = (In_Data+5)->in_data;
Tom0108 14:e6d16095ffa8 573 max_lim = (In_Data+1)->in_data;
Tom0108 14:e6d16095ffa8 574 start_lim =(In_Data+2)->in_data;
Tom0108 14:e6d16095ffa8 575 short_lim = (In_Data+6)->in_data;
Tom0108 14:e6d16095ffa8 576 slide_start = (In_Data+3)->in_data;
Tom0108 14:e6d16095ffa8 577 slide_stop = (In_Data+0)->in_data;
Tom0108 14:e6d16095ffa8 578
Tom0108 14:e6d16095ffa8 579 /* ----------------ボタン系--------------- */
Tom0108 14:e6d16095ffa8 580 //角度リセット
Tom0108 14:e6d16095ffa8 581 if(sbdbt.L1) {
Tom0108 14:e6d16095ffa8 582 yaw=def_val;
Tom0108 14:e6d16095ffa8 583 Turn_val=def_val;
Tom0108 14:e6d16095ffa8 584 }
Tom0108 14:e6d16095ffa8 585 //かごを倒す
Tom0108 14:e6d16095ffa8 586 if(CROSS) {
Tom0108 14:e6d16095ffa8 587 if(once==0) {
Tom0108 14:e6d16095ffa8 588 kago_f++;
Tom0108 14:e6d16095ffa8 589 kago_f%=2;
Tom0108 14:e6d16095ffa8 590 once++;
Tom0108 3:1063aa133b62 591 }
Tom0108 14:e6d16095ffa8 592 }
Tom0108 14:e6d16095ffa8 593 //タオルを掴んで動かす
Tom0108 14:e6d16095ffa8 594 else if(SQUARE) {
Tom0108 14:e6d16095ffa8 595 if(once==0) {
Tom0108 14:e6d16095ffa8 596 grab_f++;
Tom0108 14:e6d16095ffa8 597 grab_f%=2;
Tom0108 14:e6d16095ffa8 598 once++;
Tom0108 14:e6d16095ffa8 599 }
Tom0108 14:e6d16095ffa8 600 } else if(TRIANGLE) {
Tom0108 14:e6d16095ffa8 601 if(once==0) {
Tom0108 14:e6d16095ffa8 602 spull_f++;
Tom0108 14:e6d16095ffa8 603 spull_f%=2;
Tom0108 14:e6d16095ffa8 604 once++;
Tom0108 14:e6d16095ffa8 605 }
Tom0108 14:e6d16095ffa8 606 }
Tom0108 14:e6d16095ffa8 607 //倒すを引いて動かす
Tom0108 14:e6d16095ffa8 608 else if(CIRCLE) {
Tom0108 14:e6d16095ffa8 609 if(once==0) {
Tom0108 14:e6d16095ffa8 610 pull_f++;
Tom0108 14:e6d16095ffa8 611 pull_f%=2;
Tom0108 14:e6d16095ffa8 612 once++;
Tom0108 3:1063aa133b62 613 }
Tom0108 14:e6d16095ffa8 614 }
Tom0108 14:e6d16095ffa8 615 //タオル展開用
Tom0108 14:e6d16095ffa8 616 else if(sbdbt.START) {
Tom0108 14:e6d16095ffa8 617 if(once==0) {
Tom0108 14:e6d16095ffa8 618 unfold_f++;
Tom0108 14:e6d16095ffa8 619 unfold_f%=2;
Tom0108 14:e6d16095ffa8 620 once++;
Tom0108 11:c1a22e2e3534 621 }
Tom0108 14:e6d16095ffa8 622 }
Tom0108 14:e6d16095ffa8 623 //タオル展開用
Tom0108 14:e6d16095ffa8 624 else if(sbdbt.SELECT) {
Tom0108 14:e6d16095ffa8 625 if(once==0) {
Tom0108 14:e6d16095ffa8 626 release_f++;
Tom0108 14:e6d16095ffa8 627 release_f%=2;
Tom0108 14:e6d16095ffa8 628 once++;
Tom0108 14:e6d16095ffa8 629 }
Tom0108 14:e6d16095ffa8 630 } else if(sbdbt.PS) {
Tom0108 14:e6d16095ffa8 631 if(once==0) {
Tom0108 14:e6d16095ffa8 632 disturb=0;
Tom0108 14:e6d16095ffa8 633 disturb_f++;
Tom0108 14:e6d16095ffa8 634 disturb_f%=2;
Tom0108 14:e6d16095ffa8 635 once++;
Tom0108 14:e6d16095ffa8 636 }
Tom0108 14:e6d16095ffa8 637 } else once=0;
Tom0108 13:48c4a4c95e77 638
Tom0108 13:48c4a4c95e77 639 //半自動のデバック
Tom0108 13:48c4a4c95e77 640 // pc.printf("pull %2d%2d ",pull,pull_f);
Tom0108 13:48c4a4c95e77 641 // pc.printf("grab %2d%2d ",grab,grab_f);
Tom0108 13:48c4a4c95e77 642 // pc.printf("kago %2d%2d ",kago,kago_f);
Tom0108 13:48c4a4c95e77 643 // pc.printf("%.1f %.1f ",rawyaw[0],rawyaw[1]);
Tom0108 14:e6d16095ffa8 644 // pc.printf("yaw:%.1f %.1f",yaw, bno.euler.yaw);
Tom0108 13:48c4a4c95e77 645 // pc.printf("%d %d",keep_tim[0].read_ms(),keep_f[0]);
Tom0108 13:48c4a4c95e77 646 // pc.printf("LX:%d, LY:%d, RX:%d, RY:%d",sbdbt.LX, sbdbt.LY, sbdbt.RX, sbdbt.RY);
Tom0108 14:e6d16095ffa8 647 //for(int i=0; i<7; i++)
Tom0108 14:e6d16095ffa8 648 pc.printf("%d ",sbdbt.PS);
Tom0108 14:e6d16095ffa8 649 pc.printf("%d ",disturb);
Tom0108 13:48c4a4c95e77 650 pc.printf("\n\r");
Tom0108 0:761a63c6d020 651 } // while(1)
Tom0108 0:761a63c6d020 652 } // int main()
Tom0108 0:761a63c6d020 653
Tom0108 0:761a63c6d020 654 /* メカナムの基本移動 */
Tom0108 6:040d001acb12 655 void mecanum_Move(int lx, int ly, int rx)
Tom0108 0:761a63c6d020 656 {
Tom0108 6:040d001acb12 657 duty[0]=lx+ly;
Tom0108 6:040d001acb12 658 duty[1]=-(-lx+ly);
Tom0108 6:040d001acb12 659 duty[2]=-lx+ly;
Tom0108 6:040d001acb12 660 duty[3]=-(lx+ly);
Tom0108 0:761a63c6d020 661
Tom0108 0:761a63c6d020 662 for(int i=0; i<4; i++) {
Tom0108 8:b79d21c8178b 663 //旋回
Tom0108 6:040d001acb12 664 duty[i]+=rx;
Tom0108 8:b79d21c8178b 665
Tom0108 8:b79d21c8178b 666 //制限
Tom0108 8:b79d21c8178b 667 if(duty[i]>=99) duty[i]=99;
Tom0108 8:b79d21c8178b 668 else if(duty[i]<=-99) duty[i]=-99;
Tom0108 6:040d001acb12 669 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 670 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 671 }
Tom0108 0:761a63c6d020 672 }
Tom0108 0:761a63c6d020 673
Tom0108 6:040d001acb12 674 void AngleCorrection(double n_angle, double t_angle)
Tom0108 0:761a63c6d020 675 {
Tom0108 6:040d001acb12 676 double dif=-(t_angle-n_angle);
Tom0108 6:040d001acb12 677
Tom0108 6:040d001acb12 678 if(dif>=30) dif=30;
Tom0108 6:040d001acb12 679 else if(dif<=-30) dif=-30;
Tom0108 6:040d001acb12 680
Tom0108 0:761a63c6d020 681 for(int i=0; i<4; i++) {
Tom0108 6:040d001acb12 682 duty[i]+=dif;
Tom0108 6:040d001acb12 683 MD_SET_DRIVE(MD_Data, i, duty[i]==0? MD_BRAKE: (duty[i]>0? MD_FORWARD: MD_REVERSE));
Tom0108 6:040d001acb12 684 MD_SET_PWM(MD_Data, i, abs(duty[i]));
Tom0108 0:761a63c6d020 685 }
Tom0108 0:761a63c6d020 686 }
Tom0108 0:761a63c6d020 687
Tom0108 15:68720ff6bbc9 688 void Toggle(int *variable, int val)
Tom0108 15:68720ff6bbc9 689 {
Tom0108 15:68720ff6bbc9 690 static int once=0;
Tom0108 15:68720ff6bbc9 691 static int data[2]= {0};
Tom0108 15:68720ff6bbc9 692
Tom0108 15:68720ff6bbc9 693 if(once==0) {
Tom0108 15:68720ff6bbc9 694 data[0]=0;
Tom0108 15:68720ff6bbc9 695 once=1;
Tom0108 15:68720ff6bbc9 696 }
Tom0108 15:68720ff6bbc9 697
Tom0108 15:68720ff6bbc9 698 //値の更新
Tom0108 15:68720ff6bbc9 699 data[1]=data[0];
Tom0108 15:68720ff6bbc9 700 data[0]=val;
Tom0108 15:68720ff6bbc9 701
Tom0108 15:68720ff6bbc9 702 if(data[1]==0 && data[0]==1) {
Tom0108 15:68720ff6bbc9 703 *variable++;
Tom0108 15:68720ff6bbc9 704 *variable%=2;
Tom0108 15:68720ff6bbc9 705 } else if(data[1]==1 && data[0]==0) once=0;
Tom0108 15:68720ff6bbc9 706 }
Tom0108 15:68720ff6bbc9 707
Tom0108 0:761a63c6d020 708 /*操作法*/
Tom0108 0:761a63c6d020 709 /*
Tom0108 0:761a63c6d020 710 〇 ボタン == 回収（引きずる）
Tom0108 0:761a63c6d020 711 ? ボタン == 籠
Tom0108 0:761a63c6d020 712 △ ボタン == 回収（シーツ兼シャツ）
Tom0108 0:761a63c6d020 713 □ ボタン == 回収（タオル）
Tom0108 0:761a63c6d020 714 上 ボタン == ハンガーかけ機構 上昇
Tom0108 0:761a63c6d020 715 下 ボタン == ハンガーかけ機構 下降
Tom0108 0:761a63c6d020 716 L1 ボタン == リセット（足回り）
Tom0108 0:761a63c6d020 717 L2 ボタン == タオル掛け
Tom0108 6:040d001acb12 718 R2 ボタン == タオル真ん中掴む
Tom0108 0:761a63c6d020 719 select == ハンガー掛け 最大まで上昇
Tom0108 0:761a63c6d020 720 start == ハンガー展開
Tom0108 15:68720ff6bbc9 721 */
Tom0108 15:68720ff6bbc9 722
Tom0108 15:68720ff6bbc9 723 void Toggle(int *variable, int val){
Tom0108 15:68720ff6bbc9 724 static int data[2]={0};
Tom0108 15:68720ff6bbc9 725
Tom0108 15:68720ff6bbc9 726 data[1]=data[0];
Tom0108 15:68720ff6bbc9 727 data[0]=val;
Tom0108 15:68720ff6bbc9 728
Tom0108 15:68720ff6bbc9 729 if(data[1]==0 && data[0]==1){
Tom0108 15:68720ff6bbc9 730 *variable++;
Tom0108 15:68720ff6bbc9 731 *variable%=2;
Tom0108 15:68720ff6bbc9 732 }
Tom0108 15:68720ff6bbc9 733 }