![](/media/cache/profiles/P1030212_resized.JPG.50x50_q85.jpg)
STM32足回りプログラム
Dependencies: mbed ros_lib_kinetic
main.cpp@0:a202e1bc38a1, 2019-06-17 (annotated)
- Committer:
- TanakaRobo
- Date:
- Mon Jun 17 13:41:24 2019 +0000
- Revision:
- 0:a202e1bc38a1
- Child:
- 1:7b0db5ea0ab7
STM32 foot;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TanakaRobo | 0:a202e1bc38a1 | 1 | #include "mbed.h" |
TanakaRobo | 0:a202e1bc38a1 | 2 | #include "ros.h" |
TanakaRobo | 0:a202e1bc38a1 | 3 | #include "std_msgs/Float32MultiArray.h" |
TanakaRobo | 0:a202e1bc38a1 | 4 | #include "library/ScrpSlave.hpp" |
TanakaRobo | 0:a202e1bc38a1 | 5 | #include "library/RotaryInc.hpp" |
TanakaRobo | 0:a202e1bc38a1 | 6 | #include "library/GY521.hpp" |
TanakaRobo | 0:a202e1bc38a1 | 7 | |
TanakaRobo | 0:a202e1bc38a1 | 8 | #define MAXPWM 250 |
TanakaRobo | 0:a202e1bc38a1 | 9 | #define PERIOD 2048 |
TanakaRobo | 0:a202e1bc38a1 | 10 | #define Moter_NUM 4 |
TanakaRobo | 0:a202e1bc38a1 | 11 | |
TanakaRobo | 0:a202e1bc38a1 | 12 | #define PI2_3 2.0943951023931954923084289221863 |
TanakaRobo | 0:a202e1bc38a1 | 13 | #define PI_3 1.0471975511965977461542144610932 |
TanakaRobo | 0:a202e1bc38a1 | 14 | #define Moter_L 100 |
TanakaRobo | 0:a202e1bc38a1 | 15 | #define Rotary_L 100 |
TanakaRobo | 0:a202e1bc38a1 | 16 | |
TanakaRobo | 0:a202e1bc38a1 | 17 | #define Kp 0.048 |
TanakaRobo | 0:a202e1bc38a1 | 18 | #define Ki 1.6 |
TanakaRobo | 0:a202e1bc38a1 | 19 | #define Kd 0.00008 |
TanakaRobo | 0:a202e1bc38a1 | 20 | |
TanakaRobo | 0:a202e1bc38a1 | 21 | const PinName pwm_pin[7][3] = { |
TanakaRobo | 0:a202e1bc38a1 | 22 | {PB_1 ,PA_11,PC_6 }, |
TanakaRobo | 0:a202e1bc38a1 | 23 | {PB_13,PB_14,PB_2 }, |
TanakaRobo | 0:a202e1bc38a1 | 24 | {PB_4 ,PB_5 ,PB_15}, |
TanakaRobo | 0:a202e1bc38a1 | 25 | {PC_8 ,PC_9 ,PA_10}, |
TanakaRobo | 0:a202e1bc38a1 | 26 | {PB_6 ,PB_7 ,PB_12}, |
TanakaRobo | 0:a202e1bc38a1 | 27 | {PB_8 ,PB_9 ,PC_7 }, |
TanakaRobo | 0:a202e1bc38a1 | 28 | {PA_0 ,PA_1 ,PB_0 } |
TanakaRobo | 0:a202e1bc38a1 | 29 | }; |
TanakaRobo | 0:a202e1bc38a1 | 30 | |
TanakaRobo | 0:a202e1bc38a1 | 31 | const PinName rotary_pin[8][2] = { |
TanakaRobo | 0:a202e1bc38a1 | 32 | {PC_10,PC_11}, |
TanakaRobo | 0:a202e1bc38a1 | 33 | {PC_4 ,PA_13}, |
TanakaRobo | 0:a202e1bc38a1 | 34 | {PA_14,PA_15}, |
TanakaRobo | 0:a202e1bc38a1 | 35 | {PC_2 ,PC_3 }, |
TanakaRobo | 0:a202e1bc38a1 | 36 | {PA_12,PC_5 }, |
TanakaRobo | 0:a202e1bc38a1 | 37 | {PC_0 ,PC_1 }, |
TanakaRobo | 0:a202e1bc38a1 | 38 | {PA_6 ,PA_7 }, |
TanakaRobo | 0:a202e1bc38a1 | 39 | {PA_8 ,PA_9 } |
TanakaRobo | 0:a202e1bc38a1 | 40 | }; |
TanakaRobo | 0:a202e1bc38a1 | 41 | |
TanakaRobo | 0:a202e1bc38a1 | 42 | ScrpSlave slave(PC_12,PD_2,PH_1,0x0807ffff); |
TanakaRobo | 0:a202e1bc38a1 | 43 | |
TanakaRobo | 0:a202e1bc38a1 | 44 | DigitalOut led(PA_5); |
TanakaRobo | 0:a202e1bc38a1 | 45 | DigitalIn button(PC_13); |
TanakaRobo | 0:a202e1bc38a1 | 46 | Timer motertimer; |
TanakaRobo | 0:a202e1bc38a1 | 47 | |
TanakaRobo | 0:a202e1bc38a1 | 48 | PwmOut* Moter[Moter_NUM][2]; |
TanakaRobo | 0:a202e1bc38a1 | 49 | DigitalOut* Led[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 50 | RotaryInc* Speed[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 51 | RotaryInc* Place[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 52 | GY521 *gy; |
TanakaRobo | 0:a202e1bc38a1 | 53 | bool Ok = false; |
TanakaRobo | 0:a202e1bc38a1 | 54 | double X = 0,Y = 0,T = 0,Yaw = 0;//オドメトリ |
TanakaRobo | 0:a202e1bc38a1 | 55 | double Vx,Vy,Omega;//速度 |
TanakaRobo | 0:a202e1bc38a1 | 56 | double driveS[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 57 | |
TanakaRobo | 0:a202e1bc38a1 | 58 | ros::NodeHandle nh; |
TanakaRobo | 0:a202e1bc38a1 | 59 | |
TanakaRobo | 0:a202e1bc38a1 | 60 | void safe(){ |
TanakaRobo | 0:a202e1bc38a1 | 61 | Vx = 0; |
TanakaRobo | 0:a202e1bc38a1 | 62 | Vy = 0; |
TanakaRobo | 0:a202e1bc38a1 | 63 | Omega = 0; |
TanakaRobo | 0:a202e1bc38a1 | 64 | for(int i = 0;i < Moter_NUM;i++){ |
TanakaRobo | 0:a202e1bc38a1 | 65 | Moter[i][0]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 66 | Moter[i][1]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 67 | Led[i]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 68 | } |
TanakaRobo | 0:a202e1bc38a1 | 69 | } |
TanakaRobo | 0:a202e1bc38a1 | 70 | |
TanakaRobo | 0:a202e1bc38a1 | 71 | void Reset(){ |
TanakaRobo | 0:a202e1bc38a1 | 72 | if(!Ok){ |
TanakaRobo | 0:a202e1bc38a1 | 73 | Ok = true; |
TanakaRobo | 0:a202e1bc38a1 | 74 | }else{ |
TanakaRobo | 0:a202e1bc38a1 | 75 | led.write(0); |
TanakaRobo | 0:a202e1bc38a1 | 76 | safe(); |
TanakaRobo | 0:a202e1bc38a1 | 77 | gy->reset(0); |
TanakaRobo | 0:a202e1bc38a1 | 78 | X = 0; |
TanakaRobo | 0:a202e1bc38a1 | 79 | Y = 0; |
TanakaRobo | 0:a202e1bc38a1 | 80 | T = 0; |
TanakaRobo | 0:a202e1bc38a1 | 81 | led.write(1); |
TanakaRobo | 0:a202e1bc38a1 | 82 | } |
TanakaRobo | 0:a202e1bc38a1 | 83 | } |
TanakaRobo | 0:a202e1bc38a1 | 84 | |
TanakaRobo | 0:a202e1bc38a1 | 85 | inline bool Drive(int id,int pwm){//モーターを回す |
TanakaRobo | 0:a202e1bc38a1 | 86 | pwm = min(max(-MAXPWM,pwm),MAXPWM); |
TanakaRobo | 0:a202e1bc38a1 | 87 | if(!pwm){ |
TanakaRobo | 0:a202e1bc38a1 | 88 | Moter[id][0]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 89 | Moter[id][1]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 90 | Led[id]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 91 | }else if(0 < pwm){ |
TanakaRobo | 0:a202e1bc38a1 | 92 | Moter[id][0]->write(pwm/255); |
TanakaRobo | 0:a202e1bc38a1 | 93 | Moter[id][1]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 94 | Led[id]->write(1); |
TanakaRobo | 0:a202e1bc38a1 | 95 | }else{ |
TanakaRobo | 0:a202e1bc38a1 | 96 | Moter[id][0]->write(0); |
TanakaRobo | 0:a202e1bc38a1 | 97 | Moter[id][1]->write(-pwm/255); |
TanakaRobo | 0:a202e1bc38a1 | 98 | Led[id]->write(1); |
TanakaRobo | 0:a202e1bc38a1 | 99 | } |
TanakaRobo | 0:a202e1bc38a1 | 100 | return true; |
TanakaRobo | 0:a202e1bc38a1 | 101 | } |
TanakaRobo | 0:a202e1bc38a1 | 102 | |
TanakaRobo | 0:a202e1bc38a1 | 103 | inline void move(){ |
TanakaRobo | 0:a202e1bc38a1 | 104 | static double diff[Moter_NUM],errer[Moter_NUM],nowV[Moter_NUM],diffV[Moter_NUM],lastV[Moter_NUM],now_t; |
TanakaRobo | 0:a202e1bc38a1 | 105 | static int j; |
TanakaRobo | 0:a202e1bc38a1 | 106 | #if Moter_NUM == 3 |
TanakaRobo | 0:a202e1bc38a1 | 107 | driveS[0] = Vx*cos(Yaw) + Vy*sin(Yaw) + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 108 | driveS[1] = Vx*cos(Yaw + PI2_3) + Vy*sin(Yaw + PI2_3) + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 109 | driveS[2] = Vx*cos(Yaw - PI2_3) + Vy*sin(Yaw - PI2_3) + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 110 | #elif Moter_NUM == 4 |
TanakaRobo | 0:a202e1bc38a1 | 111 | driveS[0] = Vx*cos(Yaw) + Vy*sin(Yaw) + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 112 | driveS[1] = -Vx*sin(Yaw) + Vy*cos(Yaw) + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 113 | driveS[2] = -Vx*cos(Yaw) - Vy*sin(Yaw) + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 114 | driveS[3] = Vx*sin(Yaw) - Vy*cos(Yaw) + Omega*Moter_L; |
TanakaRobo | 0:a202e1bc38a1 | 115 | #endif |
TanakaRobo | 0:a202e1bc38a1 | 116 | now_t = motertimer.read(); |
TanakaRobo | 0:a202e1bc38a1 | 117 | motertimer.reset(); |
TanakaRobo | 0:a202e1bc38a1 | 118 | for(j = 0;j < Moter_NUM;j++){ |
TanakaRobo | 0:a202e1bc38a1 | 119 | nowV[j] = Speed[j]->getSpeed(); |
TanakaRobo | 0:a202e1bc38a1 | 120 | diff[j] = driveS[j] - nowV[j]; |
TanakaRobo | 0:a202e1bc38a1 | 121 | if(nowV[j] == 0 && driveS[j] == 0 && errer[j] != 0){ |
TanakaRobo | 0:a202e1bc38a1 | 122 | errer[j] = 0; |
TanakaRobo | 0:a202e1bc38a1 | 123 | } |
TanakaRobo | 0:a202e1bc38a1 | 124 | errer[j] += diff[j] * now_t; |
TanakaRobo | 0:a202e1bc38a1 | 125 | diffV[j] = (nowV[j] - lastV[j]) / now_t; |
TanakaRobo | 0:a202e1bc38a1 | 126 | lastV[j] = nowV[j]; |
TanakaRobo | 0:a202e1bc38a1 | 127 | Drive(j,0.08 * driveS[j] + diff[j] * Kp + errer[j] * Ki - diffV[j] * Kp); |
TanakaRobo | 0:a202e1bc38a1 | 128 | } |
TanakaRobo | 0:a202e1bc38a1 | 129 | } |
TanakaRobo | 0:a202e1bc38a1 | 130 | |
TanakaRobo | 0:a202e1bc38a1 | 131 | void getData(const std_msgs::Float32MultiArray &msgs){ |
TanakaRobo | 0:a202e1bc38a1 | 132 | switch((int)msgs.data[0]){ |
TanakaRobo | 0:a202e1bc38a1 | 133 | case -1: |
TanakaRobo | 0:a202e1bc38a1 | 134 | Reset(); |
TanakaRobo | 0:a202e1bc38a1 | 135 | break; |
TanakaRobo | 0:a202e1bc38a1 | 136 | case 0: |
TanakaRobo | 0:a202e1bc38a1 | 137 | Vx = msgs.data[1]; |
TanakaRobo | 0:a202e1bc38a1 | 138 | Vy = msgs.data[2]; |
TanakaRobo | 0:a202e1bc38a1 | 139 | Omega = msgs.data[3]; |
TanakaRobo | 0:a202e1bc38a1 | 140 | break; |
TanakaRobo | 0:a202e1bc38a1 | 141 | case 1: |
TanakaRobo | 0:a202e1bc38a1 | 142 | safe(); |
TanakaRobo | 0:a202e1bc38a1 | 143 | break; |
TanakaRobo | 0:a202e1bc38a1 | 144 | } |
TanakaRobo | 0:a202e1bc38a1 | 145 | } |
TanakaRobo | 0:a202e1bc38a1 | 146 | |
TanakaRobo | 0:a202e1bc38a1 | 147 | std_msgs::Float32MultiArray now; |
TanakaRobo | 0:a202e1bc38a1 | 148 | ros::Publisher place("place",&now); |
TanakaRobo | 0:a202e1bc38a1 | 149 | ros::Subscriber<std_msgs::Float32MultiArray> sub("moter",&getData); |
TanakaRobo | 0:a202e1bc38a1 | 150 | |
TanakaRobo | 0:a202e1bc38a1 | 151 | int main(){ |
TanakaRobo | 0:a202e1bc38a1 | 152 | nh.getHardware()->setBaud(115200); |
TanakaRobo | 0:a202e1bc38a1 | 153 | nh.initNode(); |
TanakaRobo | 0:a202e1bc38a1 | 154 | nh.advertise(place); |
TanakaRobo | 0:a202e1bc38a1 | 155 | nh.subscribe(sub); |
TanakaRobo | 0:a202e1bc38a1 | 156 | now.data_length = 7; |
TanakaRobo | 0:a202e1bc38a1 | 157 | now.data = (float*)malloc(sizeof(float)*now.data_length); |
TanakaRobo | 0:a202e1bc38a1 | 158 | int i; |
TanakaRobo | 0:a202e1bc38a1 | 159 | bool flag = false; |
TanakaRobo | 0:a202e1bc38a1 | 160 | double diff[Moter_NUM],Pspeed[Moter_NUM]; |
TanakaRobo | 0:a202e1bc38a1 | 161 | double nowVx,nowVy,nowVt; |
TanakaRobo | 0:a202e1bc38a1 | 162 | double cos_yaw_2,sin_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 163 | for(i = 0;i < Moter_NUM;i++){ |
TanakaRobo | 0:a202e1bc38a1 | 164 | Led[i] = new DigitalOut(pwm_pin[i][2]); |
TanakaRobo | 0:a202e1bc38a1 | 165 | Moter[i][0] = new PwmOut(pwm_pin[i][0]); |
TanakaRobo | 0:a202e1bc38a1 | 166 | Moter[i][1] = new PwmOut(pwm_pin[i][1]); |
TanakaRobo | 0:a202e1bc38a1 | 167 | Moter[i][0]->period_us(PERIOD); |
TanakaRobo | 0:a202e1bc38a1 | 168 | Moter[i][1]->period_us(PERIOD); |
TanakaRobo | 0:a202e1bc38a1 | 169 | Speed[i] = new RotaryInc(rotary_pin[i][0],rotary_pin[i][1],2*50.8*M_PI,256,2); |
TanakaRobo | 0:a202e1bc38a1 | 170 | Place[i] = new RotaryInc(rotary_pin[i+Moter_NUM][0],rotary_pin[i+Moter_NUM][1],2*50.8*M_PI,256,2); |
TanakaRobo | 0:a202e1bc38a1 | 171 | } |
TanakaRobo | 0:a202e1bc38a1 | 172 | Timer loop; |
TanakaRobo | 0:a202e1bc38a1 | 173 | loop.start(); |
TanakaRobo | 0:a202e1bc38a1 | 174 | while(!button && !Ok){ |
TanakaRobo | 0:a202e1bc38a1 | 175 | nh.spinOnce(); |
TanakaRobo | 0:a202e1bc38a1 | 176 | if(loop.read_ms() > 250){ |
TanakaRobo | 0:a202e1bc38a1 | 177 | led = !led; |
TanakaRobo | 0:a202e1bc38a1 | 178 | loop.reset(); |
TanakaRobo | 0:a202e1bc38a1 | 179 | } |
TanakaRobo | 0:a202e1bc38a1 | 180 | } |
TanakaRobo | 0:a202e1bc38a1 | 181 | I2C i2c(PB_3,PB_10); |
TanakaRobo | 0:a202e1bc38a1 | 182 | GY521 gyro(i2c); |
TanakaRobo | 0:a202e1bc38a1 | 183 | gy = &gyro; |
TanakaRobo | 0:a202e1bc38a1 | 184 | while(true){ |
TanakaRobo | 0:a202e1bc38a1 | 185 | nh.spinOnce(); |
TanakaRobo | 0:a202e1bc38a1 | 186 | gyro.updata(); |
TanakaRobo | 0:a202e1bc38a1 | 187 | Yaw = gyro.yaw; |
TanakaRobo | 0:a202e1bc38a1 | 188 | if(loop.read_ms() > 20){//50msごとに通信する |
TanakaRobo | 0:a202e1bc38a1 | 189 | //データ送信 |
TanakaRobo | 0:a202e1bc38a1 | 190 | now.data[0] = X; |
TanakaRobo | 0:a202e1bc38a1 | 191 | now.data[1] = Y; |
TanakaRobo | 0:a202e1bc38a1 | 192 | now.data[2] = T; |
TanakaRobo | 0:a202e1bc38a1 | 193 | now.data[3] = Yaw; |
TanakaRobo | 0:a202e1bc38a1 | 194 | now.data[4] = nowVx; |
TanakaRobo | 0:a202e1bc38a1 | 195 | now.data[5] = nowVy; |
TanakaRobo | 0:a202e1bc38a1 | 196 | now.data[6] = nowVt; |
TanakaRobo | 0:a202e1bc38a1 | 197 | place.publish(&now); |
TanakaRobo | 0:a202e1bc38a1 | 198 | loop.reset(); |
TanakaRobo | 0:a202e1bc38a1 | 199 | } |
TanakaRobo | 0:a202e1bc38a1 | 200 | Yaw *= 0.01745329251994329576923690768489;//PI/180 |
TanakaRobo | 0:a202e1bc38a1 | 201 | for(i = 0;i<Moter_NUM;++i){ |
TanakaRobo | 0:a202e1bc38a1 | 202 | diff[i] = Place[i]->diff(); |
TanakaRobo | 0:a202e1bc38a1 | 203 | Pspeed[i] = Speed[i]->getSpeed(); |
TanakaRobo | 0:a202e1bc38a1 | 204 | } |
TanakaRobo | 0:a202e1bc38a1 | 205 | //オドメトリ計算 |
TanakaRobo | 0:a202e1bc38a1 | 206 | #if Moter_NUM == 3 |
TanakaRobo | 0:a202e1bc38a1 | 207 | X += -2.0/3.0*diff[0]*cos(Yaw) + 2.0/3.0*diff[1]*cos(Yaw-PI_3) + 2.0/3.0*diff[2]*cos(Yaw+PI_3); |
TanakaRobo | 0:a202e1bc38a1 | 208 | Y += -2.0/3.0*diff[0]*sin(Yaw) + 2.0/3.0*diff[1]*sin(Yaw-PI_3) + 2.0/3.0*diff[2]*sin(Yaw+PI_3); |
TanakaRobo | 0:a202e1bc38a1 | 209 | T += diff[0]/Rotary_L/3 + diff[1]/Rotary_L/3 + diff[2]/Rotary_L/3; |
TanakaRobo | 0:a202e1bc38a1 | 210 | nowVx = -2.0/3.0*Pspeed[0]*cos(Yaw) + 2.0/3.0*Pspeed[1]*cos(Yaw-PI_3) + 2.0/3.0*Pspeed[2]*cos(Yaw+PI_3); |
TanakaRobo | 0:a202e1bc38a1 | 211 | nowVy = -2.0/3.0*Pspeed[0]*sin(Yaw) + 2.0/3.0*Pspeed[1]*sin(Yaw-PI_3) + 2.0/3.0*Pspeed[2]*sin(Yaw+PI_3); |
TanakaRobo | 0:a202e1bc38a1 | 212 | nowVt = Pspeed[0]/Rotary_L/3 + Pspeed[1]/Rotary_L/3 + Pspeed[2]/Rotary_L/3; |
TanakaRobo | 0:a202e1bc38a1 | 213 | #elif Moter_NUM == 4 |
TanakaRobo | 0:a202e1bc38a1 | 214 | cos_yaw_2 = cos(Yaw)/2.0; |
TanakaRobo | 0:a202e1bc38a1 | 215 | sin_yaw_2 = sin(Yaw)/2.0; |
TanakaRobo | 0:a202e1bc38a1 | 216 | X += diff[0]*cos_yaw_2 - diff[1]*sin_yaw_2 - diff[2]*cos_yaw_2 + diff[3]*sin_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 217 | Y += diff[0]*sin_yaw_2 + diff[1]*cos_yaw_2 - diff[2]*sin_yaw_2 - diff[3]*cos_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 218 | T += diff[0]/Rotary_L/4 + diff[1]/Rotary_L/4 + diff[2]/Rotary_L/4 + diff[3]/Rotary_L/4; |
TanakaRobo | 0:a202e1bc38a1 | 219 | nowVx = Pspeed[0]*cos_yaw_2 - Pspeed[1]*sin_yaw_2 - Pspeed[2]*cos_yaw_2 + Pspeed[3]*sin_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 220 | nowVy = Pspeed[0]*sin_yaw_2 + Pspeed[1]*cos_yaw_2 - Pspeed[2]*sin_yaw_2 - Pspeed[3]*cos_yaw_2; |
TanakaRobo | 0:a202e1bc38a1 | 221 | nowVt = Pspeed[0]/Rotary_L/4 + Pspeed[1]/Rotary_L/4 + Pspeed[2]/Rotary_L/4 + Pspeed[3]/Rotary_L/4; |
TanakaRobo | 0:a202e1bc38a1 | 222 | #endif |
TanakaRobo | 0:a202e1bc38a1 | 223 | move(); |
TanakaRobo | 0:a202e1bc38a1 | 224 | if(button && !flag){ |
TanakaRobo | 0:a202e1bc38a1 | 225 | flag = true; |
TanakaRobo | 0:a202e1bc38a1 | 226 | Reset(); |
TanakaRobo | 0:a202e1bc38a1 | 227 | }else if(!button && flag){ |
TanakaRobo | 0:a202e1bc38a1 | 228 | flag = false; |
TanakaRobo | 0:a202e1bc38a1 | 229 | } |
TanakaRobo | 0:a202e1bc38a1 | 230 | } |
TanakaRobo | 0:a202e1bc38a1 | 231 | } |