lknds
Dependencies: mbed TrapezoidControl Pulse QEI
Controller.cpp
00001 #include "Controller.h" 00002 00003 #include "Mu/Mu.h" 00004 #include "../../../LED/LED.h" 00005 00006 using namespace MU; 00007 00008 namespace CONTROLLER { 00009 Ticker MuTimer; 00010 00011 void UartUpdate(); 00012 void LostCheck(); 00013 00014 namespace { 00015 ControllerData ctrData; 00016 ControllerData keepCtrData; 00017 const uint8_t defaultData[4] = CTR_DEFAULT_DATA; 00018 const char check[] = "DR="; 00019 volatile char packet[24]; 00020 00021 bool controllerLost = false; 00022 00023 uint8_t timerCount = 0; 00024 } 00025 00026 void Controller::Initialize() { 00027 MuUart.attach(UartUpdate, Serial::RxIrq); 00028 MuTimer.attach(LostCheck, 0.025); 00029 DataReset(); 00030 } 00031 00032 ControllerData* Controller::GetData() { 00033 __disable_irq(); 00034 for(uint8_t i=0; i<CTR_DATA_LENGTH; i++) keepCtrData.buf[i] = ctrData.buf[i]; 00035 __enable_irq(); 00036 return &keepCtrData; 00037 } 00038 00039 void Controller::DataReset() { 00040 // __disable_irq(); 00041 for(uint8_t i=0; i<CTR_DATA_LENGTH; i++) ctrData.buf[i] = defaultData[i]; 00042 // __enable_irq(); 00043 } 00044 00045 bool Controller::CheckControllerLost() { 00046 return controllerLost; 00047 } 00048 00049 void UartUpdate() { 00050 static bool phase = false; 00051 static uint8_t count = 0; 00052 static uint8_t ledCount = 0; 00053 00054 char data = MuUart.getc(); 00055 00056 if(phase) { 00057 packet[count] = data; 00058 if(count < 2) { 00059 if(data != check[count]) { 00060 phase = false; 00061 // controllerLost = true; 00062 LED_MU = LED_OFF; 00063 } 00064 } 00065 else if(count == 9) { 00066 if(data != '\r') { 00067 phase = false; 00068 count = 0; 00069 } else { 00070 ctrData.buf[0] = packet[5]; 00071 ctrData.buf[1] = packet[6]; 00072 ctrData.buf[2] = packet[7]; 00073 ctrData.buf[3] = packet[8]; 00074 phase = false; 00075 timerCount = 0; 00076 controllerLost = false; 00077 LED_MU = LED_ON; 00078 } 00079 } 00080 count++; 00081 } 00082 else { 00083 if(data == '*') { 00084 count = 0; 00085 phase = true; 00086 } 00087 } 00088 } 00089 00090 void LostCheck() { 00091 timerCount++; 00092 if(timerCount == 2) LED_MU = LED_OFF; 00093 if(timerCount >= 20) { 00094 controllerLost = true; 00095 Controller::DataReset(); 00096 timerCount = 0; 00097 LED_MU = LED_OFF; 00098 } 00099 } 00100 }
Generated on Sun Jul 17 2022 15:33:12 by 1.7.2