Diff: main.cpp

Revision:

0:35bb9af90b04

diff -r 000000000000 -r 35bb9af90b04 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Sep 21 11:34:45 2019 +0000
@@ -0,0 +1,202 @@
+#include "mbed.h"
+#include "Mycan.h"
+
+Mycan can(p30, p29, 100000);   //// CAN (PinName rd, PinName td)
+
+DigitalOut LED[4] = 
+{
+    DigitalOut(LED1),
+    DigitalOut(LED2),
+    DigitalOut(LED3),
+    DigitalOut(LED4),
+};
+
+DigitalOut led_R(p14);
+DigitalOut led_B(p18);
+
+DigitalIn sw[11] = 
+{
+    DigitalIn(p5),
+    DigitalIn(p6),
+    DigitalIn(p8),
+    DigitalIn(p10),
+    DigitalIn(p12),
+    DigitalIn(p7),
+    DigitalIn(p9),
+    DigitalIn(p11),
+    DigitalIn(p13),
+    DigitalIn(p16),
+    DigitalIn(p15)
+};
+
+Ticker ticker;
+Serial pc(USBTX, USBRX);
+
+void canFnc();
+void readLineData();
+bool T_sw[4], B_sw[4], S_sw, Z_sw, M_sw;
+short int send_val[2], line_data[2];
+
+int main() 
+{
+    for(int i=0; i<11; i++)
+        sw[i].mode(PullUp);
+    ticker.attach(&canFnc, 0.0065);
+    while(1) 
+    {
+        S_sw = sw[0];
+        pc.printf("%d\t", S_sw);
+        for(int i=0; i<4; i++) {
+            B_sw[i] = sw[i+1];
+            pc.printf("%d\t", B_sw[i]);
+        }
+        for(int i=0; i<4; i++) {
+            T_sw[i] = sw[i+5];
+            pc.printf("%d\t", T_sw[i]);
+        }
+        Z_sw = sw[9];
+        if(Z_sw) {
+            led_R = 1;
+            led_B = 0;
+        } else {
+            led_R = 0;
+            led_B = 1;
+        }
+        M_sw = sw[10];
+        
+        pc.printf("%d\t", Z_sw);
+        pc.printf("%d\t", M_sw);
+        pc.printf("%d\t", send_val[0]);
+        pc.printf("%d\t", send_val[1]);
+        pc.printf("%d\t", line_data[0]);
+        pc.printf("%d\t", line_data[1]);
+        pc.printf("\n");
+    }
+}
+
+void canFnc() 
+{
+    send_val[0] = send_val[1] = 0;
+    if(S_sw) 
+        send_val[0] += 64;
+    if(B_sw[0])
+        send_val[0] += 32;
+    if(B_sw[1])
+        send_val[0] += 16;
+    if(B_sw[2])
+        send_val[0] += 8;
+    if(B_sw[3])
+        send_val[0] += 4;
+    if(!T_sw[0])
+        send_val[0] += 2;
+    if(!T_sw[1])
+        send_val[0] += 1;
+    if(!T_sw[2])
+        send_val[1] += 8;
+    if(!T_sw[3])
+        send_val[1] += 4;
+    if(Z_sw)
+        send_val[1] += 2;
+    if(M_sw)
+        send_val[1] += 1;
+    
+    readLineData();
+    
+    can.setI(2, 0, (short int)(send_val[0]));
+    can.setI(2, 1, (short int)(send_val[1]));
+    can.setI(2, 2, (short int)(line_data[0]));
+    can.setI(2, 3, (short int)(line_data[1]));
+    LED[0] = can.send();
+}
+
+void readLineData() 
+{
+    int read_line_data[4];
+    
+    for(int i = 0; i < 4; i++) {
+        can.readI();
+        read_line_data[i] = can.get(i+6, 0);
+    }
+    
+    int residue_data, true_num[4], true_all[4];
+    
+    for(int i = 0; i < 4; i++) 
+    {
+        residue_data = read_line_data[i];
+        true_all[i] = true_num[i] = 0;
+        
+        if(residue_data < 0) {
+            if(residue_data == -200) 
+                residue_data = 0;
+            else residue_data *= -1;
+            
+            true_num[i] += 1;
+            if(i == 1 || i == 3)
+                true_all[i] += 7 + 8;
+            else true_all[i] += 7;
+        }
+        
+        if(residue_data >= 64) {
+            residue_data -= 64;
+            true_num[i] += 1;
+            if(i == 1 || i == 3)
+                true_all[i] += 6 + 8;
+            else true_all[i] += 6;
+        }
+        
+        if(residue_data >= 32) {
+            residue_data -= 32;
+            true_num[i] += 1;
+            if(i == 1 || i == 3)
+                true_all[i] += 5 + 8;
+            else true_all[i] += 5;
+        }
+        
+        if(residue_data >= 16) {
+            residue_data -= 16;
+            true_num[i] += 1;
+            if(i == 1 || i == 3)
+                true_all[i] += 4 + 8;
+            else true_all[i] += 4;
+        }
+        
+        if(residue_data >= 8) {
+            residue_data -= 8;
+            true_num[i] += 1;
+            if(i == 1 || i == 3)
+                true_all[i] += 3 + 8;
+            else true_all[i] += 3;
+        }
+        
+        if(residue_data >= 4) {
+            residue_data -= 4;
+            true_num[i] += 1;
+            if(i == 1 || i == 3)
+                true_all[i] += 2 + 8;
+            else true_all[i] += 2;
+        }
+        
+        if(residue_data >= 2) {
+            residue_data -= 2;
+            true_num[i] += 1;
+            if(i == 1 || i == 3)
+                true_all[i] += 1 + 8;
+            else true_all[i] += 1;
+        }
+        
+        if(residue_data >= 1) {
+            residue_data -= 1;
+            true_num[i] += 1;
+            if(i == 1 || i == 3)
+                true_all[i] += 8;
+        }
+    }
+    
+    if(true_all[0] + true_all[1] != 0 || true_num[0] + true_num[1] != 0)
+        line_data[0] = (true_all[0] + true_all[1]) / (true_num[0] + true_num[1]);
+    else line_data[0] = 0;
+    
+    if(true_all[2] + true_all[3] != 0 || true_num[2] + true_num[3] != 0)
+        line_data[1] = (true_all[2] + true_all[3]) / (true_num[2] + true_num[3]);
+    else line_data[1] = 0;
+}
\ No newline at end of file