aa

Dependencies:   mbed TrapezoidControl QEI

Communication/Controller/Controller.cpp

Committer:
t_yamamoto
Date:
2018-09-08
Revision:
0:669ef71cba68

File content as of revision 0:669ef71cba68:

#include "Controller.h"

#include "Mu/Mu.h"
#include "../../../LED/LED.h"

using namespace MU;

namespace CONTROLLER {
    Ticker MuTimer;

    void UartUpdate();
    void LostCheck();

    namespace {
        ControllerData ctrData;
        ControllerData keepCtrData;
        const uint8_t defaultData[4] = CTR_DEFAULT_DATA;
        const char check[] = "DR=";
        volatile char packet[24];

        bool controllerLost = false;

        uint8_t timerCount = 0;
    }

    void Controller::Initialize() {
        MuUart.attach(UartUpdate, Serial::RxIrq);
        MuTimer.attach(LostCheck, 0.025);
        DataReset();
    }

    ControllerData* Controller::GetData() {
        __disable_irq();
        for(uint8_t i=0; i<CTR_DATA_LENGTH; i++) keepCtrData.buf[i] = ctrData.buf[i];
        __enable_irq();
        return &keepCtrData;
    }

    void Controller::DataReset() {
        // __disable_irq();
        for(uint8_t i=0; i<CTR_DATA_LENGTH; i++) ctrData.buf[i] = defaultData[i];
        // __enable_irq();
    }

    bool Controller::CheckControllerLost() {
        return controllerLost;
    }

    void UartUpdate() {
        static bool phase = false;
        static uint8_t count = 0;
        static uint8_t ledCount = 0;

        char data = MuUart.getc();

        if(phase) {
            packet[count] = data;
            if(count < 2) {
                if(data != check[count]) {
                    phase = false;
                    // controllerLost = true;
                    LED_MU = LED_OFF;
                }
            }
            else if(count == 9) {
                if(data != '\r') {
                    phase = false;
                    count = 0;
                } else {
                    ctrData.buf[0] = packet[5];
                    ctrData.buf[1] = packet[6];
                    ctrData.buf[2] = packet[7];
                    ctrData.buf[3] = packet[8];
                    phase = false;
                    timerCount = 0;
                    controllerLost = false;
                    LED_MU = LED_ON;
                }
            }
            count++;
        }
        else {
            if(data == '*') {
                count = 0;
                phase = true;
            }
        }
    }

    void LostCheck() {
        timerCount++;
        if(timerCount == 2) LED_MU = LED_OFF;
        if(timerCount >= 20) {
            controllerLost = true;
            Controller::DataReset();
            timerCount = 0;
            LED_MU = LED_OFF;
        }
    }
}