Miya Miyagawa
/
NEW_LineTraceHub_2
aa
main.cpp@1:63ae8cbbe758, 2019-09-03 (annotated)
- Committer:
- M_souta
- Date:
- Tue Sep 03 14:12:36 2019 +0000
- Revision:
- 1:63ae8cbbe758
- Parent:
- 0:8aa125a0a09a
- Child:
- 2:77fe1a8f5bac
kkk
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Ryosei | 0:8aa125a0a09a | 1 | #include "mbed.h" |
Ryosei | 0:8aa125a0a09a | 2 | #include "SO1602A.h" |
Ryosei | 0:8aa125a0a09a | 3 | #define select1_0 PB_4 |
Ryosei | 0:8aa125a0a09a | 4 | #define select1_1 PB_5 |
Ryosei | 0:8aa125a0a09a | 5 | #define select1_2 PB_3 |
Ryosei | 0:8aa125a0a09a | 6 | #define select1_3 PB_1 |
Ryosei | 0:8aa125a0a09a | 7 | #define select2_0 PF_0 |
Ryosei | 0:8aa125a0a09a | 8 | #define select2_1 PF_1 |
Ryosei | 0:8aa125a0a09a | 9 | #define select2_2 PA_8 |
Ryosei | 0:8aa125a0a09a | 10 | #define select2_3 PA_11 |
Ryosei | 0:8aa125a0a09a | 11 | #define select3_0 PA_4 |
Ryosei | 0:8aa125a0a09a | 12 | #define select3_1 PA_5 |
Ryosei | 0:8aa125a0a09a | 13 | #define select3_2 PA_7 |
Ryosei | 0:8aa125a0a09a | 14 | #define select3_3 PA_6 |
Ryosei | 0:8aa125a0a09a | 15 | |
Ryosei | 0:8aa125a0a09a | 16 | #define Z0 PA_0 |
Ryosei | 0:8aa125a0a09a | 17 | #define Z1 PA_1 |
Ryosei | 0:8aa125a0a09a | 18 | #define Z2 PA_3 |
Ryosei | 0:8aa125a0a09a | 19 | |
Ryosei | 0:8aa125a0a09a | 20 | #define SDA PB_7 |
Ryosei | 0:8aa125a0a09a | 21 | #define SCL PB_6 |
Ryosei | 0:8aa125a0a09a | 22 | |
Ryosei | 0:8aa125a0a09a | 23 | #define uartSEpin PA_2 |
Ryosei | 0:8aa125a0a09a | 24 | |
Ryosei | 0:8aa125a0a09a | 25 | DigitalOut uartSE(uartSEpin); |
Ryosei | 0:8aa125a0a09a | 26 | |
Ryosei | 0:8aa125a0a09a | 27 | AnalogIn y1(Z0); |
Ryosei | 0:8aa125a0a09a | 28 | AnalogIn y2(Z1); |
Ryosei | 0:8aa125a0a09a | 29 | AnalogIn y3(Z2); |
Ryosei | 0:8aa125a0a09a | 30 | |
Ryosei | 0:8aa125a0a09a | 31 | DigitalOut select1[]= { |
Ryosei | 0:8aa125a0a09a | 32 | DigitalOut(select1_0), |
Ryosei | 0:8aa125a0a09a | 33 | DigitalOut(select1_1), |
Ryosei | 0:8aa125a0a09a | 34 | DigitalOut(select1_2), |
Ryosei | 0:8aa125a0a09a | 35 | DigitalOut(select1_3), |
Ryosei | 0:8aa125a0a09a | 36 | }; |
Ryosei | 0:8aa125a0a09a | 37 | DigitalOut select2[]= { |
Ryosei | 0:8aa125a0a09a | 38 | DigitalOut(select2_0), |
Ryosei | 0:8aa125a0a09a | 39 | DigitalOut(select2_1), |
Ryosei | 0:8aa125a0a09a | 40 | DigitalOut(select2_2), |
Ryosei | 0:8aa125a0a09a | 41 | DigitalOut(select2_3), |
Ryosei | 0:8aa125a0a09a | 42 | }; |
Ryosei | 0:8aa125a0a09a | 43 | DigitalOut select3[]= { |
Ryosei | 0:8aa125a0a09a | 44 | DigitalOut(select3_0), |
Ryosei | 0:8aa125a0a09a | 45 | DigitalOut(select3_1), |
Ryosei | 0:8aa125a0a09a | 46 | DigitalOut(select3_2), |
Ryosei | 0:8aa125a0a09a | 47 | DigitalOut(select3_3), |
Ryosei | 0:8aa125a0a09a | 48 | }; |
M_souta | 1:63ae8cbbe758 | 49 | |
M_souta | 1:63ae8cbbe758 | 50 | Serial pc(USBTX,USBRX); |
Ryosei | 0:8aa125a0a09a | 51 | Serial uart(PA_9,PA_10); |
Ryosei | 0:8aa125a0a09a | 52 | I2C i2c(SDA,SCL); |
Ryosei | 0:8aa125a0a09a | 53 | SO1602A oled(i2c,0x78); |
Ryosei | 0:8aa125a0a09a | 54 | |
Ryosei | 0:8aa125a0a09a | 55 | void PinSelect(int pin) |
Ryosei | 0:8aa125a0a09a | 56 | { |
Ryosei | 0:8aa125a0a09a | 57 | bool selectnum[16][4]= { |
Ryosei | 0:8aa125a0a09a | 58 | {0,0,0,0}, |
Ryosei | 0:8aa125a0a09a | 59 | {0,0,0,1}, |
Ryosei | 0:8aa125a0a09a | 60 | {0,0,1,0}, |
Ryosei | 0:8aa125a0a09a | 61 | {0,0,1,1}, |
Ryosei | 0:8aa125a0a09a | 62 | {0,1,0,0}, |
Ryosei | 0:8aa125a0a09a | 63 | {0,1,0,1}, |
Ryosei | 0:8aa125a0a09a | 64 | {0,1,1,0}, |
Ryosei | 0:8aa125a0a09a | 65 | {0,1,1,1}, |
Ryosei | 0:8aa125a0a09a | 66 | {1,0,0,0}, |
Ryosei | 0:8aa125a0a09a | 67 | {1,0,0,1}, |
Ryosei | 0:8aa125a0a09a | 68 | {1,0,1,0}, |
Ryosei | 0:8aa125a0a09a | 69 | {1,0,1,1}, |
Ryosei | 0:8aa125a0a09a | 70 | {1,1,0,0}, |
Ryosei | 0:8aa125a0a09a | 71 | {1,1,0,1}, |
Ryosei | 0:8aa125a0a09a | 72 | {1,1,1,0}, |
Ryosei | 0:8aa125a0a09a | 73 | {1,1,1,1}, |
Ryosei | 0:8aa125a0a09a | 74 | }; |
Ryosei | 0:8aa125a0a09a | 75 | if(pin<=15) { |
Ryosei | 0:8aa125a0a09a | 76 | select1[0]=selectnum[pin][3]; |
Ryosei | 0:8aa125a0a09a | 77 | select1[1]=selectnum[pin][2]; |
Ryosei | 0:8aa125a0a09a | 78 | select1[2]=selectnum[pin][1]; |
Ryosei | 0:8aa125a0a09a | 79 | select1[3]=selectnum[pin][0]; |
Ryosei | 0:8aa125a0a09a | 80 | for(int i=0; i<=3; i++) { |
Ryosei | 0:8aa125a0a09a | 81 | select2[i]=0; |
Ryosei | 0:8aa125a0a09a | 82 | select3[i]=0; |
Ryosei | 0:8aa125a0a09a | 83 | } |
Ryosei | 0:8aa125a0a09a | 84 | } else if(pin<=31) { |
Ryosei | 0:8aa125a0a09a | 85 | select2[0]=selectnum[pin-16][3]; |
Ryosei | 0:8aa125a0a09a | 86 | select2[1]=selectnum[pin-16][2]; |
Ryosei | 0:8aa125a0a09a | 87 | select2[2]=selectnum[pin-16][1]; |
Ryosei | 0:8aa125a0a09a | 88 | select2[3]=selectnum[pin-16][0]; |
Ryosei | 0:8aa125a0a09a | 89 | for(int i=0; i<=3; i++) { |
Ryosei | 0:8aa125a0a09a | 90 | select1[i]=0; |
Ryosei | 0:8aa125a0a09a | 91 | select3[i]=0; |
Ryosei | 0:8aa125a0a09a | 92 | } |
Ryosei | 0:8aa125a0a09a | 93 | } else if(pin<=47) { |
Ryosei | 0:8aa125a0a09a | 94 | select3[0]=selectnum[pin-32][3]; |
Ryosei | 0:8aa125a0a09a | 95 | select3[1]=selectnum[pin-32][2]; |
Ryosei | 0:8aa125a0a09a | 96 | select3[2]=selectnum[pin-32][1]; |
Ryosei | 0:8aa125a0a09a | 97 | select3[3]=selectnum[pin-32][0]; |
Ryosei | 0:8aa125a0a09a | 98 | for(int i=0; i<=3; i++) { |
Ryosei | 0:8aa125a0a09a | 99 | select1[i]=0; |
Ryosei | 0:8aa125a0a09a | 100 | select2[i]=0; |
Ryosei | 0:8aa125a0a09a | 101 | } |
Ryosei | 0:8aa125a0a09a | 102 | } |
Ryosei | 0:8aa125a0a09a | 103 | } |
Ryosei | 0:8aa125a0a09a | 104 | |
Ryosei | 0:8aa125a0a09a | 105 | void LineDataRead(int read[48]) |
Ryosei | 0:8aa125a0a09a | 106 | { |
Ryosei | 0:8aa125a0a09a | 107 | float anal[48]; |
Ryosei | 0:8aa125a0a09a | 108 | for(int i=0; i<=47; i++) { |
Ryosei | 0:8aa125a0a09a | 109 | PinSelect(i); |
Ryosei | 0:8aa125a0a09a | 110 | if(i<=15) { |
Ryosei | 0:8aa125a0a09a | 111 | anal[i]=y1.read(); |
Ryosei | 0:8aa125a0a09a | 112 | } else if(i<=31) { |
Ryosei | 0:8aa125a0a09a | 113 | anal[i]=y2.read(); |
Ryosei | 0:8aa125a0a09a | 114 | } else if(i<=47) { |
Ryosei | 0:8aa125a0a09a | 115 | anal[i]=y3.read(); |
Ryosei | 0:8aa125a0a09a | 116 | } |
M_souta | 1:63ae8cbbe758 | 117 | // pc.printf("%f ",anal[i]); |
Ryosei | 0:8aa125a0a09a | 118 | |
Ryosei | 0:8aa125a0a09a | 119 | if(anal[i]<0.8f) { |
Ryosei | 0:8aa125a0a09a | 120 | read[i]=0; |
Ryosei | 0:8aa125a0a09a | 121 | } else { |
Ryosei | 0:8aa125a0a09a | 122 | read[i]=1; |
Ryosei | 0:8aa125a0a09a | 123 | } |
Ryosei | 0:8aa125a0a09a | 124 | // oled.printf("%.3f",read[i]); |
Ryosei | 0:8aa125a0a09a | 125 | |
Ryosei | 0:8aa125a0a09a | 126 | } |
Ryosei | 0:8aa125a0a09a | 127 | // oled.printf("%d %f",read[0],anal[0]); |
Ryosei | 0:8aa125a0a09a | 128 | } |
Ryosei | 0:8aa125a0a09a | 129 | |
Ryosei | 0:8aa125a0a09a | 130 | void DataTrim(char send[6],int data[48]) |
Ryosei | 0:8aa125a0a09a | 131 | { |
M_souta | 1:63ae8cbbe758 | 132 | send[0]=char((data[0]<<7)+(data[1]<<6)+(data[2]<<5)+(data[3]<<4)+(data[4]<<3)+(data[5]<<2)+(data[12]<<1)+(data[13]<<0)); |
M_souta | 1:63ae8cbbe758 | 133 | send[1]=char((data[14]<<7)+(data[15]<<6)+(data[7]<<5)+(data[6]<<4)+(data[22]<<3)+(data[23]<<2)+(data[8]<<1)+(data[9]<<0)); |
M_souta | 1:63ae8cbbe758 | 134 | send[2]=char((data[10]<<7)+(data[11]<<6)+(data[16]<<5)+(data[17]<<4)+(data[18]<<3)+(data[19]<<2)+(data[20]<<1)+(data[21]<<0)); |
M_souta | 1:63ae8cbbe758 | 135 | send[3]=char((data[27]<<7)+(data[26]<<6)+(data[25]<<5)+(data[24]<<4)+(data[40]<<3)+(data[41]<<2)+(data[28]<<1)+(data[29]<<0)); |
M_souta | 1:63ae8cbbe758 | 136 | send[4]=char((data[30]<<7)+(data[31]<<6)+(data[38]<<5)+(data[39]<<4)+(data[32]<<3)+(data[33]<<2)+(data[34]<<1)+(data[35]<<0)); |
M_souta | 1:63ae8cbbe758 | 137 | send[5]=char((data[36]<<7)+(data[37]<<6)+(data[42]<<5)+(data[43]<<4)+(data[44]<<3)+(data[45]<<2)+(data[46]<<1)+(data[47]<<0)); |
M_souta | 1:63ae8cbbe758 | 138 | |
M_souta | 1:63ae8cbbe758 | 139 | uart.putc('S'); |
M_souta | 1:63ae8cbbe758 | 140 | wait_ms(50); |
Ryosei | 0:8aa125a0a09a | 141 | for(int i=0; i<=5; i++) { |
Ryosei | 0:8aa125a0a09a | 142 | uart.putc(send[i]); |
M_souta | 1:63ae8cbbe758 | 143 | wait_ms(50); |
M_souta | 1:63ae8cbbe758 | 144 | //pc.printf("%d : %d\n\r",i,send[i]); |
Ryosei | 0:8aa125a0a09a | 145 | } |
M_souta | 1:63ae8cbbe758 | 146 | uart.putc('F'); |
M_souta | 1:63ae8cbbe758 | 147 | |
Ryosei | 0:8aa125a0a09a | 148 | for(int i=0; i<=47; i++) { |
Ryosei | 0:8aa125a0a09a | 149 | // oled.printf("%d",data[i]); |
Ryosei | 0:8aa125a0a09a | 150 | } |
Ryosei | 0:8aa125a0a09a | 151 | // oled.printf("%d",send[0]); |
Ryosei | 0:8aa125a0a09a | 152 | } |
Ryosei | 0:8aa125a0a09a | 153 | |
Ryosei | 0:8aa125a0a09a | 154 | |
Ryosei | 0:8aa125a0a09a | 155 | void printoled(char printdata[6]) |
Ryosei | 0:8aa125a0a09a | 156 | { |
Ryosei | 0:8aa125a0a09a | 157 | int trimdata[48]; |
Ryosei | 0:8aa125a0a09a | 158 | for(int i=0; i<=47; i++) { |
Ryosei | 0:8aa125a0a09a | 159 | if(i<=7) { |
Ryosei | 0:8aa125a0a09a | 160 | trimdata[i]=((printdata[0] << i) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 161 | } else if(i<=15) { |
Ryosei | 0:8aa125a0a09a | 162 | trimdata[i]=((printdata[1] << i-8) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 163 | } else if(i<=23) { |
Ryosei | 0:8aa125a0a09a | 164 | trimdata[i]=((printdata[2] << i-16) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 165 | } else if(i<=31) { |
Ryosei | 0:8aa125a0a09a | 166 | trimdata[i]=((printdata[3] << i-24) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 167 | } else if(i<=39) { |
Ryosei | 0:8aa125a0a09a | 168 | trimdata[i]=((printdata[4] << i-32) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 169 | } else if(i<=47) { |
Ryosei | 0:8aa125a0a09a | 170 | trimdata[i]=((printdata[5] << i-40) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 171 | } |
Ryosei | 0:8aa125a0a09a | 172 | } |
Ryosei | 0:8aa125a0a09a | 173 | int LAN[8]; |
Ryosei | 0:8aa125a0a09a | 174 | for(int i=0; i<=7; i++) { |
Ryosei | 0:8aa125a0a09a | 175 | LAN[i]=!trimdata[i]*-2+!trimdata[i+2]*2+!trimdata[i+4]*-1+!trimdata[i+5]*1; |
Ryosei | 0:8aa125a0a09a | 176 | } |
M_souta | 1:63ae8cbbe758 | 177 | // oled.locate(0,0); |
M_souta | 1:63ae8cbbe758 | 178 | // oled.printf("%+2d %+2d %+2d %+2d %+2d %+2d %+2d %+2d",LAN[0],LAN[1],LAN[2],LAN[3],LAN[4],LAN[5],LAN[6],LAN[7]); |
M_souta | 1:63ae8cbbe758 | 179 | //pc.printf("%+2d %+2d %+2d %+2d %+2d %+2d %+2d %+2d",LAN[0],LAN[1],LAN[2],LAN[3],LAN[4],LAN[5],LAN[6],LAN[7]); |
Ryosei | 0:8aa125a0a09a | 180 | } |
Ryosei | 0:8aa125a0a09a | 181 | int readdata[48]; |
Ryosei | 0:8aa125a0a09a | 182 | char uartdata[6]; |
Ryosei | 0:8aa125a0a09a | 183 | void Initialize() |
Ryosei | 0:8aa125a0a09a | 184 | { |
Ryosei | 0:8aa125a0a09a | 185 | for(int i=0; i<=47; i++) { |
Ryosei | 0:8aa125a0a09a | 186 | readdata[i]=0; |
Ryosei | 0:8aa125a0a09a | 187 | } |
Ryosei | 0:8aa125a0a09a | 188 | for(int i=0; i<=5; i++) { |
Ryosei | 0:8aa125a0a09a | 189 | uartdata[i]=0x00; |
Ryosei | 0:8aa125a0a09a | 190 | } |
Ryosei | 0:8aa125a0a09a | 191 | } |
Ryosei | 0:8aa125a0a09a | 192 | |
Ryosei | 0:8aa125a0a09a | 193 | int main() |
Ryosei | 0:8aa125a0a09a | 194 | { |
Ryosei | 0:8aa125a0a09a | 195 | pc.baud(9600); |
Ryosei | 0:8aa125a0a09a | 196 | uart.baud(38400); |
Ryosei | 0:8aa125a0a09a | 197 | Initialize(); |
Ryosei | 0:8aa125a0a09a | 198 | uartSE=1; |
Ryosei | 0:8aa125a0a09a | 199 | oled.init(); |
Ryosei | 0:8aa125a0a09a | 200 | oled.setDispFlag(true,false,false); |
Ryosei | 0:8aa125a0a09a | 201 | while(1) { |
Ryosei | 0:8aa125a0a09a | 202 | LineDataRead(readdata); |
Ryosei | 0:8aa125a0a09a | 203 | DataTrim(uartdata,readdata); |
M_souta | 1:63ae8cbbe758 | 204 | // oled.printf("UNTI"); |
Ryosei | 0:8aa125a0a09a | 205 | wait(0.01); |
Ryosei | 0:8aa125a0a09a | 206 | } |
Ryosei | 0:8aa125a0a09a | 207 | } |