Tony YI
/
ESDC2014
123123123123123123123123123
compass.cpp@3:4306d042af6f, 2014-07-03 (annotated)
- Committer:
- TonyYI
- Date:
- Thu Jul 03 14:52:44 2014 +0000
- Revision:
- 3:4306d042af6f
- Parent:
- 2:442902ec3aa1
123123
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
TonyYI | 0:3417ca0a36c0 | 1 | #include "compass.h" |
TonyYI | 3:4306d042af6f | 2 | /* |
TonyYI | 3:4306d042af6f | 3 | Serial pc(USBTX, USBRX); |
TonyYI | 3:4306d042af6f | 4 | |
TonyYI | 0:3417ca0a36c0 | 5 | |
TonyYI | 3:4306d042af6f | 6 | */ |
TonyYI | 3:4306d042af6f | 7 | |
TonyYI | 3:4306d042af6f | 8 | COMPASS::COMPASS(MySerial* serial) |
TonyYI | 0:3417ca0a36c0 | 9 | { |
TonyYI | 3:4306d042af6f | 10 | this->_serial = serial; |
TonyYI | 3:4306d042af6f | 11 | |
terryLAI | 1:cbec1283a16a | 12 | serial->baud(56000); |
terryLAI | 1:cbec1283a16a | 13 | serial->format(8,SerialBase::None, 1); |
TonyYI | 3:4306d042af6f | 14 | |
terryLAI | 1:cbec1283a16a | 15 | init(); |
TonyYI | 0:3417ca0a36c0 | 16 | } |
TonyYI | 0:3417ca0a36c0 | 17 | |
terryLAI | 2:442902ec3aa1 | 18 | uint16_t COMPASS::read() |
TonyYI | 0:3417ca0a36c0 | 19 | { |
TonyYI | 3:4306d042af6f | 20 | printf("before resume \r\n"); |
terryLAI | 1:cbec1283a16a | 21 | resume(); |
TonyYI | 3:4306d042af6f | 22 | printf("after resume \r\n"); |
TonyYI | 3:4306d042af6f | 23 | |
TonyYI | 3:4306d042af6f | 24 | wait(0.5); |
TonyYI | 3:4306d042af6f | 25 | printf("before run \r\n"); |
TonyYI | 3:4306d042af6f | 26 | |
TonyYI | 3:4306d042af6f | 27 | run(); |
TonyYI | 3:4306d042af6f | 28 | printf("after run \r\n"); |
TonyYI | 3:4306d042af6f | 29 | |
TonyYI | 3:4306d042af6f | 30 | printf("enter while buffer_count: %d \r\n",buffer_count); |
TonyYI | 3:4306d042af6f | 31 | |
TonyYI | 3:4306d042af6f | 32 | |
TonyYI | 3:4306d042af6f | 33 | |
TonyYI | 3:4306d042af6f | 34 | printf("enter while 1 \r\n"); |
TonyYI | 3:4306d042af6f | 35 | while(1) |
TonyYI | 3:4306d042af6f | 36 | { |
TonyYI | 3:4306d042af6f | 37 | if(buffer_count<=2) |
TonyYI | 3:4306d042af6f | 38 | { |
TonyYI | 3:4306d042af6f | 39 | buffer_count=0; |
TonyYI | 3:4306d042af6f | 40 | break; |
TonyYI | 3:4306d042af6f | 41 | } |
TonyYI | 3:4306d042af6f | 42 | |
TonyYI | 3:4306d042af6f | 43 | buffer_count--; |
TonyYI | 3:4306d042af6f | 44 | uint8_t tempc1=buffer[buffer_count]; |
TonyYI | 3:4306d042af6f | 45 | buffer_count--; |
TonyYI | 3:4306d042af6f | 46 | uint8_t tempc2=buffer[buffer_count]; |
TonyYI | 3:4306d042af6f | 47 | |
TonyYI | 3:4306d042af6f | 48 | temp[0]= tempc1; |
TonyYI | 3:4306d042af6f | 49 | temp[1]= tempc2; |
TonyYI | 3:4306d042af6f | 50 | |
TonyYI | 3:4306d042af6f | 51 | if( 0xa0==( (temp[0]) & 0xe0)) |
TonyYI | 3:4306d042af6f | 52 | { |
TonyYI | 3:4306d042af6f | 53 | flag=1; |
TonyYI | 3:4306d042af6f | 54 | printf("match !!!!!\r\n"); |
TonyYI | 3:4306d042af6f | 55 | } |
TonyYI | 3:4306d042af6f | 56 | |
TonyYI | 3:4306d042af6f | 57 | |
TonyYI | 3:4306d042af6f | 58 | |
TonyYI | 3:4306d042af6f | 59 | |
TonyYI | 3:4306d042af6f | 60 | |
TonyYI | 3:4306d042af6f | 61 | if(flag==1) |
TonyYI | 3:4306d042af6f | 62 | { |
TonyYI | 3:4306d042af6f | 63 | twobytes=(temp[0])*256+(uint8_t)(temp[1]); |
TonyYI | 3:4306d042af6f | 64 | |
TonyYI | 3:4306d042af6f | 65 | digits= temp[1] & 0x0f; |
TonyYI | 3:4306d042af6f | 66 | |
TonyYI | 3:4306d042af6f | 67 | tens= (temp[1]>>4) & 0x0f; |
TonyYI | 3:4306d042af6f | 68 | |
TonyYI | 3:4306d042af6f | 69 | hundreds= temp[0] & 0x07; |
TonyYI | 3:4306d042af6f | 70 | |
TonyYI | 3:4306d042af6f | 71 | _degree=100*hundreds+10*tens+digits; |
TonyYI | 3:4306d042af6f | 72 | printf("buffer[0]: %x\r\n",temp[0]); |
TonyYI | 3:4306d042af6f | 73 | printf("buffer[1]: %x\r\n",temp[1]); |
TonyYI | 3:4306d042af6f | 74 | temp[0]=0; |
TonyYI | 3:4306d042af6f | 75 | temp[1]=0; |
TonyYI | 3:4306d042af6f | 76 | flag=0; |
TonyYI | 3:4306d042af6f | 77 | |
TonyYI | 3:4306d042af6f | 78 | buffer_count=0; |
TonyYI | 3:4306d042af6f | 79 | |
TonyYI | 3:4306d042af6f | 80 | |
TonyYI | 3:4306d042af6f | 81 | // printf("hundreds: %d\r\n",hundreds); |
TonyYI | 3:4306d042af6f | 82 | // printf("tens: %d\r\n",tens); |
TonyYI | 3:4306d042af6f | 83 | // printf("digits: %d\r\n",digits); |
TonyYI | 3:4306d042af6f | 84 | |
TonyYI | 3:4306d042af6f | 85 | resume(); |
TonyYI | 3:4306d042af6f | 86 | |
TonyYI | 3:4306d042af6f | 87 | |
TonyYI | 3:4306d042af6f | 88 | _degree+=90; |
TonyYI | 3:4306d042af6f | 89 | |
TonyYI | 3:4306d042af6f | 90 | if(_degree>=360) |
TonyYI | 3:4306d042af6f | 91 | _degree-=360; |
TonyYI | 3:4306d042af6f | 92 | |
TonyYI | 3:4306d042af6f | 93 | printf("degree: %d\r\n",_degree); |
TonyYI | 3:4306d042af6f | 94 | printf("----------------------\r\n"); |
TonyYI | 3:4306d042af6f | 95 | return _degree; |
TonyYI | 3:4306d042af6f | 96 | // break; |
TonyYI | 3:4306d042af6f | 97 | } |
TonyYI | 3:4306d042af6f | 98 | |
TonyYI | 3:4306d042af6f | 99 | } |
TonyYI | 3:4306d042af6f | 100 | // printf("leave while 1 \r\n"); |
TonyYI | 3:4306d042af6f | 101 | // wait(0.5); |
TonyYI | 3:4306d042af6f | 102 | // |
TonyYI | 3:4306d042af6f | 103 | // printf("before resume2 \r\n"); |
TonyYI | 3:4306d042af6f | 104 | // |
TonyYI | 3:4306d042af6f | 105 | // |
TonyYI | 3:4306d042af6f | 106 | // printf("after resume2 \r\n"); |
TonyYI | 3:4306d042af6f | 107 | |
TonyYI | 3:4306d042af6f | 108 | |
TonyYI | 3:4306d042af6f | 109 | |
TonyYI | 3:4306d042af6f | 110 | return 0; |
TonyYI | 0:3417ca0a36c0 | 111 | } |
TonyYI | 0:3417ca0a36c0 | 112 | |
terryLAI | 1:cbec1283a16a | 113 | void COMPASS::init() |
TonyYI | 0:3417ca0a36c0 | 114 | { |
TonyYI | 3:4306d042af6f | 115 | buffer[0]=0; |
TonyYI | 3:4306d042af6f | 116 | buffer[1]=0; |
TonyYI | 3:4306d042af6f | 117 | count=0; |
TonyYI | 3:4306d042af6f | 118 | flag=0; |
TonyYI | 3:4306d042af6f | 119 | buffer_count=0; |
TonyYI | 3:4306d042af6f | 120 | for(int i=0;i<_BUFFER_SIZE;i++) |
TonyYI | 3:4306d042af6f | 121 | { |
TonyYI | 3:4306d042af6f | 122 | buffer[i]=0; |
TonyYI | 3:4306d042af6f | 123 | } |
TonyYI | 0:3417ca0a36c0 | 124 | } |
TonyYI | 0:3417ca0a36c0 | 125 | |
terryLAI | 1:cbec1283a16a | 126 | void COMPASS::run() |
TonyYI | 0:3417ca0a36c0 | 127 | { |
terryLAI | 1:cbec1283a16a | 128 | write2Bytes(RUN_MSB,RUN_LSB); |
TonyYI | 0:3417ca0a36c0 | 129 | } |
TonyYI | 0:3417ca0a36c0 | 130 | |
terryLAI | 1:cbec1283a16a | 131 | void COMPASS::stop() |
TonyYI | 0:3417ca0a36c0 | 132 | { |
terryLAI | 1:cbec1283a16a | 133 | write2Bytes(STOP_MSB,STOP_LSB); |
terryLAI | 1:cbec1283a16a | 134 | } |
TonyYI | 0:3417ca0a36c0 | 135 | |
terryLAI | 1:cbec1283a16a | 136 | void COMPASS::resume() |
TonyYI | 0:3417ca0a36c0 | 137 | { |
terryLAI | 1:cbec1283a16a | 138 | write2Bytes(RESUME_MSB,RESUME_LSB); |
terryLAI | 1:cbec1283a16a | 139 | } |
terryLAI | 1:cbec1283a16a | 140 | |
terryLAI | 1:cbec1283a16a | 141 | void COMPASS::reset() |
terryLAI | 1:cbec1283a16a | 142 | { |
terryLAI | 1:cbec1283a16a | 143 | write2Bytes(RST_MSB,RST_LSB); |
terryLAI | 1:cbec1283a16a | 144 | } |
terryLAI | 1:cbec1283a16a | 145 | |
terryLAI | 1:cbec1283a16a | 146 | void COMPASS::write2Bytes(char msb, char lsb) |
terryLAI | 1:cbec1283a16a | 147 | { |
TonyYI | 3:4306d042af6f | 148 | _serial->putc(lsb); |
TonyYI | 3:4306d042af6f | 149 | _serial->putc(msb); |
TonyYI | 3:4306d042af6f | 150 | |
terryLAI | 1:cbec1283a16a | 151 | } |
terryLAI | 1:cbec1283a16a | 152 | |
TonyYI | 3:4306d042af6f | 153 | void COMPASS::putToBuffer(uint8_t data) |
TonyYI | 3:4306d042af6f | 154 | { |
TonyYI | 3:4306d042af6f | 155 | if(buffer_count<_BUFFER_SIZE-10) |
TonyYI | 3:4306d042af6f | 156 | buffer_count++; |
TonyYI | 3:4306d042af6f | 157 | else |
TonyYI | 3:4306d042af6f | 158 | { |
TonyYI | 3:4306d042af6f | 159 | printf("Error full buffer \r\n"); |
TonyYI | 3:4306d042af6f | 160 | buffer_count=0; |
TonyYI | 3:4306d042af6f | 161 | } |
TonyYI | 3:4306d042af6f | 162 | |
TonyYI | 3:4306d042af6f | 163 | buffer[buffer_count]=data; |
TonyYI | 3:4306d042af6f | 164 | |
TonyYI | 3:4306d042af6f | 165 | } |
TonyYI | 3:4306d042af6f | 166 | // |
TonyYI | 3:4306d042af6f | 167 | //void COMPASS::check_time_out() |
TonyYI | 3:4306d042af6f | 168 | //{ |
TonyYI | 3:4306d042af6f | 169 | // if(flag == 1) |
TonyYI | 3:4306d042af6f | 170 | // { |
TonyYI | 3:4306d042af6f | 171 | // ON(); |
TonyYI | 3:4306d042af6f | 172 | // wait(5); |
TonyYI | 3:4306d042af6f | 173 | // mbed_reset(); |
TonyYI | 3:4306d042af6f | 174 | // } |
TonyYI | 3:4306d042af6f | 175 | // else |
TonyYI | 3:4306d042af6f | 176 | // { |
TonyYI | 3:4306d042af6f | 177 | // OFF(); |
TonyYI | 3:4306d042af6f | 178 | // } |
TonyYI | 3:4306d042af6f | 179 | //} |
TonyYI | 3:4306d042af6f | 180 | // |
TonyYI | 3:4306d042af6f | 181 | //void COMPASS::time_out_init() |
TonyYI | 3:4306d042af6f | 182 | //{ |
TonyYI | 3:4306d042af6f | 183 | // setFlag(); |
TonyYI | 3:4306d042af6f | 184 | // time_out.detach(); |
TonyYI | 3:4306d042af6f | 185 | // time_out.attach(this, &Buzzer::check_time_out, TIME_OUT); |
TonyYI | 3:4306d042af6f | 186 | //} |