NagaokaRoboticsClub_mbedTeam / FEP

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   CompactController_2017 quadOmni_yanagi NHK2017_octopus NHK2017_octopus2 ... more

FEP.cpp@3:67bcc5052829, 2017-07-11 (annotated)

Committer:
eil4nyqn
Date:
Tue Jul 11 11:16:56 2017 +0000
Revision:
3:67bcc5052829
Parent:
1:c5cf36bf2e3b 
natukawa

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eil4nyqn 0:bf959a15b079 1 #include "FEP.h"
eil4nyqn 0:bf959a15b079 2
eil4nyqn 0:bf959a15b079 3 FEP::FEP(PinName tx,PinName rx,int baud):serial(tx,rx,baud){
eil4nyqn 0:bf959a15b079 4
eil4nyqn 0:bf959a15b079 5 }
eil4nyqn 0:bf959a15b079 6 FEP::~FEP(){
eil4nyqn 0:bf959a15b079 7
eil4nyqn 0:bf959a15b079 8 }
eil4nyqn 0:bf959a15b079 9
eil4nyqn 0:bf959a15b079 10 uint8_t FEP::RST(){
eil4nyqn 0:bf959a15b079 11 uint8_t i,response;
eil4nyqn 0:bf959a15b079 12 for(i=0;i<FEP_RETRY;i++){
eil4nyqn 0:bf959a15b079 13 serial.printf("@RST\r\n");
eil4nyqn 0:bf959a15b079 14 response=FEP::getResponse();
eil4nyqn 0:bf959a15b079 15 if(response==FEP_P0) break;
eil4nyqn 0:bf959a15b079 16 }
eil4nyqn 0:bf959a15b079 17 return response;
eil4nyqn 0:bf959a15b079 18 }
eil4nyqn 0:bf959a15b079 19
eil4nyqn 0:bf959a15b079 20 uint8_t FEP::getResponse(){
eil4nyqn 0:bf959a15b079 21 int n,i=3;
eil4nyqn 0:bf959a15b079 22 char buf[256];
eil4nyqn 0:bf959a15b079 23
eil4nyqn 0:bf959a15b079 24 for(n=0; n<FEP_TIMEOUT_MS; n++) {
eil4nyqn 0:bf959a15b079 25 if(serial.readable()) {
eil4nyqn 0:bf959a15b079 26 n=0;
eil4nyqn 0:bf959a15b079 27 buf[i]=serial.getc();
eil4nyqn 0:bf959a15b079 28 if(buf[i-1]=='\r' && buf[i]=='\n') {
eil4nyqn 0:bf959a15b079 29 if(buf[i-3]=='P' && buf[i-2]=='1') {
eil4nyqn 0:bf959a15b079 30 return FEP::getResponse();
eil4nyqn 0:bf959a15b079 31 } else {
eil4nyqn 0:bf959a15b079 32 if(buf[i-3]=='P' && buf[i-2]=='0') {
eil4nyqn 0:bf959a15b079 33 return FEP_P0;
eil4nyqn 0:bf959a15b079 34 } else if(buf[i-3]=='N' && buf[i-2]=='0') {
eil4nyqn 0:bf959a15b079 35 return FEP_N0;
eil4nyqn 0:bf959a15b079 36 } else if(buf[i-3]=='N' && buf[i-2]=='1') {
eil4nyqn 0:bf959a15b079 37 return FEP_N1;
eil4nyqn 0:bf959a15b079 38 } else if(buf[i-3]=='N' && buf[i-2]=='2') {
eil4nyqn 0:bf959a15b079 39 return FEP_N2;
eil4nyqn 0:bf959a15b079 40 } else if(buf[i-3]=='N' && buf[i-2]=='3') {
eil4nyqn 0:bf959a15b079 41 return FEP_N3;
eil4nyqn 0:bf959a15b079 42 } else {
eil4nyqn 0:bf959a15b079 43 i=3;
eil4nyqn 0:bf959a15b079 44 continue;
eil4nyqn 0:bf959a15b079 45 }
eil4nyqn 0:bf959a15b079 46 }
eil4nyqn 0:bf959a15b079 47 }
eil4nyqn 0:bf959a15b079 48 i++;
eil4nyqn 0:bf959a15b079 49 } else {
eil4nyqn 0:bf959a15b079 50 wait_us(10);
eil4nyqn 0:bf959a15b079 51 continue;
eil4nyqn 0:bf959a15b079 52 }
eil4nyqn 0:bf959a15b079 53 }
eil4nyqn 0:bf959a15b079 54 return FEP_NO_RESPONSE;
eil4nyqn 0:bf959a15b079 55 }
eil4nyqn 0:bf959a15b079 56
eil4nyqn 1:c5cf36bf2e3b 57 uint8_t FEP::write_txt(char _data[],uint8_t _addr){
eil4nyqn 3:67bcc5052829 58 // serial.printf("@TXT%03uA%sB\r\n",_addr,_data);
eil4nyqn 3:67bcc5052829 59 serial.printf("@TXT%03uG",_addr);
eil4nyqn 3:67bcc5052829 60 for(int i = 0;i<sizeof(_data);i++) {
eil4nyqn 3:67bcc5052829 61 serial.printf("%02x",_data[i]);
eil4nyqn 3:67bcc5052829 62 }
eil4nyqn 3:67bcc5052829 63 serial.printf("H\r\n");
eil4nyqn 0:bf959a15b079 64 return FEP::getResponse();
eil4nyqn 0:bf959a15b079 65 }
eil4nyqn 0:bf959a15b079 66
eil4nyqn 0:bf959a15b079 67 uint8_t FEP::readIntensity(){
eil4nyqn 0:bf959a15b079 68 char data[256];
eil4nyqn 0:bf959a15b079 69 int intensity;
eil4nyqn 0:bf959a15b079 70 uint16_t i=0,n;
eil4nyqn 0:bf959a15b079 71 serial.putc('@');
eil4nyqn 0:bf959a15b079 72 serial.putc('D');
eil4nyqn 0:bf959a15b079 73 serial.putc('B');
eil4nyqn 0:bf959a15b079 74 serial.putc('M');
eil4nyqn 0:bf959a15b079 75 serial.putc('\r');
eil4nyqn 0:bf959a15b079 76 serial.putc('\n');
eil4nyqn 0:bf959a15b079 77 for(n=0; n<FEP_TIMEOUT_MS; n++) {
eil4nyqn 0:bf959a15b079 78 if(serial.readable()){
eil4nyqn 0:bf959a15b079 79 data[i]=serial.getc();
eil4nyqn 0:bf959a15b079 80 i++;
eil4nyqn 0:bf959a15b079 81 n=0;
eil4nyqn 0:bf959a15b079 82 }else{
eil4nyqn 0:bf959a15b079 83 wait_us(10);
eil4nyqn 0:bf959a15b079 84 continue;
eil4nyqn 0:bf959a15b079 85 }
eil4nyqn 0:bf959a15b079 86 if(i>7) break;
eil4nyqn 0:bf959a15b079 87 }
eil4nyqn 0:bf959a15b079 88 if(data[0]=='N' && data[1]=='0'){
eil4nyqn 0:bf959a15b079 89 return FEP_N0;
eil4nyqn 0:bf959a15b079 90 }else{
eil4nyqn 0:bf959a15b079 91 intensity=atoi(data);
eil4nyqn 0:bf959a15b079 92 _intensity=abs(intensity);
eil4nyqn 0:bf959a15b079 93 return FEP_P0;
eil4nyqn 0:bf959a15b079 94 }
eil4nyqn 0:bf959a15b079 95 }
eil4nyqn 0:bf959a15b079 96
eil4nyqn 0:bf959a15b079 97 uint8_t FEP::getIntensity(){
eil4nyqn 0:bf959a15b079 98 return _intensity;
eil4nyqn 0:bf959a15b079 99 }
eil4nyqn 0:bf959a15b079 100
eil4nyqn 3:67bcc5052829 101 uint8_t FEP::read_txt(char *str,uint8_t len)
eil4nyqn 0:bf959a15b079 102 {
eil4nyqn 0:bf959a15b079 103 uint16_t i,n,m,k;
eil4nyqn 0:bf959a15b079 104 char raw_data[256];
eil4nyqn 0:bf959a15b079 105 char data[256];
eil4nyqn 0:bf959a15b079 106 char intensity_c[3]={0};
eil4nyqn 0:bf959a15b079 107 for(k=0;k<FEP_RX_TIMEOUT_US;k++){
eil4nyqn 0:bf959a15b079 108 if(serial.readable()) {
eil4nyqn 0:bf959a15b079 109 k=0;
eil4nyqn 0:bf959a15b079 110 raw_data[0]=serial.getc();
eil4nyqn 0:bf959a15b079 111 if(raw_data[0]=='R') {
eil4nyqn 0:bf959a15b079 112 i=1;
eil4nyqn 0:bf959a15b079 113 for(k=0;k<FEP_RX_TIMEOUT_US;k++) {
eil4nyqn 0:bf959a15b079 114 if(serial.readable()) {
eil4nyqn 0:bf959a15b079 115 raw_data[i]=serial.getc();
eil4nyqn 0:bf959a15b079 116 if(raw_data[i-1]=='\r' && raw_data[i]=='\n') {
eil4nyqn 0:bf959a15b079 117 for(n=0; n<(i-10); n++) {
eil4nyqn 0:bf959a15b079 118 data[n]=raw_data[n+6];
eil4nyqn 0:bf959a15b079 119 }
eil4nyqn 0:bf959a15b079 120 intensity_c[0]=raw_data[n+6];
eil4nyqn 0:bf959a15b079 121 intensity_c[1]=raw_data[n+7];
eil4nyqn 0:bf959a15b079 122 intensity_c[2]=raw_data[n+8];
eil4nyqn 0:bf959a15b079 123 _intensity=abs(atoi(intensity_c));
eil4nyqn 3:67bcc5052829 124 if(data[0]=='G' && data[n-1]=='H') {
eil4nyqn 3:67bcc5052829 125 FEP::hex2dec(data,str,len);
eil4nyqn 0:bf959a15b079 126 return FEP_RX_SUCCESS;
eil4nyqn 0:bf959a15b079 127 } else {
eil4nyqn 0:bf959a15b079 128 return FEP_RX_FAIL;
eil4nyqn 0:bf959a15b079 129 }
eil4nyqn 0:bf959a15b079 130 }
eil4nyqn 0:bf959a15b079 131 i++;
eil4nyqn 0:bf959a15b079 132 }else{
eil4nyqn 0:bf959a15b079 133 wait_us(1);
eil4nyqn 0:bf959a15b079 134 continue;
eil4nyqn 0:bf959a15b079 135 }
eil4nyqn 0:bf959a15b079 136 }
eil4nyqn 0:bf959a15b079 137 }
eil4nyqn 0:bf959a15b079 138 }else{
eil4nyqn 0:bf959a15b079 139 wait_us(1);
eil4nyqn 0:bf959a15b079 140 continue;
eil4nyqn 0:bf959a15b079 141 }
eil4nyqn 0:bf959a15b079 142 }
eil4nyqn 0:bf959a15b079 143 return FEP_NO_RESPONSE;
eil4nyqn 0:bf959a15b079 144 }
eil4nyqn 3:67bcc5052829 145
eil4nyqn 3:67bcc5052829 146 void FEP::hex2dec(char data[],char *str,uint8_t len) {
eil4nyqn 3:67bcc5052829 147 char *endptr;
eil4nyqn 3:67bcc5052829 148 char buf[2];
eil4nyqn 3:67bcc5052829 149 for(int i = 0;i < len*2;i+=2) {
eil4nyqn 3:67bcc5052829 150 buf[0] = data[1+i];
eil4nyqn 3:67bcc5052829 151 buf[1] = data[1+i+1];
eil4nyqn 3:67bcc5052829 152 str[i/2] = (char)strtol(buf, &endptr, 16);
eil4nyqn 3:67bcc5052829 153 }
eil4nyqn 3:67bcc5052829 154 }