ケンタ ミヤザキ
Dependencies: PID Serialservo TCS3200 XQ_servo encoder encoder2 mbed moter
Fork of
nhk_2018_undercarry_test09
by 
ケンタ ミヤザキ
main.cpp@1:f4dbd6b9cc27, 2018-03-22 (annotated)
- Committer:
- kenken0721
- Date:
- Thu Mar 22 06:16:03 2018 +0000
- Revision:
- 1:f4dbd6b9cc27
- Parent:
- 0:f6828b914b1c
- Child:
- 2:c5996dd62e9c
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 | 1:f4dbd6b9cc27 | 12 | Encoder Enc_Carry_X(PA_15,PA_14,PC_1); |
| kenken0721 | 1:f4dbd6b9cc27 | 13 | Encoder Enc_Carry_Y(PF_1,PA_1,PA_0); |
| kenken0721 | 1:f4dbd6b9cc27 | 14 | int linecheck(char *buff); |
| kenken0721 | 0:f6828b914b1c | 15 | |
| kenken0721 | 0:f6828b914b1c | 16 | PwmOut pwm_pins[] = { |
| kenken0721 | 0:f6828b914b1c | 17 | PwmOut( PC_8 ), |
| kenken0721 | 0:f6828b914b1c | 18 | PwmOut( PC_6 ), |
| kenken0721 | 0:f6828b914b1c | 19 | PwmOut( PB_14 ), |
| kenken0721 | 0:f6828b914b1c | 20 | PwmOut( PB_13 ) |
| kenken0721 | 0:f6828b914b1c | 21 | }; |
| kenken0721 | 0:f6828b914b1c | 22 | |
| kenken0721 | 0:f6828b914b1c | 23 | DigitalOut dir_pins_1[] = { |
| kenken0721 | 0:f6828b914b1c | 24 | DigitalOut( PC_5 ), |
| kenken0721 | 0:f6828b914b1c | 25 | DigitalOut( PA_7 ), |
| kenken0721 | 0:f6828b914b1c | 26 | DigitalOut( PC_7 ), |
| kenken0721 | 0:f6828b914b1c | 27 | DigitalOut( PB_15) |
| kenken0721 | 0:f6828b914b1c | 28 | }; |
| kenken0721 | 0:f6828b914b1c | 29 | |
| kenken0721 | 0:f6828b914b1c | 30 | DigitalOut dir_pins_2[] = { |
| kenken0721 | 0:f6828b914b1c | 31 | DigitalOut( PB_12 ), |
| kenken0721 | 0:f6828b914b1c | 32 | DigitalOut( PA_6 ), |
| kenken0721 | 0:f6828b914b1c | 33 | DigitalOut( PB_6 ), |
| kenken0721 | 0:f6828b914b1c | 34 | DigitalOut( PB_1) |
| kenken0721 | 0:f6828b914b1c | 35 | }; |
| kenken0721 | 0:f6828b914b1c | 36 | |
| kenken0721 | 0:f6828b914b1c | 37 | bool mode = true;//trueでラジコン、falseでオート |
| kenken0721 | 0:f6828b914b1c | 38 | bool stopflag = true;//trueで機体停止 |
| kenken0721 | 0:f6828b914b1c | 39 | int posit[2] = {0};//機体の現在位置 |
| kenken0721 | 0:f6828b914b1c | 40 | int nextposit[2] = {0};//機体の位置指定 |
| kenken0721 | 0:f6828b914b1c | 41 | double power = POWER; |
| kenken0721 | 0:f6828b914b1c | 42 | int kbtread = 0; |
| kenken0721 | 0:f6828b914b1c | 43 | int count = 0; |
| kenken0721 | 0:f6828b914b1c | 44 | int counter = 0; |
| kenken0721 | 0:f6828b914b1c | 45 | int linedata = 0; |
| kenken0721 | 0:f6828b914b1c | 46 | int prelinedata = 0; |
| kenken0721 | 0:f6828b914b1c | 47 | double output = 0.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 48 | char buff1[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 49 | char buff2[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 50 | char buff3[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 51 | float x_dist = 0.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 52 | float y_dist = 0.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 53 | |
| kenken0721 | 1:f4dbd6b9cc27 | 54 | |
| kenken0721 | 1:f4dbd6b9cc27 | 55 | int main() { |
| kenken0721 | 1:f4dbd6b9cc27 | 56 | |
| kenken0721 | 1:f4dbd6b9cc27 | 57 | S_pid.init(); |
| kenken0721 | 1:f4dbd6b9cc27 | 58 | pwm_pins[0].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 59 | pwm_pins[1].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 60 | pwm_pins[2].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 61 | pwm_pins[3].period(0.00005); |
| kenken0721 | 1:f4dbd6b9cc27 | 62 | kbtpc.baud(2400); |
| kenken0721 | 1:f4dbd6b9cc27 | 63 | |
| kenken0721 | 1:f4dbd6b9cc27 | 64 | char buff1[2]; |
| kenken0721 | 1:f4dbd6b9cc27 | 65 | |
| kenken0721 | 1:f4dbd6b9cc27 | 66 | while (true) { |
| kenken0721 | 1:f4dbd6b9cc27 | 67 | |
| kenken0721 | 1:f4dbd6b9cc27 | 68 | master.read(addr1,buff1,2); |
| kenken0721 | 1:f4dbd6b9cc27 | 69 | int line = linecheck(buff1); |
| kenken0721 | 1:f4dbd6b9cc27 | 70 | kbtpc.putc(1); |
| kenken0721 | 1:f4dbd6b9cc27 | 71 | if(kbtpc.readable()){ |
| kenken0721 | 1:f4dbd6b9cc27 | 72 | kbtread = kbtpc.getc(); |
| kenken0721 | 1:f4dbd6b9cc27 | 73 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 74 | if(kbtread == 255){ |
| kenken0721 | 1:f4dbd6b9cc27 | 75 | mode = true; |
| kenken0721 | 1:f4dbd6b9cc27 | 76 | }else if(kbtread == 150){ |
| kenken0721 | 1:f4dbd6b9cc27 | 77 | mode = false; |
| kenken0721 | 1:f4dbd6b9cc27 | 78 | }else if(kbtread == 100){ |
| kenken0721 | 1:f4dbd6b9cc27 | 79 | stopflag = true; |
| kenken0721 | 1:f4dbd6b9cc27 | 80 | }else if(kbtread == 200){ |
| kenken0721 | 1:f4dbd6b9cc27 | 81 | stopflag = false; |
| kenken0721 | 1:f4dbd6b9cc27 | 82 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 83 | //--------------手動-------------------------------------- |
| kenken0721 | 1:f4dbd6b9cc27 | 84 | if(mode == true){ |
| kenken0721 | 1:f4dbd6b9cc27 | 85 | led2 = 1.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 86 | if(kbtread == 1){ |
| kenken0721 | 1:f4dbd6b9cc27 | 87 | Straight(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 88 | }else if(kbtread == 2){ |
| kenken0721 | 1:f4dbd6b9cc27 | 89 | Back(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 90 | }else if(kbtread == 3){ |
| kenken0721 | 1:f4dbd6b9cc27 | 91 | Right(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 92 | }else if(kbtread == 4){ |
| kenken0721 | 1:f4dbd6b9cc27 | 93 | Left(power, power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 94 | }else{ |
| kenken0721 | 1:f4dbd6b9cc27 | 95 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 96 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 97 | //--------------自動--------------------------------- |
| kenken0721 | 1:f4dbd6b9cc27 | 98 | }else if(mode == false){ |
| kenken0721 | 1:f4dbd6b9cc27 | 99 | led2 = 0.0; |
| kenken0721 | 1:f4dbd6b9cc27 | 100 | if(stopflag == true){ |
| kenken0721 | 1:f4dbd6b9cc27 | 101 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 102 | }else{ |
| kenken0721 | 1:f4dbd6b9cc27 | 103 | if(line == 255){ |
| kenken0721 | 1:f4dbd6b9cc27 | 104 | stopflag = true; |
| kenken0721 | 1:f4dbd6b9cc27 | 105 | Stop(pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 106 | }else{ |
| kenken0721 | 1:f4dbd6b9cc27 | 107 | output = S_pid.compute((double)line); |
| kenken0721 | 1:f4dbd6b9cc27 | 108 | if(output >= 0){ |
| kenken0721 | 1:f4dbd6b9cc27 | 109 | Straight(power, power + output, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 110 | }else { |
| kenken0721 | 1:f4dbd6b9cc27 | 111 | Straight(power + (-1 * output), power, pwm_pins, dir_pins_1, dir_pins_2); |
| kenken0721 | 1:f4dbd6b9cc27 | 112 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 113 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 114 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 115 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 116 | //pc.printf("kbtread = %d" ,kbtread); |
| kenken0721 | 1:f4dbd6b9cc27 | 117 | ///pc.printf("buff[0] = %d" ,buff1[0]); |
| kenken0721 | 1:f4dbd6b9cc27 | 118 | //pc.printf("buff[1] = %d" ,buff1[1]); |
| kenken0721 | 1:f4dbd6b9cc27 | 119 | pc.printf("line = %d" ,line); |
| kenken0721 | 1:f4dbd6b9cc27 | 120 | pc.printf("rpower = %d" ,rpower); |
| kenken0721 | 1:f4dbd6b9cc27 | 121 | pc.printf("lpower = %d" ,lpower); |
| kenken0721 | 1:f4dbd6b9cc27 | 122 | pc.printf("output = %lf \n" ,output); |
| kenken0721 | 1:f4dbd6b9cc27 | 123 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 124 | } |
| kenken0721 | 1:f4dbd6b9cc27 | 125 | |
| kenken0721 | 0:f6828b914b1c | 126 | |
| kenken0721 | 0:f6828b914b1c | 127 | int linecheck(char *buff){ |
| kenken0721 | 0:f6828b914b1c | 128 | if(buff[0] == 24){ |
| kenken0721 | 0:f6828b914b1c | 129 | linedata = 0; |
| kenken0721 | 0:f6828b914b1c | 130 | }else if(buff[0] == 56){ |
| kenken0721 | 0:f6828b914b1c | 131 | linedata = 1; |
| kenken0721 | 0:f6828b914b1c | 132 | }else if(buff[0] == 48){ |
| kenken0721 | 0:f6828b914b1c | 133 | linedata = 2; |
| kenken0721 | 0:f6828b914b1c | 134 | }else if(buff[0] == 112){ |
| kenken0721 | 0:f6828b914b1c | 135 | linedata = 3; |
| kenken0721 | 0:f6828b914b1c | 136 | }else if(buff[0] == 96){ |
| kenken0721 | 0:f6828b914b1c | 137 | linedata = 4; |
| kenken0721 | 0:f6828b914b1c | 138 | }else if(buff[0] == 224){ |
| kenken0721 | 0:f6828b914b1c | 139 | linedata = 5; |
| kenken0721 | 0:f6828b914b1c | 140 | }else if(buff[0] == 192){ |
| kenken0721 | 0:f6828b914b1c | 141 | linedata = 6; |
| kenken0721 | 0:f6828b914b1c | 142 | }else if(buff[0] == 128){ |
| kenken0721 | 0:f6828b914b1c | 143 | linedata = 7; |
| kenken0721 | 0:f6828b914b1c | 144 | }else if(buff[0] == 28){ |
| kenken0721 | 0:f6828b914b1c | 145 | linedata = -1; |
| kenken0721 | 0:f6828b914b1c | 146 | }else if(buff[0] == 12){ |
| kenken0721 | 0:f6828b914b1c | 147 | linedata = -2; |
| kenken0721 | 0:f6828b914b1c | 148 | }else if(buff[0] == 14){ |
| kenken0721 | 0:f6828b914b1c | 149 | linedata = -3; |
| kenken0721 | 0:f6828b914b1c | 150 | }else if(buff[0] == 6){ |
| kenken0721 | 0:f6828b914b1c | 151 | linedata = -4; |
| kenken0721 | 0:f6828b914b1c | 152 | }else if(buff[0] == 7){ |
| kenken0721 | 0:f6828b914b1c | 153 | linedata = -5; |
| kenken0721 | 0:f6828b914b1c | 154 | }else if(buff[0] == 3){ |
| kenken0721 | 0:f6828b914b1c | 155 | linedata = -6; |
| kenken0721 | 0:f6828b914b1c | 156 | }else if(buff[0] == 1){ |
| kenken0721 | 0:f6828b914b1c | 157 | linedata = -7; |
| kenken0721 | 0:f6828b914b1c | 158 | }else if(buff[0] == 15 && buff[1] == 15){ |
| kenken0721 | 0:f6828b914b1c | 159 | linedata = 255; |
| kenken0721 | 0:f6828b914b1c | 160 | }else if(buff[0] == 0 && buff[1] == 0){ |
| kenken0721 | 1:f4dbd6b9cc27 | 161 | linedata = prelinedata; |
| kenken0721 | 0:f6828b914b1c | 162 | }else{ |
| kenken0721 | 0:f6828b914b1c | 163 | linedata = prelinedata; |
| kenken0721 | 0:f6828b914b1c | 164 | } |
| kenken0721 | 0:f6828b914b1c | 165 | prelinedata = linedata; |
| kenken0721 | 0:f6828b914b1c | 166 | |
| kenken0721 | 0:f6828b914b1c | 167 | return linedata; |
| kenken0721 | 1:f4dbd6b9cc27 | 168 | } |