File content as of revision 0:762583fdd0de:

// 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

    }
}