Tony YI
/
ESDC2014
123123123123123123123123123
Embed:
(wiki syntax)
Show/hide line numbers
compass.cpp
00001 #include "compass.h" 00002 /* 00003 Serial pc(USBTX, USBRX); 00004 00005 00006 */ 00007 00008 COMPASS::COMPASS(MySerial* serial) 00009 { 00010 this->_serial = serial; 00011 00012 serial->baud(56000); 00013 serial->format(8,SerialBase::None, 1); 00014 00015 init(); 00016 } 00017 00018 uint16_t COMPASS::read() 00019 { 00020 printf("before resume \r\n"); 00021 resume(); 00022 printf("after resume \r\n"); 00023 00024 wait(0.5); 00025 printf("before run \r\n"); 00026 00027 run(); 00028 printf("after run \r\n"); 00029 00030 printf("enter while buffer_count: %d \r\n",buffer_count); 00031 00032 00033 00034 printf("enter while 1 \r\n"); 00035 while(1) 00036 { 00037 if(buffer_count<=2) 00038 { 00039 buffer_count=0; 00040 break; 00041 } 00042 00043 buffer_count--; 00044 uint8_t tempc1=buffer[buffer_count]; 00045 buffer_count--; 00046 uint8_t tempc2=buffer[buffer_count]; 00047 00048 temp[0]= tempc1; 00049 temp[1]= tempc2; 00050 00051 if( 0xa0==( (temp[0]) & 0xe0)) 00052 { 00053 flag=1; 00054 printf("match !!!!!\r\n"); 00055 } 00056 00057 00058 00059 00060 00061 if(flag==1) 00062 { 00063 twobytes=(temp[0])*256+(uint8_t)(temp[1]); 00064 00065 digits= temp[1] & 0x0f; 00066 00067 tens= (temp[1]>>4) & 0x0f; 00068 00069 hundreds= temp[0] & 0x07; 00070 00071 _degree=100*hundreds+10*tens+digits; 00072 printf("buffer[0]: %x\r\n",temp[0]); 00073 printf("buffer[1]: %x\r\n",temp[1]); 00074 temp[0]=0; 00075 temp[1]=0; 00076 flag=0; 00077 00078 buffer_count=0; 00079 00080 00081 // printf("hundreds: %d\r\n",hundreds); 00082 // printf("tens: %d\r\n",tens); 00083 // printf("digits: %d\r\n",digits); 00084 00085 resume(); 00086 00087 00088 _degree+=90; 00089 00090 if(_degree>=360) 00091 _degree-=360; 00092 00093 printf("degree: %d\r\n",_degree); 00094 printf("----------------------\r\n"); 00095 return _degree; 00096 // break; 00097 } 00098 00099 } 00100 // printf("leave while 1 \r\n"); 00101 // wait(0.5); 00102 // 00103 // printf("before resume2 \r\n"); 00104 // 00105 // 00106 // printf("after resume2 \r\n"); 00107 00108 00109 00110 return 0; 00111 } 00112 00113 void COMPASS::init() 00114 { 00115 buffer[0]=0; 00116 buffer[1]=0; 00117 count=0; 00118 flag=0; 00119 buffer_count=0; 00120 for(int i=0;i<_BUFFER_SIZE;i++) 00121 { 00122 buffer[i]=0; 00123 } 00124 } 00125 00126 void COMPASS::run() 00127 { 00128 write2Bytes(RUN_MSB,RUN_LSB); 00129 } 00130 00131 void COMPASS::stop() 00132 { 00133 write2Bytes(STOP_MSB,STOP_LSB); 00134 } 00135 00136 void COMPASS::resume() 00137 { 00138 write2Bytes(RESUME_MSB,RESUME_LSB); 00139 } 00140 00141 void COMPASS::reset() 00142 { 00143 write2Bytes(RST_MSB,RST_LSB); 00144 } 00145 00146 void COMPASS::write2Bytes(char msb, char lsb) 00147 { 00148 _serial->putc(lsb); 00149 _serial->putc(msb); 00150 00151 } 00152 00153 void COMPASS::putToBuffer(uint8_t data) 00154 { 00155 if(buffer_count<_BUFFER_SIZE-10) 00156 buffer_count++; 00157 else 00158 { 00159 printf("Error full buffer \r\n"); 00160 buffer_count=0; 00161 } 00162 00163 buffer[buffer_count]=data; 00164 00165 } 00166 // 00167 //void COMPASS::check_time_out() 00168 //{ 00169 // if(flag == 1) 00170 // { 00171 // ON(); 00172 // wait(5); 00173 // mbed_reset(); 00174 // } 00175 // else 00176 // { 00177 // OFF(); 00178 // } 00179 //} 00180 // 00181 //void COMPASS::time_out_init() 00182 //{ 00183 // setFlag(); 00184 // time_out.detach(); 00185 // time_out.attach(this, &Buzzer::check_time_out, TIME_OUT); 00186 //}
Generated on Thu Jul 14 2022 22:42:47 by 1.7.2