Diff: main.cpp

Revision:

0:762583fdd0de

Child:

1:ccc6ed1e4cd8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jul 16 05:05:16 2020 +0000
@@ -0,0 +1,178 @@
+// Construction macine controler for MBET
+// Onishi Itsuki
+// July 2020
+
+#include "mbed.h"
+
+#define START_BYTE 0x0f
+#define BYTE_A 2*i
+#define BYTE_B 2*i+1
+
+uint8_t buffer[8];
+int outputBuffer[12];
+int bufferIndex = 0;
+int outProcCounter = 0;
+int outProcFlag = 0;
+
+Ticker ticker;
+Serial pc(USBTX, USBRX);
+DigitalOut  stateChecker(LED1);
+DigitalOut startByteError(LED3);
+DigitalOut parityByteError(LED4);
+
+// set output pins
+DigitalOut boomOutA(p21);
+DigitalOut boomOutB(p22);
+DigitalOut armOutA(p23);
+DigitalOut armOutB(p24);
+DigitalOut bucketOutA(p25);
+DigitalOut bucketOutB(p26);
+DigitalOut slewingOutA(p20);
+DigitalOut slewingOutB(p19);
+DigitalOut rightWheelOutA(p18);
+DigitalOut rightWheelOutB(p17);
+DigitalOut leftWheelOutA(p16);
+DigitalOut leftWheelOutB(p15);
+
+
+void set_test_buffer(void)
+{
+    buffer[0] = 31;
+    buffer[1] = 250;
+    buffer[2] = 200;
+    buffer[3] = 150;
+    buffer[4] = 100;
+    buffer[5] = 50;
+    buffer[6] = 10;
+    buffer[7] = 248;
+}
+
+
+void set_output(void)
+{
+    boomOutA = outputBuffer[0];
+    boomOutB = outputBuffer[1];
+    armOutA = outputBuffer[2];
+    armOutB = outputBuffer[3];
+    bucketOutA = outputBuffer[4];
+    bucketOutB = outputBuffer[5];
+    slewingOutA = outputBuffer[6];
+    slewingOutB = outputBuffer[7];
+    rightWheelOutA = outputBuffer[8];
+    rightWheelOutB = outputBuffer[9];
+    leftWheelOutA = outputBuffer[10];
+    leftWheelOutB = outputBuffer[11];
+}
+
+
+void init_state(void)
+{
+    for(int i=0; i<6; i++) {
+        outputBuffer[BYTE_A] = 0;
+        outputBuffer[BYTE_B] = 0;
+    }
+    set_output();
+}
+
+
+void PWM_processor(void)
+{
+    for(int i=0; i<6; i++) {
+        if((buffer[i+1]==0) || (buffer[i+1]==128)) {
+            outputBuffer[BYTE_A] = 0;
+            outputBuffer[BYTE_B] = 0;
+            continue;
+        }
+
+        if(buffer[i+1]>128) {
+            outputBuffer[BYTE_A] = 0;
+            outputBuffer[BYTE_B] = 1;
+        } else {
+            outputBuffer[BYTE_A] = 1;
+            outputBuffer[BYTE_B] = 0;
+        }
+
+        // decrement
+        if (buffer[i+1] > 0)
+            buffer[i+1]--;
+    }
+    set_output();
+}
+
+
+void event_func(void)
+{
+    stateChecker = 1;
+
+    // For test
+    bufferIndex = 8;
+    set_test_buffer();
+//    for(int i=0; i<8; i++){
+//        pc.printf("%d:%d ", i, buffer[i]);
+//    }
+    uint8_t rx = buffer[0];
+    // end of code for test
+
+//        uint8_t rx = pc.getc();
+//
+//         wait start byte
+    if(bufferIndex == 0 && rx != START_BYTE) {
+        startByteError = 1;
+        pc.printf("start byte ERROR");
+        return;
+    } else {
+        startByteError = 0;
+    }
+
+//        // push serial to buffer
+//        buffer[bufferIndex++] = rx;
+
+    // when buffer is full
+    if(bufferIndex == 8) {
+        bufferIndex = 0;
+
+        // check parity
+//        uint8_t serialXOR = buffer[1];
+//        for (int i=2; i<=6; i++) {
+//            serialXOR ^= buffer[i];
+//        }
+//        if (serialXOR != buffer[7]) {
+//            parityByteError = 1;
+//            pc.printf("parity byte ERROR: XOR is %d, buffer byte is %d  ", serialXOR, buffer[7]);
+//            return;
+//        } else {
+//            outProcFlag = 1;
+//            parityByteError = 0;
+//        }
+    }
+}
+
+int main(void)
+{
+    pc.baud(921600);
+    startByteError = 0;
+    parityByteError = 0;
+    int test_count = 0;
+
+    // initial state is free run
+    init_state();
+    wait(1);
+    ticker.attach_us(PWM_processor, 500.0f);
+    pc.printf("+");
+
+    while(1) {
+        stateChecker = 0;
+//        if(pc.readable(){
+        event_func();
+        test_count++;
+//        }
+
+//      code for test
+        if(test_count%100000 == 0) {
+            pc.printf("+");
+            test_count = 1;
+        }
+//      end of code for test
+
+    }
+}