Tony YI
/
ESDC2014
123123123123123123123123123
Diff: compass.cpp
- Revision:
- 3:4306d042af6f
- Parent:
- 2:442902ec3aa1
diff -r 442902ec3aa1 -r 4306d042af6f compass.cpp --- a/compass.cpp Thu Jul 03 10:59:37 2014 +0000 +++ b/compass.cpp Thu Jul 03 14:52:44 2014 +0000 @@ -1,28 +1,126 @@ #include "compass.h" +/* +Serial pc(USBTX, USBRX); + -COMPASS::COMPASS(PinName tx, PinName rx) +*/ + +COMPASS::COMPASS(MySerial* serial) { - this->serial= new MySerial(tx,rx); + this->_serial = serial; + serial->baud(56000); serial->format(8,SerialBase::None, 1); + init(); } - uint16_t COMPASS::read() { + printf("before resume \r\n"); resume(); - buffer[0]=serial->getc(); - buffer[1]=serial->getc(); - stop(); - degree=(uint8_t)(buffer[0])*256+(uint8_t)(buffer[1]); - return degree; + printf("after resume \r\n"); + + wait(0.5); + printf("before run \r\n"); + + run(); + printf("after run \r\n"); + + printf("enter while buffer_count: %d \r\n",buffer_count); + + + + printf("enter while 1 \r\n"); + while(1) + { + if(buffer_count<=2) + { + buffer_count=0; + break; + } + + buffer_count--; + uint8_t tempc1=buffer[buffer_count]; + buffer_count--; + uint8_t tempc2=buffer[buffer_count]; + + temp[0]= tempc1; + temp[1]= tempc2; + + if( 0xa0==( (temp[0]) & 0xe0)) + { + flag=1; + printf("match !!!!!\r\n"); + } + + + + + + if(flag==1) + { + twobytes=(temp[0])*256+(uint8_t)(temp[1]); + + digits= temp[1] & 0x0f; + + tens= (temp[1]>>4) & 0x0f; + + hundreds= temp[0] & 0x07; + + _degree=100*hundreds+10*tens+digits; + printf("buffer[0]: %x\r\n",temp[0]); + printf("buffer[1]: %x\r\n",temp[1]); + temp[0]=0; + temp[1]=0; + flag=0; + + buffer_count=0; + + + // printf("hundreds: %d\r\n",hundreds); +// printf("tens: %d\r\n",tens); +// printf("digits: %d\r\n",digits); + + resume(); + + + _degree+=90; + + if(_degree>=360) + _degree-=360; + + printf("degree: %d\r\n",_degree); + printf("----------------------\r\n"); + return _degree; +// break; + } + + } + // printf("leave while 1 \r\n"); +// wait(0.5); +// +// printf("before resume2 \r\n"); +// +// +// printf("after resume2 \r\n"); + + + + return 0; } void COMPASS::init() { - run(); - stop(); + buffer[0]=0; + buffer[1]=0; + count=0; + flag=0; + buffer_count=0; + for(int i=0;i<_BUFFER_SIZE;i++) + { + buffer[i]=0; + } } void COMPASS::run() @@ -30,19 +128,16 @@ write2Bytes(RUN_MSB,RUN_LSB); } - void COMPASS::stop() { write2Bytes(STOP_MSB,STOP_LSB); } - void COMPASS::resume() { write2Bytes(RESUME_MSB,RESUME_LSB); } - void COMPASS::reset() { write2Bytes(RST_MSB,RST_LSB); @@ -50,9 +145,42 @@ void COMPASS::write2Bytes(char msb, char lsb) { - serial->putc(msb); - serial->putc(lsb); - + _serial->putc(lsb); + _serial->putc(msb); + } - +void COMPASS::putToBuffer(uint8_t data) +{ + if(buffer_count<_BUFFER_SIZE-10) + buffer_count++; + else + { + printf("Error full buffer \r\n"); + buffer_count=0; + } + + buffer[buffer_count]=data; + +} +// +//void COMPASS::check_time_out() +//{ +// if(flag == 1) +// { +// ON(); +// wait(5); +// mbed_reset(); +// } +// else +// { +// OFF(); +// } +//} +// +//void COMPASS::time_out_init() +//{ +// setFlag(); +// time_out.detach(); +// time_out.attach(this, &Buzzer::check_time_out, TIME_OUT); +//}