Miya Miyagawa
/
LineTraceHub
ROBOCON NAGANO 2019 linetrace hub
main.cpp
- Committer:
- Ryosei
- Date:
- 2019-07-20
- Revision:
- 0:05a2a33c69a2
File content as of revision 0:05a2a33c69a2:
#include "mbed.h" /* マルチプレクサ1 selectピン select1_0(1桁目) select1_1(2桁目) select1_2(3桁目) select1_3(4桁目) 読み取りピン Z0 マルチプレクサ2 selectピン select2_0(1桁目) select2_1(2桁目) select2_2(3桁目) select2_3(4桁目) 読み取りピン Z1 マルチプレクサ3 selectピン select3_0(1桁目) select3_1(2桁目) select3_2(3桁目) select3_3(4桁目) 読み取りピン Z2 */ #define select1_0 D12 #define select1_1 A1 #define select1_2 D13 #define select1_3 A0 #define select2_0 D7 #define select2_1 D8 #define select2_2 D9 #define select2_3 D10 #define select3_0 A3 #define select3_1 A4 #define select3_2 A6 #define select3_3 A5 #define Z0 PB_5 #define Z1 PB_1 #define Z2 PA_3 DigitalIn yomitori[]={ DigitalIn(Z0), DigitalIn(Z1), DigitalIn(Z2), }; DigitalOut select1[]={ DigitalOut(select1_0), DigitalOut(select1_1), DigitalOut(select1_2), DigitalOut(select1_3), }; DigitalOut select2[]={ DigitalOut(select2_0), DigitalOut(select2_1), DigitalOut(select2_2), DigitalOut(select2_3), }; DigitalOut select3[]={ DigitalOut(select3_0), DigitalOut(select3_1), DigitalOut(select3_2), DigitalOut(select3_3), }; Serial uart(D1,D0); Serial pc(USBTX,USBRX); void PinSelect(int pin){ bool selectnum[16][4]={ {0,0,0,0}, {0,0,0,1}, {0,0,1,0}, {0,0,1,1}, {0,1,0,0}, {0,1,0,1}, {0,1,1,0}, {0,1,1,1}, {1,0,0,0}, {1,0,0,1}, {1,0,1,0}, {1,0,1,1}, {1,1,0,0}, {1,1,0,1}, {1,1,1,0}, {1,1,1,1}, }; if(pin<=15){ select1[0]=selectnum[pin][3]; select1[1]=selectnum[pin][2]; select1[2]=selectnum[pin][1]; select1[3]=selectnum[pin][0]; for(int i=0;i<=3;i++){ select2[i]=0; select3[i]=0; } }else if(pin<=31){ select2[0]=selectnum[pin-16][3]; select2[1]=selectnum[pin-16][2]; select2[2]=selectnum[pin-16][1]; select2[3]=selectnum[pin-16][0]; for(int i=0;i<=3;i++){ select1[i]=0; select3[i]=0; } }else if(pin<=47){ select3[0]=selectnum[pin-32][3]; select3[1]=selectnum[pin-32][2]; select3[2]=selectnum[pin-32][1]; select3[3]=selectnum[pin-32][0]; for(int i=0;i<=3;i++){ select1[i]=0; select2[i]=0; } } } void LineDataRead(int read[48]){ for(int i=0;i<=47;i++){ PinSelect(i); if(i<=15){ read[i]=yomitori[0]; }else if(i<=31){ read[i]=yomitori[1]; }else if(i<=47){ read[i]=yomitori[2]; } } } void DataTrim(char send[6],int data[48]){ send[0]=char((data[0]<<7)+(data[1]<<6)+(data[2]<<5)+(data[3]<<4)+(data[4]<<3)+(data[5]<<2)+(data[6]<<1)+(data[7]<<0)); send[1]=char((data[8]<<7)+(data[9]<<6)+(data[10]<<5)+(data[11]<<4)+(data[12]<<3)+(data[13]<<2)+(data[14]<<1)+(data[15]<<0)); send[2]=char((data[16]<<7)+(data[17]<<6)+(data[18]<<5)+(data[19]<<4)+(data[20]<<3)+(data[21]<<2)+(data[22]<<1)+(data[23]<<0)); send[3]=char((data[24]<<7)+(data[25]<<6)+(data[26]<<5)+(data[27]<<4)+(data[28]<<3)+(data[29]<<2)+(data[30]<<1)+(data[31]<<0)); send[4]=char((data[32]<<7)+(data[33]<<6)+(data[34]<<5)+(data[35]<<4)+(data[36]<<3)+(data[37]<<2)+(data[38]<<1)+(data[39]<<0)); send[5]=char((data[40]<<7)+(data[41]<<6)+(data[42]<<5)+(data[43]<<4)+(data[44]<<3)+(data[45]<<2)+(data[46]<<1)+(data[47]<<0)); for(int i=0;i<=5;i++){ uart.putc(send[i]); } for(int i=0;i<=47;i++){ pc.printf("%d",data[i]); } pc.printf("\n"); } int readdata[48]; char uartdata[6]; void Initialize(){ for(int i=0;i<=47;i++){ readdata[i]=0; } for(int i=0;i<=5;i++){ uartdata[i]=0x00; } } DigitalOut uartSE(A7); int main() { pc.baud(9600); uart.baud(38400); Initialize(); uartSE=1; while(1) { LineDataRead(readdata); DataTrim(uartdata,readdata); wait(0.01); } }