Diff: main.cpp

Revision:

9:8004a5b72777

Parent:

2:41445b778ae0

--- a/main.cpp	Sat Dec 05 11:23:47 2015 +0000
+++ b/main.cpp	Sat Jan 23 10:42:17 2016 +0000
@@ -1,60 +1,148 @@
+//中央操舵プログラム
 #include "mbed.h"
 
-Serial rs485(p9,p10);
+#define WAIT_LOOP_TIME 0.15
+
+Serial rs485R(p9,p10);
+Serial rs485L(p13,p14);
+Serial toKeikiSerial(p28,p27);
 Serial pc(USBTX,USBRX);
-DigitalOut Driver(p5);
-DigitalIn Switch(p20);
-Ticker CheckData;
-AnalogIn val1(p16);
-AnalogIn val2(p17);
+DigitalOut DriverR(p5);
+DigitalOut DriverL(p6);
+Ticker CheckDataR;
+Ticker CheckDataL;
+DigitalIn eruronR(p7);
+DigitalIn drugR(p8);
+DigitalIn eruronL(p30);
+DigitalIn drugL(p29);
 
-signed char acc[3] = {0,0,0};
+signed char accR[3] = {0,0,0};
+signed char accL[3] = {0,0,0};
+
+signed char VR = 0;
+signed char CR = 0;
+signed char VL = 0;
+signed char CL = 0;
 
-void ch_data()
-{
-    rs485.putc('C');
+void ch_dataR(){
+    rs485R.putc('C');
     wait_ms(1);
-    Driver = 0;
+    DriverR = 0;
     wait_ms(3);
-    while(rs485.readable()) {
-    short ch = rs485.getc();
+    while(rs485R.readable()) {
+    short ch = rs485R.getc();
         switch(ch) {
             case 'X':
-                acc[0] = rs485.getc();
-                //pc.printf("Xacc:%d\n\r",acc[0]);
+                accR[0] = rs485R.getc();
                 break;
             case 'Y':
-                acc[1] = rs485.getc();
-                //pc.printf("Yacc:%d\n\r",acc[1]);
+                accR[1] = rs485R.getc();
                 break;
             case 'Z':
-                acc[2] = rs485.getc();
+                accR[2] = rs485R.getc();
+                break;
+            case 'V':
+                VR = rs485R.getc();
+                break;
+            case 'I':
+                CR = rs485R.getc();
                 break;
             default:
-                pc.printf("%d\n\r",rs485.getc());
                 break;
         }
     }
-    pc.printf("x:%i,y:%i,z:%i\n\r",acc[0],acc[1],acc[2]);
-    Driver = 1;
+    DriverR = 1;
+    wait_ms(1);
+}
+
+void ch_dataL(){
+    rs485L.putc('C');
+    wait_ms(1);
+    DriverL = 0;
+    wait_ms(3);
+    while(rs485L.readable()) {
+    short ch = rs485L.getc();
+        switch(ch) {
+            case 'X':
+                accL[0] = rs485L.getc();
+                break;
+            case 'Y':
+                accL[1] = rs485L.getc();
+                break;
+            case 'Z':
+                accL[2] = rs485L.getc();
+                break;
+            case 'V':
+                VL = rs485L.getc();
+                break;
+            case 'I':
+                CL = rs485L.getc();
+                break;
+            default:
+                break;
+        }
+    }
+    DriverL = 1;
     wait_ms(1);
 }
 
+void CALL_chDataR(){
+    ch_dataR();
+}
+
+void CALL_chDataL(){
+    ch_dataL();
+}
+
+void init(){
+    DriverR = 1;
+    DriverL = 1;
+    rs485R.baud(38400);
+    rs485L.baud(38400);
+    toKeikiSerial.baud(38400);
+    CheckDataR.attach(&CALL_chDataR,0.25);
+    wait(0.1);
+    CheckDataL.attach(&CALL_chDataL,0.25);
+}
+
+void InputAndSentControlValuesR(){
+    DriverR = 1;
+    rs485R.putc('A');
+    rs485R.putc(eruronR);
+    rs485R.putc('B');
+    rs485R.putc(drugR);    
+}
+
+void InputAndSentControlValuesL(){
+    DriverL = 1;
+    rs485L.putc('A');
+    rs485L.putc(eruronL);
+    rs485L.putc('B');
+    rs485L.putc(drugL);    
+}
+
+void SendDarasToKeiki(){
+    for(int i = 0; i < 3; i++){
+        toKeikiSerial.putc(accR[i]);
+        toKeikiSerial.putc(accL[i]);
+    }
+    toKeikiSerial.putc(VR);
+    toKeikiSerial.putc(VL);
+    toKeikiSerial.putc(CR);
+    toKeikiSerial.putc(CL);
+    toKeikiSerial.putc(eruronR);
+    toKeikiSerial.putc(eruronL);
+    toKeikiSerial.putc(drugR);
+    toKeikiSerial.putc(drugL);
+}
+
 int main()
 {
-    Driver = 1;
-    int counter = 0;
-    pc.printf("Driver\n\r");
-    rs485.baud(38400);
-    CheckData.attach(&ch_data,0.15);
+    init();
     while(1) {
-        rs485.putc('A');
-        rs485.putc(val1*180);
-        rs485.putc('B');
-        rs485.putc(counter);
-        counter++;
-        if(counter > 180)
-            counter = 0;
-        wait(0.25);
+        InputAndSentControlValuesR();
+        InputAndSentControlValuesL();
+        SendDarasToKeiki();
+        wait(WAIT_LOOP_TIME);
     }
 }