yuto kawamura / Mbed 2 deprecated cat18_operate_last

yei

Dependencies:   interface mbed enc_1multi calPID motorout KondoServoLibrary

Fork of cat18_operate by Catch the GIANT Caplico!

leftover_task.txt@23:f45eb02433a5, 2018-09-02 (annotated)

Committer:
shimizuta
Date:
Sun Sep 02 07:53:06 2018 +0000
Revision:
23:f45eb02433a5
Parent:
20:13934809e117 
can move with 3 servo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
shimizuta 1:cc056c41ae30 1 //まだ終わっていない仕事たち
shimizuta 2:4515e8cc6b60 2 const double kp[] = {0.300000, 0, 0};
shimizuta 2:4515e8cc6b60 3 const double ki[] = {0.015017, 0, 0};
shimizuta 2:4515e8cc6b60 4 const double kd[] = {0.003754, 0, 0};
shimizuta 2:4515e8cc6b60 5
shimizuta 1:cc056c41ae30 6 //check.cpp
shimizuta 10:6d1d44fa9739 7 const int kBuffer_mm = 100;
shimizuta 10:6d1d44fa9739 8 //go.cpp
shimizuta 10:6d1d44fa9739 9 const double kServoTicker_s = 0.03;
shimizuta 10:6d1d44fa9739 10 const double kServoVelocity_mm = 100;
shimizuta 1:cc056c41ae30 11
shimizuta 10:6d1d44fa9739 12 IsArrived 内の torelanceとcountの最大値
shimizuta 1:cc056c41ae30 13
shimizuta 10:6d1d44fa9739 14 //GoTo0
shimizuta 18:05f5a3323bda 15 const double kGoTo0Duty = 0.2;
shimizuta 18:05f5a3323bda 16
shimizuta 18:05f5a3323bda 17
shimizuta 18:05f5a3323bda 18 //ごみ箱
shimizuta 18:05f5a3323bda 19 int IsInBanArea(int ban_area_num, const double (&now_position_mm)[3])
shimizuta 18:05f5a3323bda 20 {
shimizuta 18:05f5a3323bda 21 int is_in_ban_area = 1;
shimizuta 18:05f5a3323bda 22 for(int i = 0; i < 3; i++) {
shimizuta 18:05f5a3323bda 23 if(min_ban_area[ban_area_num][i] < now_position_mm[i] && now_position_mm[i] < max_ban_area[ban_area_num][i]) {}
shimizuta 18:05f5a3323bda 24 else {
shimizuta 18:05f5a3323bda 25 is_in_ban_area = 0;
shimizuta 18:05f5a3323bda 26 break;
shimizuta 18:05f5a3323bda 27 }
shimizuta 18:05f5a3323bda 28 }
shimizuta 18:05f5a3323bda 29 return is_in_ban_area;
shimizuta 18:05f5a3323bda 30 }
shimizuta 18:05f5a3323bda 31
shimizuta 18:05f5a3323bda 32 /*
shimizuta 18:05f5a3323bda 33 void Above(WorkState &state)
shimizuta 18:05f5a3323bda 34 {
shimizuta 18:05f5a3323bda 35 int target_pitch_degree = 0;
shimizuta 18:05f5a3323bda 36 int target_yaw_degree = 0;
shimizuta 18:05f5a3323bda 37 switch(state.areaname) {
shimizuta 18:05f5a3323bda 38 case WORKAREA: {
shimizuta 18:05f5a3323bda 39 double target_mm[3] = {};
shimizuta 18:05f5a3323bda 40 for(int i = 0; i < sizeof(target_mm)/sizeof(target_mm[0]); i++) target_mm[i] = state.position[i];
shimizuta 18:05f5a3323bda 41 target_mm[2] += kCatchBuf_mm;
shimizuta 18:05f5a3323bda 42 PIDAbove(target_mm, state);
shimizuta 18:05f5a3323bda 43 switch(state.color) {
shimizuta 18:05f5a3323bda 44 case RED:
shimizuta 18:05f5a3323bda 45 target_mm[1] += kArmLength[3];
shimizuta 18:05f5a3323bda 46 break;
shimizuta 18:05f5a3323bda 47 case YELLOW:
shimizuta 18:05f5a3323bda 48 target_mm[1] -= kArmLength[3];
shimizuta 18:05f5a3323bda 49 break;
shimizuta 18:05f5a3323bda 50 }
shimizuta 18:05f5a3323bda 51 PIDAbove(target_mm, state);
shimizuta 18:05f5a3323bda 52 SetNextRadRelative(3, 0*kDegreeToRad);
shimizuta 18:05f5a3323bda 53 ServoMoveOnArm();
shimizuta 18:05f5a3323bda 54 switch(state.color) {
shimizuta 18:05f5a3323bda 55 case RED:
shimizuta 18:05f5a3323bda 56 SetServoYawRad(0 * kDegreeToRad);
shimizuta 18:05f5a3323bda 57 ServoMoveOnArm();
shimizuta 18:05f5a3323bda 58 target_mm[1] -= kArmLength[3];
shimizuta 18:05f5a3323bda 59 break;
shimizuta 18:05f5a3323bda 60 }
shimizuta 18:05f5a3323bda 61
shimizuta 18:05f5a3323bda 62
shimizuta 18:05f5a3323bda 63 break;
shimizuta 18:05f5a3323bda 64 case COMMONAREA:
shimizuta 18:05f5a3323bda 65 case BOX:
shimizuta 18:05f5a3323bda 66 SetNextRadRelative(3, 45*kDegreeToRad);
shimizuta 18:05f5a3323bda 67 ServoMoveOnArm();
shimizuta 18:05f5a3323bda 68 break;
shimizuta 18:05f5a3323bda 69 }
shimizuta 18:05f5a3323bda 70 }
shimizuta 18:05f5a3323bda 71 }
shimizuta 18:05f5a3323bda 72 */
shimizuta 18:05f5a3323bda 73 /*
shimizuta 18:05f5a3323bda 74 void Above(WorkState &state)
shimizuta 18:05f5a3323bda 75 {
shimizuta 18:05f5a3323bda 76 int target_pitch_degree = 0;
shimizuta 18:05f5a3323bda 77 int target_yaw_degree = 0;
shimizuta 18:05f5a3323bda 78 double target_mm = GetNowElbowZ() + kCatchBuf_mm;
shimizuta 18:05f5a3323bda 79 switch(state.areaname) {
shimizuta 18:05f5a3323bda 80 case WORKAREA:
shimizuta 18:05f5a3323bda 81 for(int i = 0; i < sizeof(calpid)/sizeof(calpid[0]); i++) calpid[i].PileReset();
shimizuta 18:05f5a3323bda 82 while(GetNowElbowZ() < target_mm) {
shimizuta 18:05f5a3323bda 83 output = calpid[0].GetOutPut(target_mm);;
shimizuta 18:05f5a3323bda 84 motor[0].OutPut(GetMotorOutPutZ());
shimizuta 18:05f5a3323bda 85 DEBUG("output %f\r\n", output);
shimizuta 18:05f5a3323bda 86 }
shimizuta 18:05f5a3323bda 87 motor[0].OutPut(0);
shimizuta 18:05f5a3323bda 88 switch(state.color){
shimizuta 18:05f5a3323bda 89 case RED:
shimizuta 18:05f5a3323bda 90
shimizuta 18:05f5a3323bda 91 target_pitch_degree = 0;
shimizuta 18:05f5a3323bda 92 target_yaw_degree = 90;
shimizuta 18:05f5a3323bda 93 break;
shimizuta 18:05f5a3323bda 94 case COMMONAREA:
shimizuta 18:05f5a3323bda 95 case BOX:
shimizuta 18:05f5a3323bda 96 target_pitch_degree = 45;
shimizuta 18:05f5a3323bda 97 target_yaw_degree = 90;
shimizuta 18:05f5a3323bda 98 break;
shimizuta 18:05f5a3323bda 99 }
shimizuta 18:05f5a3323bda 100 SetServoYawRad(target_yaw_degree * kDegreeToRad);
shimizuta 18:05f5a3323bda 101 ServoMoveOnArm();
shimizuta 18:05f5a3323bda 102 SetNextRadRelative(3, target_pitch_degree*kDegreeToRad);
shimizuta 18:05f5a3323bda 103 ServoMoveOnArm();
shimizuta 18:05f5a3323bda 104
shimizuta 18:05f5a3323bda 105 }
shimizuta 18:05f5a3323bda 106 */
shimizuta 18:05f5a3323bda 107 /*
shimizuta 18:05f5a3323bda 108 void PIDAbove(const double (&target_mm)[3], WorkState &state)
shimizuta 18:05f5a3323bda 109 {
shimizuta 18:05f5a3323bda 110
shimizuta 18:05f5a3323bda 111 double now_yaw_rad = GetNowRad(2);
shimizuta 18:05f5a3323bda 112 double now_pitch_rad = GetNowRad(3);
shimizuta 18:05f5a3323bda 113 for(int i = 0; i < sizeof(calpid)/sizeof(calpid[0]); i++) calpid[i].PileReset();
shimizuta 18:05f5a3323bda 114 int is_finish = 0;
shimizuta 18:05f5a3323bda 115 while(is_finish == 0) {
shimizuta 18:05f5a3323bda 116 double now_position_mm[3] = {GetNowTipLocateX(), GetNowTipLocateY(), GetNowTipLocateZ()};
shimizuta 18:05f5a3323bda 117 //収束したらその回でループ終了
shimizuta 18:05f5a3323bda 118 if(IsArrived(target_mm, now_position_mm) == 1) is_finish = 1;
shimizuta 18:05f5a3323bda 119 AllTargetToElbow(target_mm, now_yaw_rad, now_pitch_rad, state);
shimizuta 18:05f5a3323bda 120 //motor出力
shimizuta 18:05f5a3323bda 121 if(motor_move_ticket > 0) {
shimizuta 18:05f5a3323bda 122 motor_move_ticket = 0;
shimizuta 18:05f5a3323bda 123 output = calpid[0].GetOutPut(GetTargetElbow(2));;
shimizuta 18:05f5a3323bda 124 motor[0].OutPut(GetMotorOutPutZ());
shimizuta 18:05f5a3323bda 125 //DEBUG("%f\r\n",GetMotorOutPutZ());
shimizuta 18:05f5a3323bda 126 }
shimizuta 18:05f5a3323bda 127 //servoの出力.kPIDTicker_s毎にmotor出力を変更
shimizuta 18:05f5a3323bda 128 if(servo_move_ticket > 0) {
shimizuta 18:05f5a3323bda 129 servo_move_ticket = 0;
shimizuta 18:05f5a3323bda 130 //出力値計算
shimizuta 18:05f5a3323bda 131 CalServoMove();
shimizuta 18:05f5a3323bda 132 //出力
shimizuta 18:05f5a3323bda 133 ServoMoveOnArm();
shimizuta 18:05f5a3323bda 134 }
shimizuta 18:05f5a3323bda 135 }
shimizuta 18:05f5a3323bda 136 for(int i = 0; i < sizeof(motor)/sizeof(motor[0]); i++) motor[i].OutPut(0);
shimizuta 18:05f5a3323bda 137 }
shimizuta 19:48c3af917932 138 */
shimizuta 19:48c3af917932 139
shimizuta 19:48c3af917932 140 /*
shimizuta 19:48c3af917932 141 int AllowChangeDirection(double yaw_rad, double pitch_rad,
shimizuta 19:48c3af917932 142 const double (&now_position_mm)[3], const double (&now_elbow_mm)[3])
shimizuta 19:48c3af917932 143 {
shimizuta 19:48c3af917932 144 //アーム根本の現在地の取得
shimizuta 19:48c3af917932 145 double next[3] = {};//形状変化後の値
shimizuta 19:48c3af917932 146 for(int i = 0; i < sizeof(next)/sizeof(next[0]); i++) {
shimizuta 19:48c3af917932 147 next[i] = now_elbow_mm[i] + GetFromElbowToHand(i, yaw_rad, pitch_rad);
shimizuta 19:48c3af917932 148 }
shimizuta 19:48c3af917932 149 int isok = 1;
shimizuta 19:48c3af917932 150 for(int i = 0; i < 3; i++) {
shimizuta 19:48c3af917932 151 int out_area_num = CheckBanArea(i, next[i], now_position_mm);
shimizuta 19:48c3af917932 152 if(out_area_num <= -1 || (out_area_num == 0 && GetCounterpartIsInCommon() == 0)) {}
shimizuta 19:48c3af917932 153 else isok = 0;
shimizuta 19:48c3af917932 154 }
shimizuta 19:48c3af917932 155 return isok;
shimizuta 19:48c3af917932 156 }
shimizuta 19:48c3af917932 157 */
shimizuta 19:48c3af917932 158
shimizuta 19:48c3af917932 159 void Priority()
shimizuta 19:48c3af917932 160 {
shimizuta 19:48c3af917932 161 work[23].priority=1000;//SearchCommonArea で使用。最初の時の比較に使用.最後だということを示す。
shimizuta 19:48c3af917932 162
shimizuta 19:48c3af917932 163 work[11].priority=1;
shimizuta 19:48c3af917932 164 work[10].priority=2;
shimizuta 19:48c3af917932 165 work[9].priority=3;
shimizuta 19:48c3af917932 166 work[8].priority=4;
shimizuta 19:48c3af917932 167 work[7].priority=5;
shimizuta 19:48c3af917932 168 work[6].priority=6;
shimizuta 19:48c3af917932 169
shimizuta 19:48c3af917932 170 work[16].priority=3;
shimizuta 19:48c3af917932 171 work[15].priority=4;
shimizuta 19:48c3af917932 172 work[14].priority=5;
shimizuta 19:48c3af917932 173 work[13].priority=6;
shimizuta 19:48c3af917932 174 work[12].priority=7;
shimizuta 19:48c3af917932 175
shimizuta 19:48c3af917932 176 work[22].priority=4;
shimizuta 19:48c3af917932 177 work[21].priority=5;
shimizuta 19:48c3af917932 178 work[20].priority=6;
shimizuta 19:48c3af917932 179 work[19].priority=7;
shimizuta 19:48c3af917932 180 work[18].priority=8;
shimizuta 19:48c3af917932 181 work[17].priority=9;
shimizuta 19:48c3af917932 182
shimizuta 19:48c3af917932 183
shimizuta 19:48c3af917932 184
shimizuta 19:48c3af917932 185 //ワークエリア
shimizuta 19:48c3af917932 186 /*
shimizuta 19:48c3af917932 187 103 102 0
shimizuta 19:48c3af917932 188 102 101 100
shimizuta 19:48c3af917932 189 */
shimizuta 19:48c3af917932 190 work[0].priority=102;
shimizuta 19:48c3af917932 191 work[1].priority=101;
shimizuta 19:48c3af917932 192 work[2].priority=100;
shimizuta 19:48c3af917932 193 work[3].priority=103;
shimizuta 19:48c3af917932 194 work[4].priority=102;
shimizuta 19:48c3af917932 195 work[5].priority=0;
shimizuta 19:48c3af917932 196
shimizuta 20:13934809e117 197 }
shimizuta 20:13934809e117 198
shimizuta 20:13934809e117 199
shimizuta 20:13934809e117 200
shimizuta 20:13934809e117 201
shimizuta 20:13934809e117 202 /*
shimizuta 20:13934809e117 203 double GetIdealYawRad(WorkState &state,MoveTiming timing)
shimizuta 20:13934809e117 204 {
shimizuta 20:13934809e117 205 double degree = 0;
shimizuta 20:13934809e117 206 switch(state.areaname) {
shimizuta 20:13934809e117 207 case BOX:
shimizuta 20:13934809e117 208 case COMMONAREA:
shimizuta 20:13934809e117 209 degree = 90;
shimizuta 20:13934809e117 210 break;
shimizuta 20:13934809e117 211 case WORKAREA:
shimizuta 20:13934809e117 212 switch(state.color) {
shimizuta 20:13934809e117 213 case RED:
shimizuta 20:13934809e117 214 degree = 270;
shimizuta 20:13934809e117 215 break;
shimizuta 20:13934809e117 216 case YELLOW:
shimizuta 20:13934809e117 217 degree = 90;
shimizuta 20:13934809e117 218 break;
shimizuta 20:13934809e117 219 }
shimizuta 20:13934809e117 220 break;
shimizuta 20:13934809e117 221 }
shimizuta 20:13934809e117 222 return degree * kDegreeToRad;
shimizuta 20:13934809e117 223 }
shimizuta 20:13934809e117 224 double GetIdealPitchRad(WorkState &state,MoveTiming timing)
shimizuta 20:13934809e117 225 {
shimizuta 20:13934809e117 226 double degree = 0;
shimizuta 20:13934809e117 227 switch(state.areaname) {
shimizuta 20:13934809e117 228 case BOX:
shimizuta 20:13934809e117 229 switch(timing) {
shimizuta 20:13934809e117 230 case PREMOVE:
shimizuta 20:13934809e117 231 degree = 45;
shimizuta 20:13934809e117 232 break;
shimizuta 20:13934809e117 233 case FINISHMOVE:
shimizuta 20:13934809e117 234 degree = 0;
shimizuta 20:13934809e117 235 break;
shimizuta 20:13934809e117 236 }
shimizuta 20:13934809e117 237 break;
shimizuta 20:13934809e117 238 case COMMONAREA:
shimizuta 20:13934809e117 239 degree = 0;
shimizuta 20:13934809e117 240 break;
shimizuta 20:13934809e117 241 case WORKAREA:
shimizuta 20:13934809e117 242 degree = -90;
shimizuta 20:13934809e117 243 break;
shimizuta 20:13934809e117 244 }
shimizuta 20:13934809e117 245 return degree * kDegreeToRad;
shimizuta 20:13934809e117 246 }
shimizuta 23:f45eb02433a5 247 */
shimizuta 23:f45eb02433a5 248 /*
shimizuta 23:f45eb02433a5 249 int ExitBanArea(const double (&target)[3], LocateParam now, double (&result)[3])
shimizuta 23:f45eb02433a5 250 {
shimizuta 23:f45eb02433a5 251 int is_ban_area = 0;
shimizuta 23:f45eb02433a5 252 double after_mm[2][3] = {};//変更後の値。[0][]が先端、[1][]が肘が入っているときの変更値
shimizuta 23:f45eb02433a5 253 int change[3] = {};//どの座標の値を変更するか
shimizuta 23:f45eb02433a5 254 int ban_area[2] = {-1,-1};//[0]:先端が入っている禁止領域,[1]:肘が入っている禁止領域
shimizuta 23:f45eb02433a5 255 for(int i = 0; i < sizeof(result)/sizeof(result[0]); i++) result[i] = target[i];
shimizuta 23:f45eb02433a5 256
shimizuta 23:f45eb02433a5 257 //先端のチェック{
shimizuta 23:f45eb02433a5 258 ban_area[0] = CheckBanArea(0, now.position[0], now.position);
shimizuta 23:f45eb02433a5 259 #ifdef FORDEBUG
shimizuta 23:f45eb02433a5 260 DEBUG("tip in ban %d \r\n",ban_area[0]);
shimizuta 23:f45eb02433a5 261 #endif
shimizuta 23:f45eb02433a5 262 if(ban_area[0] == 0 && GetCounterpartIsInCommon() == 1) {
shimizuta 23:f45eb02433a5 263 after_mm[0][1] = min_ban_area[ban_area[0]][1] - kStopBuff_mm;
shimizuta 23:f45eb02433a5 264 change[1] = 1;
shimizuta 23:f45eb02433a5 265 } else if(ban_area[0] == 2) {
shimizuta 23:f45eb02433a5 266 after_mm[0][1] = min_ban_area[ban_area[0]][1] - kStopBuff_mm;
shimizuta 23:f45eb02433a5 267 change[1] = 1;
shimizuta 23:f45eb02433a5 268 if(now.position[2] < center[ban_area[0]][2]) {
shimizuta 23:f45eb02433a5 269 after_mm[0][2] = min_ban_area[ban_area[0]][2] - kStopBuff_mm;
shimizuta 23:f45eb02433a5 270 change[2] = 2;
shimizuta 23:f45eb02433a5 271 }
shimizuta 23:f45eb02433a5 272 } else if(ban_area[0] > 0) {
shimizuta 23:f45eb02433a5 273 after_mm[0][2] = max_ban_area[ban_area[0]][2] + kStopBuff_mm;
shimizuta 23:f45eb02433a5 274 change[2] = 1;
shimizuta 23:f45eb02433a5 275 }
shimizuta 23:f45eb02433a5 276 //肘のチェック
shimizuta 23:f45eb02433a5 277 ban_area[1] = CheckBanArea(0, now.elbow[0], now.elbow);
shimizuta 23:f45eb02433a5 278 #ifdef FORDEBUG
shimizuta 23:f45eb02433a5 279 DEBUG("now elbow %f, %f, %f\r\n", now.elbow[0],now.elbow[1],now.elbow[2]);
shimizuta 23:f45eb02433a5 280 DEBUG("elbow in ban %d \r\n",ban_area[1]);
shimizuta 23:f45eb02433a5 281 #endif
shimizuta 23:f45eb02433a5 282 if(ban_area[1] > 0) {
shimizuta 23:f45eb02433a5 283 after_mm[1][2] = max_ban_area[ban_area[1]][2] + kStopBuff_mm + GetFromElbowToHand(2, now.yaw_rad, now.pitch_rad);
shimizuta 23:f45eb02433a5 284 change[2] = 1;
shimizuta 23:f45eb02433a5 285 }
shimizuta 23:f45eb02433a5 286 //最終的な変更値
shimizuta 23:f45eb02433a5 287 if(change[1] == 1) {
shimizuta 23:f45eb02433a5 288 result[1] = after_mm[0][1];
shimizuta 23:f45eb02433a5 289 //DEBUG("ExitBanArea change y %f\r\n",result[1]);
shimizuta 23:f45eb02433a5 290 is_ban_area = 1;
shimizuta 23:f45eb02433a5 291 }
shimizuta 23:f45eb02433a5 292 if(change[2] == 1) {
shimizuta 23:f45eb02433a5 293 if(after_mm[0][2] > after_mm[1][2]) result[2] = after_mm[0][2];
shimizuta 23:f45eb02433a5 294 else result[2] = after_mm[1][2];
shimizuta 23:f45eb02433a5 295 is_ban_area = 1;
shimizuta 23:f45eb02433a5 296 } else if(change[2] == 2) {
shimizuta 23:f45eb02433a5 297 result[2] = after_mm[0][2];
shimizuta 23:f45eb02433a5 298 is_ban_area = 1;
shimizuta 23:f45eb02433a5 299 }
shimizuta 23:f45eb02433a5 300 return is_ban_area;
shimizuta 23:f45eb02433a5 301 }
shimizuta 23:f45eb02433a5 302 */
shimizuta 23:f45eb02433a5 303 /*
shimizuta 23:f45eb02433a5 304 if( is_tip_ban == 1) {
shimizuta 23:f45eb02433a5 305 result[1] = min_ban_area[out_area_num][1] - kStopBuff_mm;
shimizuta 23:f45eb02433a5 306 result[2] = max_ban_area[out_area_num][2] + kStopBuff_mm;
shimizuta 23:f45eb02433a5 307 } else {
shimizuta 23:f45eb02433a5 308 result[2] = max_ban_area[out_area_num][2] + kStopBuff_mm
shimizuta 23:f45eb02433a5 309 + GetFromElbowToHand(2, now.yaw_rad, now.pitch_rad);
shimizuta 23:f45eb02433a5 310 }
shimizuta 23:f45eb02433a5 311 */
shimizuta 23:f45eb02433a5 312
shimizuta 23:f45eb02433a5 313 void ServoMoveOnArmInTurns()
shimizuta 23:f45eb02433a5 314 {
shimizuta 23:f45eb02433a5 315 static int count = 0;
shimizuta 23:f45eb02433a5 316 if(count == 0) {
shimizuta 23:f45eb02433a5 317 for(int i = 0; i < 1; i++) {
shimizuta 23:f45eb02433a5 318 double rad = GetNextRadRelative(i);
shimizuta 23:f45eb02433a5 319 ServoRad(i, rad);
shimizuta 23:f45eb02433a5 320 }
shimizuta 23:f45eb02433a5 321 } else if (count == 1) {
shimizuta 23:f45eb02433a5 322 for(int i = 1; i < 3; i++) {
shimizuta 23:f45eb02433a5 323 double rad = GetNextRadRelative(i);
shimizuta 23:f45eb02433a5 324 ServoRad(i, rad);
shimizuta 23:f45eb02433a5 325 }
shimizuta 23:f45eb02433a5 326 } else if(count == 2) {
shimizuta 23:f45eb02433a5 327 double rad = GetNextRadRelative(3);
shimizuta 23:f45eb02433a5 328 ServoRad(3, rad);
shimizuta 23:f45eb02433a5 329 }
shimizuta 23:f45eb02433a5 330 ++count;
shimizuta 23:f45eb02433a5 331 if(count == 3) count = 0;
shimizuta 23:f45eb02433a5 332 }