![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
aa
Dependencies: mbed TrapezoidControl QEI
Communication/Controller/Controller.cpp@31:45f97698857b, 2019-09-21 (annotated)
- Committer:
- yabahiro
- Date:
- Sat Sep 21 04:28:31 2019 +0000
- Revision:
- 31:45f97698857b
- Parent:
- 0:669ef71cba68
aa;
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
t_yamamoto | 0:669ef71cba68 | 1 | #include "Controller.h" |
t_yamamoto | 0:669ef71cba68 | 2 | |
t_yamamoto | 0:669ef71cba68 | 3 | #include "Mu/Mu.h" |
t_yamamoto | 0:669ef71cba68 | 4 | #include "../../../LED/LED.h" |
t_yamamoto | 0:669ef71cba68 | 5 | |
t_yamamoto | 0:669ef71cba68 | 6 | using namespace MU; |
t_yamamoto | 0:669ef71cba68 | 7 | |
t_yamamoto | 0:669ef71cba68 | 8 | namespace CONTROLLER { |
t_yamamoto | 0:669ef71cba68 | 9 | Ticker MuTimer; |
t_yamamoto | 0:669ef71cba68 | 10 | |
t_yamamoto | 0:669ef71cba68 | 11 | void UartUpdate(); |
t_yamamoto | 0:669ef71cba68 | 12 | void LostCheck(); |
t_yamamoto | 0:669ef71cba68 | 13 | |
t_yamamoto | 0:669ef71cba68 | 14 | namespace { |
t_yamamoto | 0:669ef71cba68 | 15 | ControllerData ctrData; |
t_yamamoto | 0:669ef71cba68 | 16 | ControllerData keepCtrData; |
t_yamamoto | 0:669ef71cba68 | 17 | const uint8_t defaultData[4] = CTR_DEFAULT_DATA; |
t_yamamoto | 0:669ef71cba68 | 18 | const char check[] = "DR="; |
t_yamamoto | 0:669ef71cba68 | 19 | volatile char packet[24]; |
t_yamamoto | 0:669ef71cba68 | 20 | |
t_yamamoto | 0:669ef71cba68 | 21 | bool controllerLost = false; |
t_yamamoto | 0:669ef71cba68 | 22 | |
t_yamamoto | 0:669ef71cba68 | 23 | uint8_t timerCount = 0; |
t_yamamoto | 0:669ef71cba68 | 24 | } |
t_yamamoto | 0:669ef71cba68 | 25 | |
t_yamamoto | 0:669ef71cba68 | 26 | void Controller::Initialize() { |
t_yamamoto | 0:669ef71cba68 | 27 | MuUart.attach(UartUpdate, Serial::RxIrq); |
t_yamamoto | 0:669ef71cba68 | 28 | MuTimer.attach(LostCheck, 0.025); |
t_yamamoto | 0:669ef71cba68 | 29 | DataReset(); |
t_yamamoto | 0:669ef71cba68 | 30 | } |
t_yamamoto | 0:669ef71cba68 | 31 | |
t_yamamoto | 0:669ef71cba68 | 32 | ControllerData* Controller::GetData() { |
t_yamamoto | 0:669ef71cba68 | 33 | __disable_irq(); |
t_yamamoto | 0:669ef71cba68 | 34 | for(uint8_t i=0; i<CTR_DATA_LENGTH; i++) keepCtrData.buf[i] = ctrData.buf[i]; |
t_yamamoto | 0:669ef71cba68 | 35 | __enable_irq(); |
t_yamamoto | 0:669ef71cba68 | 36 | return &keepCtrData; |
t_yamamoto | 0:669ef71cba68 | 37 | } |
t_yamamoto | 0:669ef71cba68 | 38 | |
t_yamamoto | 0:669ef71cba68 | 39 | void Controller::DataReset() { |
t_yamamoto | 0:669ef71cba68 | 40 | // __disable_irq(); |
t_yamamoto | 0:669ef71cba68 | 41 | for(uint8_t i=0; i<CTR_DATA_LENGTH; i++) ctrData.buf[i] = defaultData[i]; |
t_yamamoto | 0:669ef71cba68 | 42 | // __enable_irq(); |
t_yamamoto | 0:669ef71cba68 | 43 | } |
t_yamamoto | 0:669ef71cba68 | 44 | |
t_yamamoto | 0:669ef71cba68 | 45 | bool Controller::CheckControllerLost() { |
t_yamamoto | 0:669ef71cba68 | 46 | return controllerLost; |
t_yamamoto | 0:669ef71cba68 | 47 | } |
t_yamamoto | 0:669ef71cba68 | 48 | |
t_yamamoto | 0:669ef71cba68 | 49 | void UartUpdate() { |
t_yamamoto | 0:669ef71cba68 | 50 | static bool phase = false; |
t_yamamoto | 0:669ef71cba68 | 51 | static uint8_t count = 0; |
t_yamamoto | 0:669ef71cba68 | 52 | static uint8_t ledCount = 0; |
t_yamamoto | 0:669ef71cba68 | 53 | |
t_yamamoto | 0:669ef71cba68 | 54 | char data = MuUart.getc(); |
t_yamamoto | 0:669ef71cba68 | 55 | |
t_yamamoto | 0:669ef71cba68 | 56 | if(phase) { |
t_yamamoto | 0:669ef71cba68 | 57 | packet[count] = data; |
t_yamamoto | 0:669ef71cba68 | 58 | if(count < 2) { |
t_yamamoto | 0:669ef71cba68 | 59 | if(data != check[count]) { |
t_yamamoto | 0:669ef71cba68 | 60 | phase = false; |
t_yamamoto | 0:669ef71cba68 | 61 | // controllerLost = true; |
t_yamamoto | 0:669ef71cba68 | 62 | LED_MU = LED_OFF; |
t_yamamoto | 0:669ef71cba68 | 63 | } |
t_yamamoto | 0:669ef71cba68 | 64 | } |
t_yamamoto | 0:669ef71cba68 | 65 | else if(count == 9) { |
t_yamamoto | 0:669ef71cba68 | 66 | if(data != '\r') { |
t_yamamoto | 0:669ef71cba68 | 67 | phase = false; |
t_yamamoto | 0:669ef71cba68 | 68 | count = 0; |
t_yamamoto | 0:669ef71cba68 | 69 | } else { |
t_yamamoto | 0:669ef71cba68 | 70 | ctrData.buf[0] = packet[5]; |
t_yamamoto | 0:669ef71cba68 | 71 | ctrData.buf[1] = packet[6]; |
t_yamamoto | 0:669ef71cba68 | 72 | ctrData.buf[2] = packet[7]; |
t_yamamoto | 0:669ef71cba68 | 73 | ctrData.buf[3] = packet[8]; |
t_yamamoto | 0:669ef71cba68 | 74 | phase = false; |
t_yamamoto | 0:669ef71cba68 | 75 | timerCount = 0; |
t_yamamoto | 0:669ef71cba68 | 76 | controllerLost = false; |
t_yamamoto | 0:669ef71cba68 | 77 | LED_MU = LED_ON; |
t_yamamoto | 0:669ef71cba68 | 78 | } |
t_yamamoto | 0:669ef71cba68 | 79 | } |
t_yamamoto | 0:669ef71cba68 | 80 | count++; |
t_yamamoto | 0:669ef71cba68 | 81 | } |
t_yamamoto | 0:669ef71cba68 | 82 | else { |
t_yamamoto | 0:669ef71cba68 | 83 | if(data == '*') { |
t_yamamoto | 0:669ef71cba68 | 84 | count = 0; |
t_yamamoto | 0:669ef71cba68 | 85 | phase = true; |
t_yamamoto | 0:669ef71cba68 | 86 | } |
t_yamamoto | 0:669ef71cba68 | 87 | } |
t_yamamoto | 0:669ef71cba68 | 88 | } |
t_yamamoto | 0:669ef71cba68 | 89 | |
t_yamamoto | 0:669ef71cba68 | 90 | void LostCheck() { |
t_yamamoto | 0:669ef71cba68 | 91 | timerCount++; |
t_yamamoto | 0:669ef71cba68 | 92 | if(timerCount == 2) LED_MU = LED_OFF; |
t_yamamoto | 0:669ef71cba68 | 93 | if(timerCount >= 20) { |
t_yamamoto | 0:669ef71cba68 | 94 | controllerLost = true; |
t_yamamoto | 0:669ef71cba68 | 95 | Controller::DataReset(); |
t_yamamoto | 0:669ef71cba68 | 96 | timerCount = 0; |
t_yamamoto | 0:669ef71cba68 | 97 | LED_MU = LED_OFF; |
t_yamamoto | 0:669ef71cba68 | 98 | } |
t_yamamoto | 0:669ef71cba68 | 99 | } |
t_yamamoto | 0:669ef71cba68 | 100 | } |