Miya Miyagawa / Mbed 2 deprecated AutoMaster_si-tu

lknds

Dependencies:   mbed TrapezoidControl Pulse QEI

Communication/Controller/Controller.cpp@25:d367d1e7a153, 2019-09-27 (annotated)

Committer:
Ryosei
Date:
Fri Sep 27 05:19:58 2019 +0000
Revision:
25:d367d1e7a153
Parent:
0:669ef71cba68 
dskb

Who changed what in which revision?

UserRevisionLine numberNew 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 }