Takushima Yukimasa / Mbed 2 deprecated R19_ManuTakukAHWGeo

a

Dependencies:   mbed

main.cpp@20:3f68738c4019, 2019-10-11 (annotated)

Committer:
Tom0108
Date:
Fri Oct 11 07:29:15 2019 +0000
Revision:
20:3f68738c4019
Parent:
19:340fc4370258 
Gyro reset measures

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