panel
Dependencies: mbed
main.cpp@0:f3ac2d80d3d0, 2022-09-13 (annotated)
- Committer:
- koki_konishi
- Date:
- Tue Sep 13 06:07:06 2022 +0000
- Revision:
- 0:f3ac2d80d3d0
- Child:
- 1:de511d3d8fd3
robocon2022
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
koki_konishi | 0:f3ac2d80d3d0 | 1 | #include "mbed.h" |
koki_konishi | 0:f3ac2d80d3d0 | 2 | #include <string> |
koki_konishi | 0:f3ac2d80d3d0 | 3 | |
koki_konishi | 0:f3ac2d80d3d0 | 4 | using namespace std; |
koki_konishi | 0:f3ac2d80d3d0 | 5 | |
koki_konishi | 0:f3ac2d80d3d0 | 6 | Ticker timer; |
koki_konishi | 0:f3ac2d80d3d0 | 7 | |
koki_konishi | 0:f3ac2d80d3d0 | 8 | Serial controller_uart(PA_0,PA_1,19200); |
koki_konishi | 0:f3ac2d80d3d0 | 9 | Serial ems_uart(PA_9,PB_7,19200); |
koki_konishi | 0:f3ac2d80d3d0 | 10 | Serial usb_uart(USBTX,USBRX,9600); |
koki_konishi | 0:f3ac2d80d3d0 | 11 | |
koki_konishi | 0:f3ac2d80d3d0 | 12 | DigitalOut EMS(PA_10); |
koki_konishi | 0:f3ac2d80d3d0 | 13 | |
koki_konishi | 0:f3ac2d80d3d0 | 14 | DigitalIn ZONE(PC_5); |
koki_konishi | 0:f3ac2d80d3d0 | 15 | DigitalIn PS1(PC_12); |
koki_konishi | 0:f3ac2d80d3d0 | 16 | DigitalIn PS2(PA_5); |
koki_konishi | 0:f3ac2d80d3d0 | 17 | DigitalIn TS1(PC_11); |
koki_konishi | 0:f3ac2d80d3d0 | 18 | DigitalIn TS2(PC_10); |
koki_konishi | 0:f3ac2d80d3d0 | 19 | DigitalIn TS3(PC_6); |
koki_konishi | 0:f3ac2d80d3d0 | 20 | DigitalIn TS4(PC_8); |
koki_konishi | 0:f3ac2d80d3d0 | 21 | |
koki_konishi | 0:f3ac2d80d3d0 | 22 | volatile int cnt = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 23 | volatile int con_status = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 24 | volatile unsigned char tx_data[11] = { 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x00 }; |
koki_konishi | 0:f3ac2d80d3d0 | 25 | volatile unsigned char dualshock_data[10] = { 0x00, 0x00, 0x00, 0x00, 0x80, 0x80, 0x80, 0x80 }; |
koki_konishi | 0:f3ac2d80d3d0 | 26 | volatile unsigned char dualshock_data_number = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 27 | volatile unsigned char dualshock_cmd_getflag = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 28 | |
koki_konishi | 0:f3ac2d80d3d0 | 29 | //遠隔非常停止受信処理 |
koki_konishi | 0:f3ac2d80d3d0 | 30 | void recieve_ems(){ |
koki_konishi | 0:f3ac2d80d3d0 | 31 | uint8_t rxdata = ems_uart.getc(); |
koki_konishi | 0:f3ac2d80d3d0 | 32 | |
koki_konishi | 0:f3ac2d80d3d0 | 33 | if(rxdata == 0xEE){ |
koki_konishi | 0:f3ac2d80d3d0 | 34 | cnt = 50; |
koki_konishi | 0:f3ac2d80d3d0 | 35 | } |
koki_konishi | 0:f3ac2d80d3d0 | 36 | } |
koki_konishi | 0:f3ac2d80d3d0 | 37 | |
koki_konishi | 0:f3ac2d80d3d0 | 38 | void time() { |
koki_konishi | 0:f3ac2d80d3d0 | 39 | if(dualshock_cmd_getflag & 0x02) { |
koki_konishi | 0:f3ac2d80d3d0 | 40 | dualshock_cmd_getflag = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 41 | |
koki_konishi | 0:f3ac2d80d3d0 | 42 | tx_data[1] = 0x00; |
koki_konishi | 0:f3ac2d80d3d0 | 43 | tx_data[2] = 0x00; |
koki_konishi | 0:f3ac2d80d3d0 | 44 | tx_data[3] = 0x00; |
koki_konishi | 0:f3ac2d80d3d0 | 45 | tx_data[4] = 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 46 | tx_data[5] = 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 47 | tx_data[6] = 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 48 | tx_data[7] = 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 49 | |
koki_konishi | 0:f3ac2d80d3d0 | 50 | dualshock_data[1] = 0x00; |
koki_konishi | 0:f3ac2d80d3d0 | 51 | dualshock_data[2] = 0x00; |
koki_konishi | 0:f3ac2d80d3d0 | 52 | dualshock_data[3] = 0x00; |
koki_konishi | 0:f3ac2d80d3d0 | 53 | dualshock_data[4] = 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 54 | dualshock_data[5] = 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 55 | dualshock_data[6] = 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 56 | dualshock_data[7] = 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 57 | |
koki_konishi | 0:f3ac2d80d3d0 | 58 | con_status = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 59 | }else |
koki_konishi | 0:f3ac2d80d3d0 | 60 | dualshock_cmd_getflag |= 0x02; |
koki_konishi | 0:f3ac2d80d3d0 | 61 | } |
koki_konishi | 0:f3ac2d80d3d0 | 62 | |
koki_konishi | 0:f3ac2d80d3d0 | 63 | //JETSON送信処理 |
koki_konishi | 0:f3ac2d80d3d0 | 64 | void send_tx_data(){ |
koki_konishi | 0:f3ac2d80d3d0 | 65 | usb_uart.putc('C'); |
koki_konishi | 0:f3ac2d80d3d0 | 66 | for(int i = 1; i < 9; i++){ |
koki_konishi | 0:f3ac2d80d3d0 | 67 | usb_uart.putc(tx_data[i]); |
koki_konishi | 0:f3ac2d80d3d0 | 68 | } |
koki_konishi | 0:f3ac2d80d3d0 | 69 | usb_uart.putc('E'); |
koki_konishi | 0:f3ac2d80d3d0 | 70 | } |
koki_konishi | 0:f3ac2d80d3d0 | 71 | |
koki_konishi | 0:f3ac2d80d3d0 | 72 | //コントローラー受信処理 |
koki_konishi | 0:f3ac2d80d3d0 | 73 | void read_controller_data() { |
koki_konishi | 0:f3ac2d80d3d0 | 74 | unsigned char rxdata; |
koki_konishi | 0:f3ac2d80d3d0 | 75 | |
koki_konishi | 0:f3ac2d80d3d0 | 76 | rxdata = controller_uart.getc(); |
koki_konishi | 0:f3ac2d80d3d0 | 77 | |
koki_konishi | 0:f3ac2d80d3d0 | 78 | if (dualshock_data_number == 0 && rxdata == 'S') |
koki_konishi | 0:f3ac2d80d3d0 | 79 | dualshock_data_number++; |
koki_konishi | 0:f3ac2d80d3d0 | 80 | else if (dualshock_data_number >= 1 && dualshock_data_number <= 7) { |
koki_konishi | 0:f3ac2d80d3d0 | 81 | if (dualshock_data_number <= 3) |
koki_konishi | 0:f3ac2d80d3d0 | 82 | dualshock_data[dualshock_data_number] = ~rxdata; |
koki_konishi | 0:f3ac2d80d3d0 | 83 | else |
koki_konishi | 0:f3ac2d80d3d0 | 84 | dualshock_data[dualshock_data_number] = rxdata; |
koki_konishi | 0:f3ac2d80d3d0 | 85 | dualshock_data_number++; |
koki_konishi | 0:f3ac2d80d3d0 | 86 | } |
koki_konishi | 0:f3ac2d80d3d0 | 87 | else if (dualshock_data_number == 8 && rxdata == 'E') { |
koki_konishi | 0:f3ac2d80d3d0 | 88 | dualshock_cmd_getflag = 0x01; |
koki_konishi | 0:f3ac2d80d3d0 | 89 | for(int i = 0; i < 8; i++){ |
koki_konishi | 0:f3ac2d80d3d0 | 90 | tx_data[i] = dualshock_data[i]; |
koki_konishi | 0:f3ac2d80d3d0 | 91 | } |
koki_konishi | 0:f3ac2d80d3d0 | 92 | con_status = 1; |
koki_konishi | 0:f3ac2d80d3d0 | 93 | dualshock_data_number = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 94 | } |
koki_konishi | 0:f3ac2d80d3d0 | 95 | } |
koki_konishi | 0:f3ac2d80d3d0 | 96 | |
koki_konishi | 0:f3ac2d80d3d0 | 97 | //main |
koki_konishi | 0:f3ac2d80d3d0 | 98 | int main(){ |
koki_konishi | 0:f3ac2d80d3d0 | 99 | int c = 9; |
koki_konishi | 0:f3ac2d80d3d0 | 100 | |
koki_konishi | 0:f3ac2d80d3d0 | 101 | ZONE.mode(PullUp); |
koki_konishi | 0:f3ac2d80d3d0 | 102 | PS1.mode(PullUp); |
koki_konishi | 0:f3ac2d80d3d0 | 103 | PS2.mode(PullUp); |
koki_konishi | 0:f3ac2d80d3d0 | 104 | TS1.mode(PullUp); |
koki_konishi | 0:f3ac2d80d3d0 | 105 | TS2.mode(PullUp); |
koki_konishi | 0:f3ac2d80d3d0 | 106 | TS3.mode(PullUp); |
koki_konishi | 0:f3ac2d80d3d0 | 107 | TS4.mode(PullUp); |
koki_konishi | 0:f3ac2d80d3d0 | 108 | |
koki_konishi | 0:f3ac2d80d3d0 | 109 | timer.attach_us(&time, 100000); |
koki_konishi | 0:f3ac2d80d3d0 | 110 | |
koki_konishi | 0:f3ac2d80d3d0 | 111 | controller_uart.attach(&read_controller_data, Serial::RxIrq); //受信割り込み |
koki_konishi | 0:f3ac2d80d3d0 | 112 | ems_uart.attach(&recieve_ems, Serial::RxIrq); |
koki_konishi | 0:f3ac2d80d3d0 | 113 | |
koki_konishi | 0:f3ac2d80d3d0 | 114 | while(1){ |
koki_konishi | 0:f3ac2d80d3d0 | 115 | if(cnt){ |
koki_konishi | 0:f3ac2d80d3d0 | 116 | EMS = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 117 | cnt--; |
koki_konishi | 0:f3ac2d80d3d0 | 118 | }else{ |
koki_konishi | 0:f3ac2d80d3d0 | 119 | EMS = 1; |
koki_konishi | 0:f3ac2d80d3d0 | 120 | } |
koki_konishi | 0:f3ac2d80d3d0 | 121 | |
koki_konishi | 0:f3ac2d80d3d0 | 122 | if(con_status){ |
koki_konishi | 0:f3ac2d80d3d0 | 123 | tx_data[8] |= 0x80; |
koki_konishi | 0:f3ac2d80d3d0 | 124 | } |
koki_konishi | 0:f3ac2d80d3d0 | 125 | if(!ZONE){ |
koki_konishi | 0:f3ac2d80d3d0 | 126 | tx_data[8] |= 0x40; |
koki_konishi | 0:f3ac2d80d3d0 | 127 | } |
koki_konishi | 0:f3ac2d80d3d0 | 128 | if(!PS1){ |
koki_konishi | 0:f3ac2d80d3d0 | 129 | tx_data[8] |= 0x20; |
koki_konishi | 0:f3ac2d80d3d0 | 130 | } |
koki_konishi | 0:f3ac2d80d3d0 | 131 | if(!PS2){ |
koki_konishi | 0:f3ac2d80d3d0 | 132 | tx_data[8] |= 0x10; |
koki_konishi | 0:f3ac2d80d3d0 | 133 | } |
koki_konishi | 0:f3ac2d80d3d0 | 134 | if(!TS1){ |
koki_konishi | 0:f3ac2d80d3d0 | 135 | tx_data[8] |= 0x08; |
koki_konishi | 0:f3ac2d80d3d0 | 136 | } |
koki_konishi | 0:f3ac2d80d3d0 | 137 | if(!TS2){ |
koki_konishi | 0:f3ac2d80d3d0 | 138 | tx_data[8] |= 0x04; |
koki_konishi | 0:f3ac2d80d3d0 | 139 | } |
koki_konishi | 0:f3ac2d80d3d0 | 140 | if(!TS3){ |
koki_konishi | 0:f3ac2d80d3d0 | 141 | tx_data[8] |= 0x02; |
koki_konishi | 0:f3ac2d80d3d0 | 142 | } |
koki_konishi | 0:f3ac2d80d3d0 | 143 | if(!TS4){ |
koki_konishi | 0:f3ac2d80d3d0 | 144 | tx_data[8] |= 0x01; |
koki_konishi | 0:f3ac2d80d3d0 | 145 | } |
koki_konishi | 0:f3ac2d80d3d0 | 146 | |
koki_konishi | 0:f3ac2d80d3d0 | 147 | if(!cnt){ |
koki_konishi | 0:f3ac2d80d3d0 | 148 | send_tx_data(); |
koki_konishi | 0:f3ac2d80d3d0 | 149 | tx_data[8] = 0; |
koki_konishi | 0:f3ac2d80d3d0 | 150 | c = 9; |
koki_konishi | 0:f3ac2d80d3d0 | 151 | }else{ |
koki_konishi | 0:f3ac2d80d3d0 | 152 | c--; |
koki_konishi | 0:f3ac2d80d3d0 | 153 | } |
koki_konishi | 0:f3ac2d80d3d0 | 154 | |
koki_konishi | 0:f3ac2d80d3d0 | 155 | wait_ms(10); |
koki_konishi | 0:f3ac2d80d3d0 | 156 | } |
koki_konishi | 0:f3ac2d80d3d0 | 157 | } |