Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@19:340fc4370258, 2019-10-09 (annotated)

Committer:
Tom0108
Date:
Wed Oct 09 09:00:01 2019 +0000
Revision:
19:340fc4370258
Parent:
18:d6be6154f9aa
Child:
20:3f68738c4019
10/09

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