Miya Miyagawa
/
NEW_LineTraceHub_2
aa
main.cpp@0:8aa125a0a09a, 2019-08-30 (annotated)
- Committer:
- Ryosei
- Date:
- Fri Aug 30 06:19:52 2019 +0000
- Revision:
- 0:8aa125a0a09a
- Child:
- 1:63ae8cbbe758
aa
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 | }; |
Ryosei | 0:8aa125a0a09a | 49 | Serial uart(PA_9,PA_10); |
Ryosei | 0:8aa125a0a09a | 50 | Serial pc(USBTX,USBRX); |
Ryosei | 0:8aa125a0a09a | 51 | I2C i2c(SDA,SCL); |
Ryosei | 0:8aa125a0a09a | 52 | SO1602A oled(i2c,0x78); |
Ryosei | 0:8aa125a0a09a | 53 | |
Ryosei | 0:8aa125a0a09a | 54 | void PinSelect(int pin) |
Ryosei | 0:8aa125a0a09a | 55 | { |
Ryosei | 0:8aa125a0a09a | 56 | bool selectnum[16][4]= { |
Ryosei | 0:8aa125a0a09a | 57 | {0,0,0,0}, |
Ryosei | 0:8aa125a0a09a | 58 | {0,0,0,1}, |
Ryosei | 0:8aa125a0a09a | 59 | {0,0,1,0}, |
Ryosei | 0:8aa125a0a09a | 60 | {0,0,1,1}, |
Ryosei | 0:8aa125a0a09a | 61 | {0,1,0,0}, |
Ryosei | 0:8aa125a0a09a | 62 | {0,1,0,1}, |
Ryosei | 0:8aa125a0a09a | 63 | {0,1,1,0}, |
Ryosei | 0:8aa125a0a09a | 64 | {0,1,1,1}, |
Ryosei | 0:8aa125a0a09a | 65 | {1,0,0,0}, |
Ryosei | 0:8aa125a0a09a | 66 | {1,0,0,1}, |
Ryosei | 0:8aa125a0a09a | 67 | {1,0,1,0}, |
Ryosei | 0:8aa125a0a09a | 68 | {1,0,1,1}, |
Ryosei | 0:8aa125a0a09a | 69 | {1,1,0,0}, |
Ryosei | 0:8aa125a0a09a | 70 | {1,1,0,1}, |
Ryosei | 0:8aa125a0a09a | 71 | {1,1,1,0}, |
Ryosei | 0:8aa125a0a09a | 72 | {1,1,1,1}, |
Ryosei | 0:8aa125a0a09a | 73 | }; |
Ryosei | 0:8aa125a0a09a | 74 | if(pin<=15) { |
Ryosei | 0:8aa125a0a09a | 75 | select1[0]=selectnum[pin][3]; |
Ryosei | 0:8aa125a0a09a | 76 | select1[1]=selectnum[pin][2]; |
Ryosei | 0:8aa125a0a09a | 77 | select1[2]=selectnum[pin][1]; |
Ryosei | 0:8aa125a0a09a | 78 | select1[3]=selectnum[pin][0]; |
Ryosei | 0:8aa125a0a09a | 79 | for(int i=0; i<=3; i++) { |
Ryosei | 0:8aa125a0a09a | 80 | select2[i]=0; |
Ryosei | 0:8aa125a0a09a | 81 | select3[i]=0; |
Ryosei | 0:8aa125a0a09a | 82 | } |
Ryosei | 0:8aa125a0a09a | 83 | } else if(pin<=31) { |
Ryosei | 0:8aa125a0a09a | 84 | select2[0]=selectnum[pin-16][3]; |
Ryosei | 0:8aa125a0a09a | 85 | select2[1]=selectnum[pin-16][2]; |
Ryosei | 0:8aa125a0a09a | 86 | select2[2]=selectnum[pin-16][1]; |
Ryosei | 0:8aa125a0a09a | 87 | select2[3]=selectnum[pin-16][0]; |
Ryosei | 0:8aa125a0a09a | 88 | for(int i=0; i<=3; i++) { |
Ryosei | 0:8aa125a0a09a | 89 | select1[i]=0; |
Ryosei | 0:8aa125a0a09a | 90 | select3[i]=0; |
Ryosei | 0:8aa125a0a09a | 91 | } |
Ryosei | 0:8aa125a0a09a | 92 | } else if(pin<=47) { |
Ryosei | 0:8aa125a0a09a | 93 | select3[0]=selectnum[pin-32][3]; |
Ryosei | 0:8aa125a0a09a | 94 | select3[1]=selectnum[pin-32][2]; |
Ryosei | 0:8aa125a0a09a | 95 | select3[2]=selectnum[pin-32][1]; |
Ryosei | 0:8aa125a0a09a | 96 | select3[3]=selectnum[pin-32][0]; |
Ryosei | 0:8aa125a0a09a | 97 | for(int i=0; i<=3; i++) { |
Ryosei | 0:8aa125a0a09a | 98 | select1[i]=0; |
Ryosei | 0:8aa125a0a09a | 99 | select2[i]=0; |
Ryosei | 0:8aa125a0a09a | 100 | } |
Ryosei | 0:8aa125a0a09a | 101 | } |
Ryosei | 0:8aa125a0a09a | 102 | } |
Ryosei | 0:8aa125a0a09a | 103 | |
Ryosei | 0:8aa125a0a09a | 104 | void LineDataRead(int read[48]) |
Ryosei | 0:8aa125a0a09a | 105 | { |
Ryosei | 0:8aa125a0a09a | 106 | float anal[48]; |
Ryosei | 0:8aa125a0a09a | 107 | for(int i=0; i<=47; i++) { |
Ryosei | 0:8aa125a0a09a | 108 | PinSelect(i); |
Ryosei | 0:8aa125a0a09a | 109 | if(i<=15) { |
Ryosei | 0:8aa125a0a09a | 110 | anal[i]=y1.read(); |
Ryosei | 0:8aa125a0a09a | 111 | } else if(i<=31) { |
Ryosei | 0:8aa125a0a09a | 112 | anal[i]=y2.read(); |
Ryosei | 0:8aa125a0a09a | 113 | } else if(i<=47) { |
Ryosei | 0:8aa125a0a09a | 114 | anal[i]=y3.read(); |
Ryosei | 0:8aa125a0a09a | 115 | } |
Ryosei | 0:8aa125a0a09a | 116 | // pc.printf("%f ",anal[i]); |
Ryosei | 0:8aa125a0a09a | 117 | |
Ryosei | 0:8aa125a0a09a | 118 | if(anal[i]<0.8f) { |
Ryosei | 0:8aa125a0a09a | 119 | read[i]=0; |
Ryosei | 0:8aa125a0a09a | 120 | } else { |
Ryosei | 0:8aa125a0a09a | 121 | read[i]=1; |
Ryosei | 0:8aa125a0a09a | 122 | } |
Ryosei | 0:8aa125a0a09a | 123 | // oled.printf("%.3f",read[i]); |
Ryosei | 0:8aa125a0a09a | 124 | |
Ryosei | 0:8aa125a0a09a | 125 | } |
Ryosei | 0:8aa125a0a09a | 126 | // oled.printf("%d %f",read[0],anal[0]); |
Ryosei | 0:8aa125a0a09a | 127 | } |
Ryosei | 0:8aa125a0a09a | 128 | |
Ryosei | 0:8aa125a0a09a | 129 | void DataTrim(char send[6],int data[48]) |
Ryosei | 0:8aa125a0a09a | 130 | { |
Ryosei | 0:8aa125a0a09a | 131 | send[0]=char((data[0]<<7)+(data[5]<<6)+(data[1]<<5)+(data[4]<<4)+(data[2]<<3)+(data[3]<<2)+(data[12]<<1)+(data[6]<<0)); |
Ryosei | 0:8aa125a0a09a | 132 | send[1]=char((data[13]<<7)+(data[7]<<6)+(data[14]<<5)+(data[15]<<4)+(data[22]<<3)+(data[11]<<2)+(data[23]<<1)+(data[10]<<0)); |
Ryosei | 0:8aa125a0a09a | 133 | send[2]=char((data[8]<<7)+(data[9]<<6)+(data[16]<<5)+(data[21]<<4)+(data[17]<<3)+(data[20]<<2)+(data[18]<<1)+(data[19]<<0)); |
Ryosei | 0:8aa125a0a09a | 134 | send[3]=char((data[27]<<7)+(data[41]<<6)+(data[37]<<5)+(data[40]<<4)+(data[25]<<3)+(data[24]<<2)+(data[28]<<1)+(data[39]<<0)); |
Ryosei | 0:8aa125a0a09a | 135 | send[4]=char((data[29]<<7)+(data[38]<<6)+(data[30]<<5)+(data[31]<<4)+(data[32]<<3)+(data[37]<<2)+(data[33]<<1)+(data[36]<<0)); |
Ryosei | 0:8aa125a0a09a | 136 | send[5]=char((data[34]<<7)+(data[35]<<6)+(data[42]<<5)+(data[47]<<4)+(data[43]<<3)+(data[46]<<2)+(data[44]<<1)+(data[45]<<0)); |
Ryosei | 0:8aa125a0a09a | 137 | for(int i=0; i<=5; i++) { |
Ryosei | 0:8aa125a0a09a | 138 | uart.putc(send[i]); |
Ryosei | 0:8aa125a0a09a | 139 | } |
Ryosei | 0:8aa125a0a09a | 140 | for(int i=0; i<=47; i++) { |
Ryosei | 0:8aa125a0a09a | 141 | // oled.printf("%d",data[i]); |
Ryosei | 0:8aa125a0a09a | 142 | } |
Ryosei | 0:8aa125a0a09a | 143 | // oled.printf("%d",send[0]); |
Ryosei | 0:8aa125a0a09a | 144 | } |
Ryosei | 0:8aa125a0a09a | 145 | |
Ryosei | 0:8aa125a0a09a | 146 | |
Ryosei | 0:8aa125a0a09a | 147 | void printoled(char printdata[6]) |
Ryosei | 0:8aa125a0a09a | 148 | { |
Ryosei | 0:8aa125a0a09a | 149 | int trimdata[48]; |
Ryosei | 0:8aa125a0a09a | 150 | for(int i=0; i<=47; i++) { |
Ryosei | 0:8aa125a0a09a | 151 | if(i<=7) { |
Ryosei | 0:8aa125a0a09a | 152 | trimdata[i]=((printdata[0] << i) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 153 | } else if(i<=15) { |
Ryosei | 0:8aa125a0a09a | 154 | trimdata[i]=((printdata[1] << i-8) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 155 | } else if(i<=23) { |
Ryosei | 0:8aa125a0a09a | 156 | trimdata[i]=((printdata[2] << i-16) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 157 | } else if(i<=31) { |
Ryosei | 0:8aa125a0a09a | 158 | trimdata[i]=((printdata[3] << i-24) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 159 | } else if(i<=39) { |
Ryosei | 0:8aa125a0a09a | 160 | trimdata[i]=((printdata[4] << i-32) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 161 | } else if(i<=47) { |
Ryosei | 0:8aa125a0a09a | 162 | trimdata[i]=((printdata[5] << i-40) & 0b10000000)>>7; |
Ryosei | 0:8aa125a0a09a | 163 | } |
Ryosei | 0:8aa125a0a09a | 164 | } |
Ryosei | 0:8aa125a0a09a | 165 | int LAN[8]; |
Ryosei | 0:8aa125a0a09a | 166 | for(int i=0; i<=7; i++) { |
Ryosei | 0:8aa125a0a09a | 167 | LAN[i]=!trimdata[i]*-2+!trimdata[i+2]*2+!trimdata[i+4]*-1+!trimdata[i+5]*1; |
Ryosei | 0:8aa125a0a09a | 168 | } |
Ryosei | 0:8aa125a0a09a | 169 | oled.locate(0,0); |
Ryosei | 0:8aa125a0a09a | 170 | 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]); |
Ryosei | 0:8aa125a0a09a | 171 | } |
Ryosei | 0:8aa125a0a09a | 172 | int readdata[48]; |
Ryosei | 0:8aa125a0a09a | 173 | char uartdata[6]; |
Ryosei | 0:8aa125a0a09a | 174 | void Initialize() |
Ryosei | 0:8aa125a0a09a | 175 | { |
Ryosei | 0:8aa125a0a09a | 176 | for(int i=0; i<=47; i++) { |
Ryosei | 0:8aa125a0a09a | 177 | readdata[i]=0; |
Ryosei | 0:8aa125a0a09a | 178 | } |
Ryosei | 0:8aa125a0a09a | 179 | for(int i=0; i<=5; i++) { |
Ryosei | 0:8aa125a0a09a | 180 | uartdata[i]=0x00; |
Ryosei | 0:8aa125a0a09a | 181 | } |
Ryosei | 0:8aa125a0a09a | 182 | } |
Ryosei | 0:8aa125a0a09a | 183 | |
Ryosei | 0:8aa125a0a09a | 184 | int main() |
Ryosei | 0:8aa125a0a09a | 185 | { |
Ryosei | 0:8aa125a0a09a | 186 | pc.baud(9600); |
Ryosei | 0:8aa125a0a09a | 187 | uart.baud(38400); |
Ryosei | 0:8aa125a0a09a | 188 | Initialize(); |
Ryosei | 0:8aa125a0a09a | 189 | uartSE=1; |
Ryosei | 0:8aa125a0a09a | 190 | oled.init(); |
Ryosei | 0:8aa125a0a09a | 191 | oled.setDispFlag(true,false,false); |
Ryosei | 0:8aa125a0a09a | 192 | while(1) { |
Ryosei | 0:8aa125a0a09a | 193 | LineDataRead(readdata); |
Ryosei | 0:8aa125a0a09a | 194 | DataTrim(uartdata,readdata); |
Ryosei | 0:8aa125a0a09a | 195 | printoled(uartdata); |
Ryosei | 0:8aa125a0a09a | 196 | wait(0.01); |
Ryosei | 0:8aa125a0a09a | 197 | } |
Ryosei | 0:8aa125a0a09a | 198 | } |