EunKwang Lim / ARS

NT-ARSV1 Simple code(not completed)

http://ntrexlab.tistory.com/15

http://backup.ntrex.co.kr/mart7/mall.php?cat=049029000&query=view&no=41925

ARS.cpp@0:609146aec0da, 2015-03-14 (annotated)

Committer:
SWteamofAREC
Date:
Sat Mar 14 02:45:00 2015 +0000
Revision:
0:609146aec0da
NT-ARSV1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SWteamofAREC 0:609146aec0da 1 #include "ARS.h"
SWteamofAREC 0:609146aec0da 2
SWteamofAREC 0:609146aec0da 3
SWteamofAREC 0:609146aec0da 4 ARS::ARS(PinName tx,PinName rx) : _ARS(tx,rx)
SWteamofAREC 0:609146aec0da 5 {
SWteamofAREC 0:609146aec0da 6 rollAngle=0;
SWteamofAREC 0:609146aec0da 7 pitchAngle=0;
SWteamofAREC 0:609146aec0da 8 rollVel=0;
SWteamofAREC 0:609146aec0da 9 pitchVel=0;
SWteamofAREC 0:609146aec0da 10 } //생성자
SWteamofAREC 0:609146aec0da 11
SWteamofAREC 0:609146aec0da 12 void ARS::set(int baudRate=115200)
SWteamofAREC 0:609146aec0da 13 {
SWteamofAREC 0:609146aec0da 14 _ARS.baud(baudRate);
SWteamofAREC 0:609146aec0da 15 _ARS.printf("<CAH>");
SWteamofAREC 0:609146aec0da 16 }
SWteamofAREC 0:609146aec0da 17
SWteamofAREC 0:609146aec0da 18 void ARS::getdata() //데이터를 한줄만 받는다.
SWteamofAREC 0:609146aec0da 19 {
SWteamofAREC 0:609146aec0da 20 char incomingByte=0;
SWteamofAREC 0:609146aec0da 21 int resultValues=0;
SWteamofAREC 0:609146aec0da 22 int consideringMinusSign=1;
SWteamofAREC 0:609146aec0da 23 int selectingValuesFromARS=0;
SWteamofAREC 0:609146aec0da 24
SWteamofAREC 0:609146aec0da 25
SWteamofAREC 0:609146aec0da 26 while(true) {
SWteamofAREC 0:609146aec0da 27
SWteamofAREC 0:609146aec0da 28 incomingByte=_ARS.getc();
SWteamofAREC 0:609146aec0da 29
SWteamofAREC 0:609146aec0da 30 // Operation minus sign.
SWteamofAREC 0:609146aec0da 31 if(incomingByte == '-') {
SWteamofAREC 0:609146aec0da 32 consideringMinusSign = -1;
SWteamofAREC 0:609146aec0da 33 }
SWteamofAREC 0:609146aec0da 34
SWteamofAREC 0:609146aec0da 35 //Operation number type
SWteamofAREC 0:609146aec0da 36 if(incomingByte >= '0' && incomingByte <= '9') {
SWteamofAREC 0:609146aec0da 37 resultValues = resultValues*10 + incomingByte - '0';
SWteamofAREC 0:609146aec0da 38 }
SWteamofAREC 0:609146aec0da 39
SWteamofAREC 0:609146aec0da 40 //Operation end of data
SWteamofAREC 0:609146aec0da 41 if(incomingByte == ',' || incomingByte == '>') {
SWteamofAREC 0:609146aec0da 42 resultValues = resultValues * consideringMinusSign;
SWteamofAREC 0:609146aec0da 43 consideringMinusSign = 1; //next calculation, using plus sign
SWteamofAREC 0:609146aec0da 44 selectingValuesFromARS += 1; //increase the var.
SWteamofAREC 0:609146aec0da 45
SWteamofAREC 0:609146aec0da 46 switch(selectingValuesFromARS) {
SWteamofAREC 0:609146aec0da 47 case 1 : // getting roll angle as degree
SWteamofAREC 0:609146aec0da 48 rollAngle = resultValues * scaleFactorOfARS * rad2degree;
SWteamofAREC 0:609146aec0da 49 break;
SWteamofAREC 0:609146aec0da 50 case 2 : // getting pitch angle as degree
SWteamofAREC 0:609146aec0da 51 pitchAngle = resultValues * scaleFactorOfARS * rad2degree;
SWteamofAREC 0:609146aec0da 52 break;
SWteamofAREC 0:609146aec0da 53 case 3 : // getting roll anglular velocity as degree
SWteamofAREC 0:609146aec0da 54 rollVel = resultValues * scaleFactorOfARS * rad2degree;
SWteamofAREC 0:609146aec0da 55 break;
SWteamofAREC 0:609146aec0da 56
SWteamofAREC 0:609146aec0da 57 case 4 : // getting pitch anglular velocity as degree
SWteamofAREC 0:609146aec0da 58 pitchVel = resultValues * scaleFactorOfARS * rad2degree;
SWteamofAREC 0:609146aec0da 59 break;
SWteamofAREC 0:609146aec0da 60 }
SWteamofAREC 0:609146aec0da 61 resultValues = 0; //initializing the number.
SWteamofAREC 0:609146aec0da 62 }
SWteamofAREC 0:609146aec0da 63 if (incomingByte == '>') {
SWteamofAREC 0:609146aec0da 64 selectingValuesFromARS +=1;
SWteamofAREC 0:609146aec0da 65 }
SWteamofAREC 0:609146aec0da 66 if (selectingValuesFromARS == 5) {
SWteamofAREC 0:609146aec0da 67 selectingValuesFromARS=0;
SWteamofAREC 0:609146aec0da 68 break;
SWteamofAREC 0:609146aec0da 69 }
SWteamofAREC 0:609146aec0da 70
SWteamofAREC 0:609146aec0da 71 }
SWteamofAREC 0:609146aec0da 72 }
SWteamofAREC 0:609146aec0da 73
SWteamofAREC 0:609146aec0da 74
SWteamofAREC 0:609146aec0da 75
SWteamofAREC 0:609146aec0da 76 float ARS::getrollangle()
SWteamofAREC 0:609146aec0da 77 {
SWteamofAREC 0:609146aec0da 78 return rollAngle;
SWteamofAREC 0:609146aec0da 79 }
SWteamofAREC 0:609146aec0da 80
SWteamofAREC 0:609146aec0da 81 float ARS::getpitchangle()
SWteamofAREC 0:609146aec0da 82 {
SWteamofAREC 0:609146aec0da 83 return pitchAngle;
SWteamofAREC 0:609146aec0da 84 }
SWteamofAREC 0:609146aec0da 85
SWteamofAREC 0:609146aec0da 86 float ARS::getrollvel()
SWteamofAREC 0:609146aec0da 87 {
SWteamofAREC 0:609146aec0da 88 return rollVel;
SWteamofAREC 0:609146aec0da 89 }
SWteamofAREC 0:609146aec0da 90
SWteamofAREC 0:609146aec0da 91 float ARS::getpitchvel()
SWteamofAREC 0:609146aec0da 92 {
SWteamofAREC 0:609146aec0da 93 return pitchVel;
SWteamofAREC 0:609146aec0da 94 }