ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test08
by 
ケンタ ミヤザキ
main.cpp@2:c5996dd62e9c, 2018-03-23 (annotated)
- Committer:
- kenken0721
- Date:
- Fri Mar 23 12:34:57 2018 +0000
- Revision:
- 2:c5996dd62e9c
- Parent:
- 1:f4dbd6b9cc27
- Child:
- 3:aaa2fde4fafd
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| kenken0721 | 0:f6828b914b1c | 1 | #include "mbed.h" |
| kenken0721 | 0:f6828b914b1c | 2 | #include "config.h" |
| kenken0721 | 0:f6828b914b1c | 3 | #include "moter.h" |
| kenken0721 | 0:f6828b914b1c | 4 | #include "PID.h" |
| kenken0721 | 1:f4dbd6b9cc27 | 5 | #include "encoder.h" |
| kenken0721 | 0:f6828b914b1c | 6 | |
| kenken0721 | 0:f6828b914b1c | 7 | Serial pc(USBTX, USBRX); |
| kenken0721 | 0:f6828b914b1c | 8 | Serial kbtpc(PC_12 ,PD_2);//serial5 |
| kenken0721 | 0:f6828b914b1c | 9 | I2C master(sda, scl); |
| kenken0721 | 0:f6828b914b1c | 10 | DigitalOut led2(LED2); |
| kenken0721 | 0:f6828b914b1c | 11 | PID S_pid(SP, SI, SD, 0.0); |
| kenken0721 | 2:c5996dd62e9c | 12 | PID R_pid(RP, RI, RD, 0.0); |
| kenken0721 | 2:c5996dd62e9c | 13 | PID L_pid(LP, LI, LD, 0.0); |
| kenken0721 | 2:c5996dd62e9c | 14 | Encoder Enc_Carry_Y(PA_15,PA_14,PC_1); |
| kenken0721 | 2:c5996dd62e9c | 15 | Encoder Enc_Carry_X(PA_4,PB_0,PB_7); |
| kenken0721 | 2:c5996dd62e9c | 16 | void linecheck(char *buff ,int data[2]); |
| kenken0721 | 2:c5996dd62e9c | 17 | void LineCheck(int dmode); |
| kenken0721 | 0:f6828b914b1c | 18 | |
| kenken0721 | 0:f6828b914b1c | 19 | PwmOut pwm_pins[] = { |
| kenken0721 | 0:f6828b914b1c | 20 | PwmOut( PC_8 ), |
| kenken0721 | 0:f6828b914b1c | 21 | PwmOut( PC_6 ), |
| kenken0721 | 0:f6828b914b1c | 22 | PwmOut( PB_14 ), |
| kenken0721 | 0:f6828b914b1c | 23 | PwmOut( PB_13 ) |
| kenken0721 | 0:f6828b914b1c | 24 | }; |
| kenken0721 | 0:f6828b914b1c | 25 | |
| kenken0721 | 0:f6828b914b1c | 26 | DigitalOut dir_pins_1[] = { |
| kenken0721 | 0:f6828b914b1c | 27 | DigitalOut( PC_5 ), |
| kenken0721 | 0:f6828b914b1c | 28 | DigitalOut( PA_7 ), |
| kenken0721 | 2:c5996dd62e9c | 29 | DigitalOut( PB_6 ), |
| kenken0721 | 2:c5996dd62e9c | 30 | DigitalOut( PB_1) |
| kenken0721 | 0:f6828b914b1c | 31 | }; |
| kenken0721 | 0:f6828b914b1c | 32 | |
| kenken0721 | 0:f6828b914b1c | 33 | DigitalOut dir_pins_2[] = { |
| kenken0721 | 0:f6828b914b1c | 34 | DigitalOut( PB_12 ), |
| kenken0721 | 0:f6828b914b1c | 35 | DigitalOut( PA_6 ), |
| kenken0721 | 2:c5996dd62e9c | 36 | DigitalOut( PC_7 ), |
| kenken0721 | 2:c5996dd62e9c | 37 | DigitalOut( PB_15) |
| kenken0721 | 0:f6828b914b1c | 38 | }; |
| kenken0721 | 0:f6828b914b1c | 39 | |
| kenken0721 | 2:c5996dd62e9c | 40 | int crosscount = 0; |
| kenken0721 | 0:f6828b914b1c | 41 | bool mode = true;//trueでラジコン、falseでオート |
| kenken0721 | 0:f6828b914b1c | 42 | bool stopflag = true;//trueで機体停止 |
| kenken0721 | 0:f6828b914b1c | 43 | double power = POWER; |
| kenken0721 | 2:c5996dd62e9c | 44 | double rpower = POWER; |
| kenken0721 | 2:c5996dd62e9c | 45 | double lpower = POWER; |
| kenken0721 | 0:f6828b914b1c | 46 | int kbtread = 0; |
| kenken0721 | 0:f6828b914b1c | 47 | int count = 0; |
| kenken0721 | 0:f6828b914b1c | 48 | int counter = 0; |
| kenken0721 | 2:c5996dd62e9c | 49 | int linedata_1[2] = {0}; |
| kenken0721 | 2:c5996dd62e9c | 50 | int linedata_2[2] = {0}; |
| kenken0721 | 2:c5996dd62e9c | 51 | int linedata_3[2] = {0}; |
| kenken0721 | 0:f6828b914b1c | 52 | int prelinedata = 0; |
| kenken0721 | 0:f6828b914b1c | 53 | double output = 0.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 54 | char buff1[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 55 | char buff2[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 56 | char buff3[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 57 | float x_dist = 0.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 58 | float y_dist = 0.0; |
| kenken0721 | 2:c5996dd62e9c | 59 | float tar_x_dist = 0.0; |
| kenken0721 | 2:c5996dd62e9c | 60 | float tar_y_dist = 0.0; |
| kenken0721 | 2:c5996dd62e9c | 61 | bool ontheline = false; |
| kenken0721 | 2:c5996dd62e9c | 62 | int directmode = STRAIGHT; |
| kenken0721 | 1:f4dbd6b9cc27 | 63 | |
| kenken0721 | 2:c5996dd62e9c | 64 | int main() { |
| kenken0721 | 1:f4dbd6b9cc27 | 65 | S_pid.init(); |
| kenken0721 | 1:f4dbd6b9cc27 | 66 | pwm_pins[0].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 67 | pwm_pins[1].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 68 | pwm_pins[2].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 69 | pwm_pins[3].period(0.00005); |
| kenken0721 | 2:c5996dd62e9c | 70 | kbtpc.baud(9600); |
| kenken0721 | 2:c5996dd62e9c | 71 | master.frequency(100000); |
| kenken0721 | 1:f4dbd6b9cc27 | 72 | while (true) { |
| kenken0721 | 2:c5996dd62e9c | 73 | if(directmode == STRAIGHT) master.read(addr1,buff1,2); |
| kenken0721 | 2:c5996dd62e9c | 74 | if(directmode == RIGHT) master.read(addr2,buff2,2); |
| kenken0721 | 2:c5996dd62e9c | 75 | if(directmode == LEFT) master.read(addr3,buff3,2); |
| kenken0721 | 2:c5996dd62e9c | 76 | |
| kenken0721 | 2:c5996dd62e9c | 77 | //master.read(addr1,buff1,2); |
| kenken0721 | 2:c5996dd62e9c | 78 | linecheck(buff1,linedata_1); |
| kenken0721 | 2:c5996dd62e9c | 79 | x_dist = Enc_Carry_X.read_rotate(); |
| kenken0721 | 2:c5996dd62e9c | 80 | y_dist = Enc_Carry_Y.read_rotate(); |
| kenken0721 | 1:f4dbd6b9cc27 | 81 | kbtpc.putc(1); |
| kenken0721 | 1:f4dbd6b9cc27 | 82 | if(kbtpc.readable()){ |
| kenken0721 | 1:f4dbd6b9cc27 | 83 | kbtread = kbtpc.getc(); |
| kenken0721 | 1:f4dbd6b9cc27 | 84 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 85 | if(kbtread == 255){ |
| kenken0721 | 1:f4dbd6b9cc27 | 86 | mode = true; |
| kenken0721 | 1:f4dbd6b9cc27 | 87 | }else if(kbtread == 150){ |
| kenken0721 | 1:f4dbd6b9cc27 | 88 | mode = false; |
| kenken0721 | 1:f4dbd6b9cc27 | 89 | }else if(kbtread == 100){ |
| kenken0721 | 1:f4dbd6b9cc27 | 90 | stopflag = true; |
| kenken0721 | 1:f4dbd6b9cc27 | 91 | }else if(kbtread == 200){ |
| kenken0721 | 1:f4dbd6b9cc27 | 92 | stopflag = false; |
| kenken0721 | 2:c5996dd62e9c | 93 | }else if(kbtread == STRAIGHT){ |
| kenken0721 | 2:c5996dd62e9c | 94 | directmode = STRAIGHT; |
| kenken0721 | 2:c5996dd62e9c | 95 | }else if(kbtread == RIGHT){ |
| kenken0721 | 2:c5996dd62e9c | 96 | directmode = RIGHT; |
| kenken0721 | 2:c5996dd62e9c | 97 | }else if(kbtread == LEFT){ |
| kenken0721 | 2:c5996dd62e9c | 98 | directmode = LEFT; |
| kenken0721 | 1:f4dbd6b9cc27 | 99 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 100 | //--------------手動-------------------------------------- |
| kenken0721 | 1:f4dbd6b9cc27 | 101 | if(mode == true){ |
| kenken0721 | 1:f4dbd6b9cc27 | 102 | led2 = 1.0; |
| kenken0721 | 2:c5996dd62e9c | 103 | if(kbtread == STRAIGHT){ |
| kenken0721 | 1:f4dbd6b9cc27 | 104 | Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 105 | }else if(kbtread == BACK){ |
| kenken0721 | 1:f4dbd6b9cc27 | 106 | Back(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 107 | }else if(kbtread == RIGHT){ |
| kenken0721 | 1:f4dbd6b9cc27 | 108 | Right(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 109 | }else if(kbtread == LEFT){ |
| kenken0721 | 1:f4dbd6b9cc27 | 110 | Left(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 111 | }else{ |
| kenken0721 | 1:f4dbd6b9cc27 | 112 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 113 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 114 | //--------------自動--------------------------------- |
| kenken0721 | 1:f4dbd6b9cc27 | 115 | }else if(mode == false){ |
| kenken0721 | 1:f4dbd6b9cc27 | 116 | led2 = 0.0; |
| kenken0721 | 2:c5996dd62e9c | 117 | |
| kenken0721 | 1:f4dbd6b9cc27 | 118 | if(stopflag == true){ |
| kenken0721 | 1:f4dbd6b9cc27 | 119 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 120 | }else{ |
| kenken0721 | 2:c5996dd62e9c | 121 | LineCheck(directmode); |
| kenken0721 | 2:c5996dd62e9c | 122 | if(crosscount == 1){ |
| kenken0721 | 2:c5996dd62e9c | 123 | directmode = STOP; |
| kenken0721 | 1:f4dbd6b9cc27 | 124 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 125 | crosscount = 0; |
| kenken0721 | 1:f4dbd6b9cc27 | 126 | }else{ |
| kenken0721 | 2:c5996dd62e9c | 127 | switch(directmode){ |
| kenken0721 | 2:c5996dd62e9c | 128 | case STOP: |
| kenken0721 | 2:c5996dd62e9c | 129 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 130 | break; |
| kenken0721 | 2:c5996dd62e9c | 131 | case STRAIGHT: |
| kenken0721 | 2:c5996dd62e9c | 132 | //master.read(addr1,buff1,2); |
| kenken0721 | 2:c5996dd62e9c | 133 | linecheck(buff1, linedata_1); |
| kenken0721 | 2:c5996dd62e9c | 134 | output = S_pid.compute((double)linedata_1[0]); |
| kenken0721 | 2:c5996dd62e9c | 135 | if(output >= 0){ |
| kenken0721 | 2:c5996dd62e9c | 136 | rpower = power; |
| kenken0721 | 2:c5996dd62e9c | 137 | lpower = power + output; |
| kenken0721 | 2:c5996dd62e9c | 138 | Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 139 | }else { |
| kenken0721 | 2:c5996dd62e9c | 140 | rpower = power + (-1 * output); |
| kenken0721 | 2:c5996dd62e9c | 141 | lpower = power; |
| kenken0721 | 2:c5996dd62e9c | 142 | Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 143 | } |
| kenken0721 | 2:c5996dd62e9c | 144 | break; |
| kenken0721 | 2:c5996dd62e9c | 145 | case RIGHT: |
| kenken0721 | 2:c5996dd62e9c | 146 | //master.read(addr2,buff2,2); |
| kenken0721 | 2:c5996dd62e9c | 147 | linecheck(buff2, linedata_2); |
| kenken0721 | 2:c5996dd62e9c | 148 | output = R_pid.compute((double)linedata_2[0]); |
| kenken0721 | 1:f4dbd6b9cc27 | 149 | if(output >= 0){ |
| kenken0721 | 2:c5996dd62e9c | 150 | rpower = power; |
| kenken0721 | 2:c5996dd62e9c | 151 | lpower = power + output; |
| kenken0721 | 2:c5996dd62e9c | 152 | Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 153 | }else { |
| kenken0721 | 2:c5996dd62e9c | 154 | rpower = power + (-1 * output); |
| kenken0721 | 2:c5996dd62e9c | 155 | lpower = power; |
| kenken0721 | 2:c5996dd62e9c | 156 | Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 157 | } |
| kenken0721 | 2:c5996dd62e9c | 158 | break; |
| kenken0721 | 2:c5996dd62e9c | 159 | case LEFT: |
| kenken0721 | 2:c5996dd62e9c | 160 | //master.read(addr3,buff3,2); |
| kenken0721 | 2:c5996dd62e9c | 161 | linecheck(buff3, linedata_3); |
| kenken0721 | 2:c5996dd62e9c | 162 | output = L_pid.compute((double)linedata_3[0]); |
| kenken0721 | 2:c5996dd62e9c | 163 | if(output >= 0){ |
| kenken0721 | 2:c5996dd62e9c | 164 | rpower = power; |
| kenken0721 | 2:c5996dd62e9c | 165 | lpower = power + output; |
| kenken0721 | 2:c5996dd62e9c | 166 | Left(lpower, rpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 167 | }else { |
| kenken0721 | 2:c5996dd62e9c | 168 | rpower = power + (-1 * output); |
| kenken0721 | 2:c5996dd62e9c | 169 | lpower = power; |
| kenken0721 | 2:c5996dd62e9c | 170 | Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 171 | } |
| kenken0721 | 2:c5996dd62e9c | 172 | break; |
| kenken0721 | 2:c5996dd62e9c | 173 | default: |
| kenken0721 | 2:c5996dd62e9c | 174 | break; |
| kenken0721 | 1:f4dbd6b9cc27 | 175 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 176 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 177 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 178 | } |
| kenken0721 | 2:c5996dd62e9c | 179 | //pc.printf("kbtread = %d \r" ,kbtread); |
| kenken0721 | 2:c5996dd62e9c | 180 | //pc.printf("buff1[0] = %d \r" ,buff1[0]); |
| kenken0721 | 2:c5996dd62e9c | 181 | //pc.printf("buff2[0] = %d \r" ,buff2[0]); |
| kenken0721 | 2:c5996dd62e9c | 182 | //pc.printf("line1 = %d \r" ,linedata_1[0]); |
| kenken0721 | 2:c5996dd62e9c | 183 | //pc.printf("line2 = %d \r" ,linedata_1[1]); |
| kenken0721 | 2:c5996dd62e9c | 184 | //pc.printf("xdist = %f \r",x_dist); |
| kenken0721 | 2:c5996dd62e9c | 185 | //pc.printf("ydist = %f \r",y_dist); |
| kenken0721 | 2:c5996dd62e9c | 186 | //pc.printf("tary = %f \r",tar_y_dist); |
| kenken0721 | 2:c5996dd62e9c | 187 | //pc.printf("stopflag = %d \r",stopflag); |
| kenken0721 | 2:c5996dd62e9c | 188 | //pc.printf("cross = %d \r",crosscount); |
| kenken0721 | 1:f4dbd6b9cc27 | 189 | //pc.printf("buff[1] = %d" ,buff1[1]); |
| kenken0721 | 2:c5996dd62e9c | 190 | //pc.printf("line = %d" ,line); |
| kenken0721 | 2:c5996dd62e9c | 191 | //pc.printf("rpower = %lf" ,rpower); |
| kenken0721 | 2:c5996dd62e9c | 192 | //pc.printf("lpower = %lf" ,lpower); |
| kenken0721 | 2:c5996dd62e9c | 193 | //pc.printf("dicmode = %d \r",directmode ); |
| kenken0721 | 1:f4dbd6b9cc27 | 194 | pc.printf("output = %lf \n" ,output); |
| kenken0721 | 1:f4dbd6b9cc27 | 195 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 196 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 197 | |
| kenken0721 | 2:c5996dd62e9c | 198 | void LineCheck(int dmode){ |
| kenken0721 | 2:c5996dd62e9c | 199 | if(dmode == STRAIGHT){ |
| kenken0721 | 2:c5996dd62e9c | 200 | if(linedata_1[1] == 1){ |
| kenken0721 | 2:c5996dd62e9c | 201 | ontheline = true; |
| kenken0721 | 2:c5996dd62e9c | 202 | tar_y_dist = y_dist; |
| kenken0721 | 2:c5996dd62e9c | 203 | } |
| kenken0721 | 2:c5996dd62e9c | 204 | if(ontheline == true){ |
| kenken0721 | 2:c5996dd62e9c | 205 | if((y_dist - tar_y_dist) >= STOP_DIST){ |
| kenken0721 | 2:c5996dd62e9c | 206 | ontheline = false; |
| kenken0721 | 2:c5996dd62e9c | 207 | crosscount++; |
| kenken0721 | 2:c5996dd62e9c | 208 | } |
| kenken0721 | 2:c5996dd62e9c | 209 | } |
| kenken0721 | 2:c5996dd62e9c | 210 | }else if(dmode == RIGHT){ |
| kenken0721 | 2:c5996dd62e9c | 211 | ontheline = true; |
| kenken0721 | 2:c5996dd62e9c | 212 | tar_x_dist = x_dist; |
| kenken0721 | 2:c5996dd62e9c | 213 | if(ontheline == true){ |
| kenken0721 | 2:c5996dd62e9c | 214 | if((x_dist - tar_x_dist) >= STOP_DIST){ |
| kenken0721 | 2:c5996dd62e9c | 215 | ontheline = false; |
| kenken0721 | 2:c5996dd62e9c | 216 | crosscount++; |
| kenken0721 | 2:c5996dd62e9c | 217 | } |
| kenken0721 | 2:c5996dd62e9c | 218 | } |
| kenken0721 | 2:c5996dd62e9c | 219 | }else if(dmode == LEFT){ |
| kenken0721 | 2:c5996dd62e9c | 220 | ontheline = true; |
| kenken0721 | 2:c5996dd62e9c | 221 | tar_x_dist = x_dist; |
| kenken0721 | 2:c5996dd62e9c | 222 | if(ontheline == true){ |
| kenken0721 | 2:c5996dd62e9c | 223 | if((x_dist - tar_x_dist) <= STOP_DIST){ |
| kenken0721 | 2:c5996dd62e9c | 224 | ontheline = false; |
| kenken0721 | 2:c5996dd62e9c | 225 | crosscount++; |
| kenken0721 | 2:c5996dd62e9c | 226 | } |
| kenken0721 | 2:c5996dd62e9c | 227 | } |
| kenken0721 | 2:c5996dd62e9c | 228 | } |
| kenken0721 | 2:c5996dd62e9c | 229 | } |
| kenken0721 | 0:f6828b914b1c | 230 | |
| kenken0721 | 2:c5996dd62e9c | 231 | void linecheck(char *buff, int data[2]){ |
| kenken0721 | 0:f6828b914b1c | 232 | if(buff[0] == 24){ |
| kenken0721 | 2:c5996dd62e9c | 233 | data[0] = 0; |
| kenken0721 | 2:c5996dd62e9c | 234 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 235 | }else if(buff[0] == 56){ |
| kenken0721 | 2:c5996dd62e9c | 236 | data[0] = 1; |
| kenken0721 | 2:c5996dd62e9c | 237 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 238 | }else if(buff[0] == 48){ |
| kenken0721 | 2:c5996dd62e9c | 239 | data[0] = 2; |
| kenken0721 | 2:c5996dd62e9c | 240 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 241 | }else if(buff[0] == 112){ |
| kenken0721 | 2:c5996dd62e9c | 242 | data[0] = 3; |
| kenken0721 | 2:c5996dd62e9c | 243 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 244 | }else if(buff[0] == 96){ |
| kenken0721 | 2:c5996dd62e9c | 245 | data[0] = 4; |
| kenken0721 | 2:c5996dd62e9c | 246 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 247 | }else if(buff[0] == 224){ |
| kenken0721 | 2:c5996dd62e9c | 248 | data[0] = 5; |
| kenken0721 | 2:c5996dd62e9c | 249 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 250 | }else if(buff[0] == 192){ |
| kenken0721 | 2:c5996dd62e9c | 251 | data[0] = 6; |
| kenken0721 | 2:c5996dd62e9c | 252 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 253 | }else if(buff[0] == 128){ |
| kenken0721 | 2:c5996dd62e9c | 254 | data[0] = 7; |
| kenken0721 | 2:c5996dd62e9c | 255 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 256 | }else if(buff[0] == 28){ |
| kenken0721 | 2:c5996dd62e9c | 257 | data[0] = -1; |
| kenken0721 | 2:c5996dd62e9c | 258 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 259 | }else if(buff[0] == 12){ |
| kenken0721 | 2:c5996dd62e9c | 260 | data[0] = -2; |
| kenken0721 | 2:c5996dd62e9c | 261 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 262 | }else if(buff[0] == 14){ |
| kenken0721 | 2:c5996dd62e9c | 263 | data[0] = -3; |
| kenken0721 | 2:c5996dd62e9c | 264 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 265 | }else if(buff[0] == 6){ |
| kenken0721 | 2:c5996dd62e9c | 266 | data[0] = -4; |
| kenken0721 | 2:c5996dd62e9c | 267 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 268 | }else if(buff[0] == 7){ |
| kenken0721 | 2:c5996dd62e9c | 269 | data[0] = -5; |
| kenken0721 | 2:c5996dd62e9c | 270 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 271 | }else if(buff[0] == 3){ |
| kenken0721 | 2:c5996dd62e9c | 272 | data[0] = -6; |
| kenken0721 | 2:c5996dd62e9c | 273 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 274 | }else if(buff[0] == 1){ |
| kenken0721 | 2:c5996dd62e9c | 275 | data[0] = -7; |
| kenken0721 | 2:c5996dd62e9c | 276 | data[1] = 0; |
| kenken0721 | 2:c5996dd62e9c | 277 | }else if(buff[0] == 255){ |
| kenken0721 | 2:c5996dd62e9c | 278 | data[0] = prelinedata; |
| kenken0721 | 2:c5996dd62e9c | 279 | data[1] = 1; |
| kenken0721 | 2:c5996dd62e9c | 280 | }else if(buff[0] == 0){ |
| kenken0721 | 2:c5996dd62e9c | 281 | data[0] = prelinedata; |
| kenken0721 | 2:c5996dd62e9c | 282 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 283 | }else{ |
| kenken0721 | 2:c5996dd62e9c | 284 | data[0] = prelinedata; |
| kenken0721 | 2:c5996dd62e9c | 285 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 286 | } |
| kenken0721 | 2:c5996dd62e9c | 287 | prelinedata = data[0]; |
| kenken0721 | 0:f6828b914b1c | 288 | |
| kenken0721 | 1:f4dbd6b9cc27 | 289 | } |