ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test08
by 
ケンタ ミヤザキ
main.cpp@3:aaa2fde4fafd, 2018-03-24 (annotated)
- Committer:
- kenken0721
- Date:
- Sat Mar 24 10:50:25 2018 +0000
- Revision:
- 3:aaa2fde4fafd
- Parent:
- 2:c5996dd62e9c
- Child:
- 4:eabb7a738ff8
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 | 3:aaa2fde4fafd | 16 | DigitalOut buzzer(PA_10); |
| kenken0721 | 3:aaa2fde4fafd | 17 | DigitalOut lamp(PC_3); |
| kenken0721 | 3:aaa2fde4fafd | 18 | Timer timer; |
| kenken0721 | 2:c5996dd62e9c | 19 | void linecheck(char *buff ,int data[2]); |
| kenken0721 | 2:c5996dd62e9c | 20 | void LineCheck(int dmode); |
| kenken0721 | 0:f6828b914b1c | 21 | |
| kenken0721 | 0:f6828b914b1c | 22 | PwmOut pwm_pins[] = { |
| kenken0721 | 0:f6828b914b1c | 23 | PwmOut( PC_8 ), |
| kenken0721 | 0:f6828b914b1c | 24 | PwmOut( PC_6 ), |
| kenken0721 | 0:f6828b914b1c | 25 | PwmOut( PB_14 ), |
| kenken0721 | 0:f6828b914b1c | 26 | PwmOut( PB_13 ) |
| kenken0721 | 0:f6828b914b1c | 27 | }; |
| kenken0721 | 0:f6828b914b1c | 28 | |
| kenken0721 | 0:f6828b914b1c | 29 | DigitalOut dir_pins_1[] = { |
| kenken0721 | 0:f6828b914b1c | 30 | DigitalOut( PC_5 ), |
| kenken0721 | 0:f6828b914b1c | 31 | DigitalOut( PA_7 ), |
| kenken0721 | 2:c5996dd62e9c | 32 | DigitalOut( PB_6 ), |
| kenken0721 | 2:c5996dd62e9c | 33 | DigitalOut( PB_1) |
| kenken0721 | 0:f6828b914b1c | 34 | }; |
| kenken0721 | 0:f6828b914b1c | 35 | |
| kenken0721 | 0:f6828b914b1c | 36 | DigitalOut dir_pins_2[] = { |
| kenken0721 | 0:f6828b914b1c | 37 | DigitalOut( PB_12 ), |
| kenken0721 | 0:f6828b914b1c | 38 | DigitalOut( PA_6 ), |
| kenken0721 | 2:c5996dd62e9c | 39 | DigitalOut( PC_7 ), |
| kenken0721 | 2:c5996dd62e9c | 40 | DigitalOut( PB_15) |
| kenken0721 | 0:f6828b914b1c | 41 | }; |
| kenken0721 | 0:f6828b914b1c | 42 | |
| kenken0721 | 2:c5996dd62e9c | 43 | int crosscount = 0; |
| kenken0721 | 0:f6828b914b1c | 44 | bool mode = true;//trueでラジコン、falseでオート |
| kenken0721 | 0:f6828b914b1c | 45 | bool stopflag = true;//trueで機体停止 |
| kenken0721 | 0:f6828b914b1c | 46 | double power = POWER; |
| kenken0721 | 2:c5996dd62e9c | 47 | double rpower = POWER; |
| kenken0721 | 2:c5996dd62e9c | 48 | double lpower = POWER; |
| kenken0721 | 0:f6828b914b1c | 49 | int kbtread = 0; |
| kenken0721 | 0:f6828b914b1c | 50 | int count = 0; |
| kenken0721 | 0:f6828b914b1c | 51 | int counter = 0; |
| kenken0721 | 2:c5996dd62e9c | 52 | int linedata_1[2] = {0}; |
| kenken0721 | 2:c5996dd62e9c | 53 | int linedata_2[2] = {0}; |
| kenken0721 | 2:c5996dd62e9c | 54 | int linedata_3[2] = {0}; |
| kenken0721 | 0:f6828b914b1c | 55 | int prelinedata = 0; |
| kenken0721 | 0:f6828b914b1c | 56 | double output = 0.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 57 | char buff1[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 58 | char buff2[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 59 | char buff3[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 60 | float x_dist = 0.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 61 | float y_dist = 0.0; |
| kenken0721 | 2:c5996dd62e9c | 62 | float tar_x_dist = 0.0; |
| kenken0721 | 2:c5996dd62e9c | 63 | float tar_y_dist = 0.0; |
| kenken0721 | 2:c5996dd62e9c | 64 | bool ontheline = false; |
| kenken0721 | 2:c5996dd62e9c | 65 | int directmode = STRAIGHT; |
| kenken0721 | 3:aaa2fde4fafd | 66 | int state = START; |
| kenken0721 | 3:aaa2fde4fafd | 67 | int times = 0; |
| kenken0721 | 3:aaa2fde4fafd | 68 | bool throwing = false; |
| kenken0721 | 3:aaa2fde4fafd | 69 | int main() { |
| kenken0721 | 3:aaa2fde4fafd | 70 | timer.reset(); |
| kenken0721 | 3:aaa2fde4fafd | 71 | timer.stop(); |
| kenken0721 | 3:aaa2fde4fafd | 72 | //timer.start(); |
| kenken0721 | 1:f4dbd6b9cc27 | 73 | S_pid.init(); |
| kenken0721 | 3:aaa2fde4fafd | 74 | R_pid.init(); |
| kenken0721 | 3:aaa2fde4fafd | 75 | L_pid.init(); |
| kenken0721 | 1:f4dbd6b9cc27 | 76 | pwm_pins[0].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 77 | pwm_pins[1].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 78 | pwm_pins[2].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 79 | pwm_pins[3].period(0.00005); |
| kenken0721 | 3:aaa2fde4fafd | 80 | pc.baud(115200); |
| kenken0721 | 2:c5996dd62e9c | 81 | kbtpc.baud(9600); |
| kenken0721 | 2:c5996dd62e9c | 82 | master.frequency(100000); |
| kenken0721 | 3:aaa2fde4fafd | 83 | lamp = 1.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 84 | while (true) { |
| kenken0721 | 3:aaa2fde4fafd | 85 | //if(directmode == STRAIGHT){ |
| kenken0721 | 3:aaa2fde4fafd | 86 | master.read(addr1,buff1,2); |
| kenken0721 | 3:aaa2fde4fafd | 87 | linecheck(buff1,linedata_1); |
| kenken0721 | 3:aaa2fde4fafd | 88 | //} |
| kenken0721 | 3:aaa2fde4fafd | 89 | //if(directmode == RIGHT){ |
| kenken0721 | 3:aaa2fde4fafd | 90 | master.read(addr2,buff2,2); |
| kenken0721 | 3:aaa2fde4fafd | 91 | linecheck(buff2,linedata_2); |
| kenken0721 | 3:aaa2fde4fafd | 92 | //} |
| kenken0721 | 3:aaa2fde4fafd | 93 | //if(directmode == LEFT){ |
| kenken0721 | 3:aaa2fde4fafd | 94 | master.read(addr3,buff3,2); |
| kenken0721 | 3:aaa2fde4fafd | 95 | linecheck(buff3,linedata_3); |
| kenken0721 | 3:aaa2fde4fafd | 96 | //} |
| kenken0721 | 2:c5996dd62e9c | 97 | //master.read(addr1,buff1,2); |
| kenken0721 | 3:aaa2fde4fafd | 98 | //linecheck(buff1,linedata_1); |
| kenken0721 | 3:aaa2fde4fafd | 99 | //linecheck(buff1,linedata_1); |
| kenken0721 | 3:aaa2fde4fafd | 100 | //linecheck(buff1,linedata_1); |
| kenken0721 | 2:c5996dd62e9c | 101 | x_dist = Enc_Carry_X.read_rotate(); |
| kenken0721 | 2:c5996dd62e9c | 102 | y_dist = Enc_Carry_Y.read_rotate(); |
| kenken0721 | 3:aaa2fde4fafd | 103 | kbtpc.putc(state); |
| kenken0721 | 1:f4dbd6b9cc27 | 104 | if(kbtpc.readable()){ |
| kenken0721 | 1:f4dbd6b9cc27 | 105 | kbtread = kbtpc.getc(); |
| kenken0721 | 1:f4dbd6b9cc27 | 106 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 107 | if(kbtread == 255){ |
| kenken0721 | 1:f4dbd6b9cc27 | 108 | mode = true; |
| kenken0721 | 1:f4dbd6b9cc27 | 109 | }else if(kbtread == 150){ |
| kenken0721 | 1:f4dbd6b9cc27 | 110 | mode = false; |
| kenken0721 | 1:f4dbd6b9cc27 | 111 | }else if(kbtread == 100){ |
| kenken0721 | 1:f4dbd6b9cc27 | 112 | stopflag = true; |
| kenken0721 | 1:f4dbd6b9cc27 | 113 | }else if(kbtread == 200){ |
| kenken0721 | 1:f4dbd6b9cc27 | 114 | stopflag = false; |
| kenken0721 | 2:c5996dd62e9c | 115 | }else if(kbtread == STRAIGHT){ |
| kenken0721 | 2:c5996dd62e9c | 116 | directmode = STRAIGHT; |
| kenken0721 | 2:c5996dd62e9c | 117 | }else if(kbtread == RIGHT){ |
| kenken0721 | 2:c5996dd62e9c | 118 | directmode = RIGHT; |
| kenken0721 | 2:c5996dd62e9c | 119 | }else if(kbtread == LEFT){ |
| kenken0721 | 2:c5996dd62e9c | 120 | directmode = LEFT; |
| kenken0721 | 3:aaa2fde4fafd | 121 | }else if(kbtread == MANUAL){ |
| kenken0721 | 3:aaa2fde4fafd | 122 | state = MANUAL; |
| kenken0721 | 3:aaa2fde4fafd | 123 | }else if(kbtread == START){ |
| kenken0721 | 3:aaa2fde4fafd | 124 | state = START; |
| kenken0721 | 1:f4dbd6b9cc27 | 125 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 126 | //--------------手動-------------------------------------- |
| kenken0721 | 1:f4dbd6b9cc27 | 127 | if(mode == true){ |
| kenken0721 | 1:f4dbd6b9cc27 | 128 | led2 = 1.0; |
| kenken0721 | 2:c5996dd62e9c | 129 | if(kbtread == STRAIGHT){ |
| kenken0721 | 1:f4dbd6b9cc27 | 130 | Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 131 | }else if(kbtread == BACK){ |
| kenken0721 | 1:f4dbd6b9cc27 | 132 | Back(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 133 | }else if(kbtread == RIGHT){ |
| kenken0721 | 1:f4dbd6b9cc27 | 134 | Right(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 135 | }else if(kbtread == LEFT){ |
| kenken0721 | 1:f4dbd6b9cc27 | 136 | Left(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 137 | }else{ |
| kenken0721 | 1:f4dbd6b9cc27 | 138 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 139 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 140 | //--------------自動--------------------------------- |
| kenken0721 | 1:f4dbd6b9cc27 | 141 | }else if(mode == false){ |
| kenken0721 | 1:f4dbd6b9cc27 | 142 | led2 = 0.0; |
| kenken0721 | 3:aaa2fde4fafd | 143 | //timer.reset(); |
| kenken0721 | 3:aaa2fde4fafd | 144 | //timer.start(); |
| kenken0721 | 1:f4dbd6b9cc27 | 145 | if(stopflag == true){ |
| kenken0721 | 1:f4dbd6b9cc27 | 146 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 147 | }else{ |
| kenken0721 | 2:c5996dd62e9c | 148 | LineCheck(directmode); |
| kenken0721 | 3:aaa2fde4fafd | 149 | switch(state){ |
| kenken0721 | 3:aaa2fde4fafd | 150 | case MANUAL: |
| kenken0721 | 3:aaa2fde4fafd | 151 | if(crosscount == 1){ |
| kenken0721 | 3:aaa2fde4fafd | 152 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 153 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 3:aaa2fde4fafd | 154 | crosscount = 0; |
| kenken0721 | 3:aaa2fde4fafd | 155 | } |
| kenken0721 | 3:aaa2fde4fafd | 156 | break; |
| kenken0721 | 3:aaa2fde4fafd | 157 | |
| kenken0721 | 3:aaa2fde4fafd | 158 | //------------------------------------------ |
| kenken0721 | 3:aaa2fde4fafd | 159 | case START: |
| kenken0721 | 3:aaa2fde4fafd | 160 | if(crosscount == 0){ |
| kenken0721 | 3:aaa2fde4fafd | 161 | directmode = STRAIGHT; |
| kenken0721 | 3:aaa2fde4fafd | 162 | }else if(crosscount == 1 && directmode == STRAIGHT){ |
| kenken0721 | 3:aaa2fde4fafd | 163 | directmode = LEFT; |
| kenken0721 | 3:aaa2fde4fafd | 164 | prelinedata = 0; |
| kenken0721 | 3:aaa2fde4fafd | 165 | }else if(crosscount == 1 && directmode == LEFT){ |
| kenken0721 | 3:aaa2fde4fafd | 166 | directmode = LEFT; |
| kenken0721 | 3:aaa2fde4fafd | 167 | }else if(crosscount == 2){ |
| kenken0721 | 3:aaa2fde4fafd | 168 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 169 | state = STAND_BY_1; |
| kenken0721 | 3:aaa2fde4fafd | 170 | crosscount = 0; |
| kenken0721 | 3:aaa2fde4fafd | 171 | prelinedata = 0; |
| kenken0721 | 3:aaa2fde4fafd | 172 | } |
| kenken0721 | 3:aaa2fde4fafd | 173 | break; |
| kenken0721 | 3:aaa2fde4fafd | 174 | //----------------------------------------------- |
| kenken0721 | 3:aaa2fde4fafd | 175 | case STAND_BY_1: |
| kenken0721 | 3:aaa2fde4fafd | 176 | crosscount = 0; |
| kenken0721 | 3:aaa2fde4fafd | 177 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 178 | //buzzer = 1.0; |
| kenken0721 | 3:aaa2fde4fafd | 179 | if(kbtread == RED_RECEIVE){ |
| kenken0721 | 3:aaa2fde4fafd | 180 | state = RED_RECEIVE; |
| kenken0721 | 3:aaa2fde4fafd | 181 | }else if(kbtread == BLUE_RECEIVE){ |
| kenken0721 | 3:aaa2fde4fafd | 182 | state = BLUE_RECEIVE_1; |
| kenken0721 | 3:aaa2fde4fafd | 183 | } |
| kenken0721 | 3:aaa2fde4fafd | 184 | break; |
| kenken0721 | 3:aaa2fde4fafd | 185 | case RED_RECEIVE: |
| kenken0721 | 3:aaa2fde4fafd | 186 | if(crosscount == 0){ |
| kenken0721 | 3:aaa2fde4fafd | 187 | directmode = LEFT; |
| kenken0721 | 3:aaa2fde4fafd | 188 | }else if(crosscount == 1 && throwing == false){ |
| kenken0721 | 3:aaa2fde4fafd | 189 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 190 | prelinedata = 0; |
| kenken0721 | 3:aaa2fde4fafd | 191 | if(kbtread == THROWING){ |
| kenken0721 | 3:aaa2fde4fafd | 192 | throwing = true; |
| kenken0721 | 3:aaa2fde4fafd | 193 | buzzer = 1.0; |
| kenken0721 | 3:aaa2fde4fafd | 194 | timer.start(); |
| kenken0721 | 3:aaa2fde4fafd | 195 | //lamp = 0.0; |
| kenken0721 | 3:aaa2fde4fafd | 196 | //wait(3.0); |
| kenken0721 | 3:aaa2fde4fafd | 197 | //buzzer = 0.0; |
| kenken0721 | 3:aaa2fde4fafd | 198 | prelinedata = 0; |
| kenken0721 | 3:aaa2fde4fafd | 199 | //directmode = RIGHT; |
| kenken0721 | 3:aaa2fde4fafd | 200 | } |
| kenken0721 | 3:aaa2fde4fafd | 201 | }else if(crosscount == 1 && throwing == true){ |
| kenken0721 | 3:aaa2fde4fafd | 202 | times = timer.read(); |
| kenken0721 | 3:aaa2fde4fafd | 203 | if(times > 2){ |
| kenken0721 | 3:aaa2fde4fafd | 204 | lamp = 0.0; |
| kenken0721 | 3:aaa2fde4fafd | 205 | buzzer = 0.0; |
| kenken0721 | 3:aaa2fde4fafd | 206 | directmode = RIGHT; |
| kenken0721 | 3:aaa2fde4fafd | 207 | timer.reset(); |
| kenken0721 | 3:aaa2fde4fafd | 208 | timer.stop(); |
| kenken0721 | 3:aaa2fde4fafd | 209 | } |
| kenken0721 | 3:aaa2fde4fafd | 210 | }else if(crosscount == 2){ |
| kenken0721 | 3:aaa2fde4fafd | 211 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 212 | state = STAND_BY_1; |
| kenken0721 | 3:aaa2fde4fafd | 213 | throwing = false; |
| kenken0721 | 3:aaa2fde4fafd | 214 | crosscount = 0; |
| kenken0721 | 3:aaa2fde4fafd | 215 | } |
| kenken0721 | 3:aaa2fde4fafd | 216 | break; |
| kenken0721 | 3:aaa2fde4fafd | 217 | case BLUE_RECEIVE_1: |
| kenken0721 | 3:aaa2fde4fafd | 218 | if(crosscount == 0){ |
| kenken0721 | 3:aaa2fde4fafd | 219 | directmode = RIGHT; |
| kenken0721 | 3:aaa2fde4fafd | 220 | }else if(crosscount == 1){ |
| kenken0721 | 3:aaa2fde4fafd | 221 | directmode = STRAIGHT; |
| kenken0721 | 3:aaa2fde4fafd | 222 | }else if(crosscount == 2){ |
| kenken0721 | 3:aaa2fde4fafd | 223 | directmode = LEFT; |
| kenken0721 | 3:aaa2fde4fafd | 224 | }else if(crosscount == 3){ |
| kenken0721 | 3:aaa2fde4fafd | 225 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 226 | prelinedata = 0; |
| kenken0721 | 3:aaa2fde4fafd | 227 | state = STAND_BY_2; |
| kenken0721 | 3:aaa2fde4fafd | 228 | crosscount = 0; |
| kenken0721 | 3:aaa2fde4fafd | 229 | } |
| kenken0721 | 3:aaa2fde4fafd | 230 | break; |
| kenken0721 | 3:aaa2fde4fafd | 231 | case STAND_BY_2: |
| kenken0721 | 3:aaa2fde4fafd | 232 | crosscount = 0; |
| kenken0721 | 3:aaa2fde4fafd | 233 | if(kbtread == BLUE_RECEIVE){ |
| kenken0721 | 3:aaa2fde4fafd | 234 | state = BLUE_RECEIVE_2; |
| kenken0721 | 3:aaa2fde4fafd | 235 | }else if(kbtread == YELLOW_RECEIVE){ |
| kenken0721 | 3:aaa2fde4fafd | 236 | state = YELLOW_RECEIVE; |
| kenken0721 | 3:aaa2fde4fafd | 237 | } |
| kenken0721 | 3:aaa2fde4fafd | 238 | break; |
| kenken0721 | 3:aaa2fde4fafd | 239 | case BLUE_RECEIVE_2: |
| kenken0721 | 3:aaa2fde4fafd | 240 | if(crosscount == 0){ |
| kenken0721 | 3:aaa2fde4fafd | 241 | directmode = LEFT; |
| kenken0721 | 3:aaa2fde4fafd | 242 | }else if(crosscount == 1){ |
| kenken0721 | 3:aaa2fde4fafd | 243 | if(throwing == false){ |
| kenken0721 | 3:aaa2fde4fafd | 244 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 245 | prelinedata = 0; |
| kenken0721 | 3:aaa2fde4fafd | 246 | if(kbtread == THROWING){ |
| kenken0721 | 3:aaa2fde4fafd | 247 | throwing = true; |
| kenken0721 | 3:aaa2fde4fafd | 248 | buzzer = 1.0; |
| kenken0721 | 3:aaa2fde4fafd | 249 | timer.start(); |
| kenken0721 | 3:aaa2fde4fafd | 250 | prelinedata = 0; |
| kenken0721 | 3:aaa2fde4fafd | 251 | } |
| kenken0721 | 3:aaa2fde4fafd | 252 | }else{ |
| kenken0721 | 3:aaa2fde4fafd | 253 | times = timer.read(); |
| kenken0721 | 3:aaa2fde4fafd | 254 | if(times > 2){ |
| kenken0721 | 3:aaa2fde4fafd | 255 | buzzer = 0.0; |
| kenken0721 | 3:aaa2fde4fafd | 256 | directmode = RIGHT; |
| kenken0721 | 3:aaa2fde4fafd | 257 | timer.reset(); |
| kenken0721 | 3:aaa2fde4fafd | 258 | timer.stop(); |
| kenken0721 | 3:aaa2fde4fafd | 259 | } |
| kenken0721 | 3:aaa2fde4fafd | 260 | } |
| kenken0721 | 3:aaa2fde4fafd | 261 | }else if(crosscount == 2){ |
| kenken0721 | 3:aaa2fde4fafd | 262 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 263 | state = STAND_BY_2; |
| kenken0721 | 3:aaa2fde4fafd | 264 | throwing = false; |
| kenken0721 | 3:aaa2fde4fafd | 265 | crosscount = 0; |
| kenken0721 | 3:aaa2fde4fafd | 266 | } |
| kenken0721 | 3:aaa2fde4fafd | 267 | break; |
| kenken0721 | 3:aaa2fde4fafd | 268 | case YELLOW_RECEIVE: |
| kenken0721 | 3:aaa2fde4fafd | 269 | if(crosscount != 4){ |
| kenken0721 | 3:aaa2fde4fafd | 270 | directmode = LEFT; |
| kenken0721 | 3:aaa2fde4fafd | 271 | }else{ |
| kenken0721 | 3:aaa2fde4fafd | 272 | directmode = STOP; |
| kenken0721 | 3:aaa2fde4fafd | 273 | } |
| kenken0721 | 3:aaa2fde4fafd | 274 | break; |
| kenken0721 | 3:aaa2fde4fafd | 275 | default: |
| kenken0721 | 3:aaa2fde4fafd | 276 | break; |
| kenken0721 | 3:aaa2fde4fafd | 277 | } |
| kenken0721 | 3:aaa2fde4fafd | 278 | /* |
| kenken0721 | 2:c5996dd62e9c | 279 | if(crosscount == 1){ |
| kenken0721 | 2:c5996dd62e9c | 280 | directmode = STOP; |
| kenken0721 | 1:f4dbd6b9cc27 | 281 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 282 | crosscount = 0; |
| kenken0721 | 3:aaa2fde4fafd | 283 | } |
| kenken0721 | 3:aaa2fde4fafd | 284 | */ |
| kenken0721 | 3:aaa2fde4fafd | 285 | switch(directmode){ |
| kenken0721 | 2:c5996dd62e9c | 286 | case STOP: |
| kenken0721 | 2:c5996dd62e9c | 287 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 288 | break; |
| kenken0721 | 2:c5996dd62e9c | 289 | case STRAIGHT: |
| kenken0721 | 2:c5996dd62e9c | 290 | //master.read(addr1,buff1,2); |
| kenken0721 | 3:aaa2fde4fafd | 291 | //linecheck(buff1, linedata_1); |
| kenken0721 | 2:c5996dd62e9c | 292 | output = S_pid.compute((double)linedata_1[0]); |
| kenken0721 | 2:c5996dd62e9c | 293 | if(output >= 0){ |
| kenken0721 | 2:c5996dd62e9c | 294 | rpower = power; |
| kenken0721 | 2:c5996dd62e9c | 295 | lpower = power + output; |
| kenken0721 | 2:c5996dd62e9c | 296 | Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 297 | }else { |
| kenken0721 | 2:c5996dd62e9c | 298 | rpower = power + (-1 * output); |
| kenken0721 | 2:c5996dd62e9c | 299 | lpower = power; |
| kenken0721 | 2:c5996dd62e9c | 300 | Straight(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 301 | } |
| kenken0721 | 2:c5996dd62e9c | 302 | break; |
| kenken0721 | 2:c5996dd62e9c | 303 | case RIGHT: |
| kenken0721 | 2:c5996dd62e9c | 304 | //master.read(addr2,buff2,2); |
| kenken0721 | 3:aaa2fde4fafd | 305 | //linecheck(buff2, linedata_2); |
| kenken0721 | 2:c5996dd62e9c | 306 | output = R_pid.compute((double)linedata_2[0]); |
| kenken0721 | 1:f4dbd6b9cc27 | 307 | if(output >= 0){ |
| kenken0721 | 2:c5996dd62e9c | 308 | rpower = power; |
| kenken0721 | 2:c5996dd62e9c | 309 | lpower = power + output; |
| kenken0721 | 2:c5996dd62e9c | 310 | Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 311 | }else { |
| kenken0721 | 2:c5996dd62e9c | 312 | rpower = power + (-1 * output); |
| kenken0721 | 2:c5996dd62e9c | 313 | lpower = power; |
| kenken0721 | 2:c5996dd62e9c | 314 | Right(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 315 | } |
| kenken0721 | 2:c5996dd62e9c | 316 | break; |
| kenken0721 | 2:c5996dd62e9c | 317 | case LEFT: |
| kenken0721 | 2:c5996dd62e9c | 318 | //master.read(addr3,buff3,2); |
| kenken0721 | 3:aaa2fde4fafd | 319 | //linecheck(buff3, linedata_3); |
| kenken0721 | 2:c5996dd62e9c | 320 | output = L_pid.compute((double)linedata_3[0]); |
| kenken0721 | 2:c5996dd62e9c | 321 | if(output >= 0){ |
| kenken0721 | 2:c5996dd62e9c | 322 | rpower = power; |
| kenken0721 | 2:c5996dd62e9c | 323 | lpower = power + output; |
| kenken0721 | 3:aaa2fde4fafd | 324 | Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 325 | }else { |
| kenken0721 | 2:c5996dd62e9c | 326 | rpower = power + (-1 * output); |
| kenken0721 | 2:c5996dd62e9c | 327 | lpower = power; |
| kenken0721 | 2:c5996dd62e9c | 328 | Left(rpower, lpower, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 2:c5996dd62e9c | 329 | } |
| kenken0721 | 2:c5996dd62e9c | 330 | break; |
| kenken0721 | 2:c5996dd62e9c | 331 | default: |
| kenken0721 | 2:c5996dd62e9c | 332 | break; |
| kenken0721 | 1:f4dbd6b9cc27 | 333 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 334 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 335 | } |
| kenken0721 | 3:aaa2fde4fafd | 336 | //pc.printf("kbtread = %d \r" ,kbtread); |
| kenken0721 | 2:c5996dd62e9c | 337 | //pc.printf("buff1[0] = %d \r" ,buff1[0]); |
| kenken0721 | 2:c5996dd62e9c | 338 | //pc.printf("buff2[0] = %d \r" ,buff2[0]); |
| kenken0721 | 2:c5996dd62e9c | 339 | //pc.printf("line1 = %d \r" ,linedata_1[0]); |
| kenken0721 | 3:aaa2fde4fafd | 340 | //pc.printf("line2 = %d \r" ,linedata_3[1]); |
| kenken0721 | 2:c5996dd62e9c | 341 | //pc.printf("xdist = %f \r",x_dist); |
| kenken0721 | 3:aaa2fde4fafd | 342 | //pc.printf("tar_xdist = %f \r",tar_x_dist); |
| kenken0721 | 3:aaa2fde4fafd | 343 | //pc.printf("ontheline = %d \r",ontheline); |
| kenken0721 | 3:aaa2fde4fafd | 344 | |
| kenken0721 | 2:c5996dd62e9c | 345 | //pc.printf("ydist = %f \r",y_dist); |
| kenken0721 | 2:c5996dd62e9c | 346 | //pc.printf("tary = %f \r",tar_y_dist); |
| kenken0721 | 2:c5996dd62e9c | 347 | //pc.printf("stopflag = %d \r",stopflag); |
| kenken0721 | 2:c5996dd62e9c | 348 | //pc.printf("cross = %d \r",crosscount); |
| kenken0721 | 1:f4dbd6b9cc27 | 349 | //pc.printf("buff[1] = %d" ,buff1[1]); |
| kenken0721 | 2:c5996dd62e9c | 350 | //pc.printf("line = %d" ,line); |
| kenken0721 | 2:c5996dd62e9c | 351 | //pc.printf("rpower = %lf" ,rpower); |
| kenken0721 | 3:aaa2fde4fafd | 352 | //pc.printf("lpower = %lf" ,lpower); |
| kenken0721 | 3:aaa2fde4fafd | 353 | pc.printf("dicmode = %d \r",directmode); |
| kenken0721 | 1:f4dbd6b9cc27 | 354 | pc.printf("output = %lf \n" ,output); |
| kenken0721 | 1:f4dbd6b9cc27 | 355 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 356 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 357 | |
| kenken0721 | 2:c5996dd62e9c | 358 | void LineCheck(int dmode){ |
| kenken0721 | 2:c5996dd62e9c | 359 | if(dmode == STRAIGHT){ |
| kenken0721 | 2:c5996dd62e9c | 360 | if(linedata_1[1] == 1){ |
| kenken0721 | 2:c5996dd62e9c | 361 | ontheline = true; |
| kenken0721 | 2:c5996dd62e9c | 362 | tar_y_dist = y_dist; |
| kenken0721 | 2:c5996dd62e9c | 363 | } |
| kenken0721 | 2:c5996dd62e9c | 364 | if(ontheline == true){ |
| kenken0721 | 3:aaa2fde4fafd | 365 | if((y_dist - tar_y_dist) >= Y_STOP_DIST){ |
| kenken0721 | 2:c5996dd62e9c | 366 | ontheline = false; |
| kenken0721 | 2:c5996dd62e9c | 367 | crosscount++; |
| kenken0721 | 2:c5996dd62e9c | 368 | } |
| kenken0721 | 2:c5996dd62e9c | 369 | } |
| kenken0721 | 2:c5996dd62e9c | 370 | }else if(dmode == RIGHT){ |
| kenken0721 | 3:aaa2fde4fafd | 371 | if(linedata_2[1] == 1){ |
| kenken0721 | 3:aaa2fde4fafd | 372 | ontheline = true; |
| kenken0721 | 3:aaa2fde4fafd | 373 | tar_x_dist = x_dist; |
| kenken0721 | 3:aaa2fde4fafd | 374 | } |
| kenken0721 | 2:c5996dd62e9c | 375 | if(ontheline == true){ |
| kenken0721 | 3:aaa2fde4fafd | 376 | if((x_dist - tar_x_dist) >= X_STOP_DIST){ |
| kenken0721 | 2:c5996dd62e9c | 377 | ontheline = false; |
| kenken0721 | 2:c5996dd62e9c | 378 | crosscount++; |
| kenken0721 | 2:c5996dd62e9c | 379 | } |
| kenken0721 | 2:c5996dd62e9c | 380 | } |
| kenken0721 | 2:c5996dd62e9c | 381 | }else if(dmode == LEFT){ |
| kenken0721 | 3:aaa2fde4fafd | 382 | if(linedata_3[1] == 1){ |
| kenken0721 | 3:aaa2fde4fafd | 383 | ontheline = true; |
| kenken0721 | 3:aaa2fde4fafd | 384 | tar_x_dist = x_dist; |
| kenken0721 | 3:aaa2fde4fafd | 385 | } |
| kenken0721 | 2:c5996dd62e9c | 386 | if(ontheline == true){ |
| kenken0721 | 3:aaa2fde4fafd | 387 | if((tar_x_dist - x_dist) >= X_STOP_DIST){ |
| kenken0721 | 2:c5996dd62e9c | 388 | ontheline = false; |
| kenken0721 | 2:c5996dd62e9c | 389 | crosscount++; |
| kenken0721 | 2:c5996dd62e9c | 390 | } |
| kenken0721 | 2:c5996dd62e9c | 391 | } |
| kenken0721 | 2:c5996dd62e9c | 392 | } |
| kenken0721 | 2:c5996dd62e9c | 393 | } |
| kenken0721 | 0:f6828b914b1c | 394 | |
| kenken0721 | 2:c5996dd62e9c | 395 | void linecheck(char *buff, int data[2]){ |
| kenken0721 | 0:f6828b914b1c | 396 | if(buff[0] == 24){ |
| kenken0721 | 2:c5996dd62e9c | 397 | data[0] = 0; |
| kenken0721 | 2:c5996dd62e9c | 398 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 399 | }else if(buff[0] == 56){ |
| kenken0721 | 2:c5996dd62e9c | 400 | data[0] = 1; |
| kenken0721 | 2:c5996dd62e9c | 401 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 402 | }else if(buff[0] == 48){ |
| kenken0721 | 2:c5996dd62e9c | 403 | data[0] = 2; |
| kenken0721 | 2:c5996dd62e9c | 404 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 405 | }else if(buff[0] == 112){ |
| kenken0721 | 2:c5996dd62e9c | 406 | data[0] = 3; |
| kenken0721 | 2:c5996dd62e9c | 407 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 408 | }else if(buff[0] == 96){ |
| kenken0721 | 2:c5996dd62e9c | 409 | data[0] = 4; |
| kenken0721 | 2:c5996dd62e9c | 410 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 411 | }else if(buff[0] == 224){ |
| kenken0721 | 2:c5996dd62e9c | 412 | data[0] = 5; |
| kenken0721 | 2:c5996dd62e9c | 413 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 414 | }else if(buff[0] == 192){ |
| kenken0721 | 2:c5996dd62e9c | 415 | data[0] = 6; |
| kenken0721 | 2:c5996dd62e9c | 416 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 417 | }else if(buff[0] == 128){ |
| kenken0721 | 2:c5996dd62e9c | 418 | data[0] = 7; |
| kenken0721 | 2:c5996dd62e9c | 419 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 420 | }else if(buff[0] == 28){ |
| kenken0721 | 2:c5996dd62e9c | 421 | data[0] = -1; |
| kenken0721 | 2:c5996dd62e9c | 422 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 423 | }else if(buff[0] == 12){ |
| kenken0721 | 2:c5996dd62e9c | 424 | data[0] = -2; |
| kenken0721 | 2:c5996dd62e9c | 425 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 426 | }else if(buff[0] == 14){ |
| kenken0721 | 2:c5996dd62e9c | 427 | data[0] = -3; |
| kenken0721 | 2:c5996dd62e9c | 428 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 429 | }else if(buff[0] == 6){ |
| kenken0721 | 2:c5996dd62e9c | 430 | data[0] = -4; |
| kenken0721 | 2:c5996dd62e9c | 431 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 432 | }else if(buff[0] == 7){ |
| kenken0721 | 2:c5996dd62e9c | 433 | data[0] = -5; |
| kenken0721 | 2:c5996dd62e9c | 434 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 435 | }else if(buff[0] == 3){ |
| kenken0721 | 2:c5996dd62e9c | 436 | data[0] = -6; |
| kenken0721 | 2:c5996dd62e9c | 437 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 438 | }else if(buff[0] == 1){ |
| kenken0721 | 2:c5996dd62e9c | 439 | data[0] = -7; |
| kenken0721 | 2:c5996dd62e9c | 440 | data[1] = 0; |
| kenken0721 | 2:c5996dd62e9c | 441 | }else if(buff[0] == 255){ |
| kenken0721 | 2:c5996dd62e9c | 442 | data[0] = prelinedata; |
| kenken0721 | 2:c5996dd62e9c | 443 | data[1] = 1; |
| kenken0721 | 2:c5996dd62e9c | 444 | }else if(buff[0] == 0){ |
| kenken0721 | 2:c5996dd62e9c | 445 | data[0] = prelinedata; |
| kenken0721 | 2:c5996dd62e9c | 446 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 447 | }else{ |
| kenken0721 | 2:c5996dd62e9c | 448 | data[0] = prelinedata; |
| kenken0721 | 2:c5996dd62e9c | 449 | data[1] = 0; |
| kenken0721 | 0:f6828b914b1c | 450 | } |
| kenken0721 | 2:c5996dd62e9c | 451 | prelinedata = data[0]; |
| kenken0721 | 3:aaa2fde4fafd | 452 | } |