ROBOCON NAGANO 2019 linetrace hub

Dependencies:   mbed

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);
    }
}