Diff: compass.cpp

Revision:

4:a377ecb9364f

Parent:

3:4306d042af6f

--- a/compass.cpp	Thu Jul 03 14:52:44 2014 +0000
+++ b/compass.cpp	Fri Jul 04 13:05:15 2014 +0000
@@ -1,186 +1,156 @@
 #include "compass.h"
-/*
-Serial pc(USBTX, USBRX);
 
-
-*/
+Compass::Compass(MySerial _compassSerial)
+{
+     printf(" _compassSerial %p...\r\n",_compassSerial);
+     this->_compassSerial = _compassSerial;
+     temp=_compassSerial;
+     printf(" this->_compassSerial %p...\r\n",this->_compassSerial);
+    buffer = new uint8_t [DATA_BUFFER_SIZE];
+    _degree = 0;
+    _MSB = 0;
+    _LSB = 0;
+    _in = _out = 0;
+    
+         printf(" this->_compassSerial %p...\r\n",this->_compassSerial);
 
-COMPASS::COMPASS(MySerial* serial)
-{
-   this->_serial = serial;
-   
-   serial->baud(56000);
-   serial->format(8,SerialBase::None, 1);
-   
-   init();
+  
+    if(_compassSerial->writeable())
+    {
+            printf("_writable()...\r\n");
+        this->_compassSerial->putc('1');
+    }
+    printf("_MSB = 1 wrote...\r\n");
+    if(_compassSerial->writeable())
+    {
+        printf("_writable()...\r\n");
+        this->_compassSerial->putc('2');
+    }
+    printf("_LSB = 2 wrote...\r\n");
+    wait(0.1);
+
 }
 
-uint16_t COMPASS::read()
+Compass::~Compass()
 {
-    printf("before resume  \r\n");
-    resume();
-    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;
+    delete[] buffer;
 }
 
-void COMPASS::init() 
+void Compass::putToBuffer(uint8_t _x)
 {
-    buffer[0]=0;
-    buffer[1]=0;
-    count=0;
-    flag=0;
-    buffer_count=0;
-    for(int i=0;i<_BUFFER_SIZE;i++)
+    buffer[_in++] = _x;
+    if(_in == DATA_BUFFER_SIZE-1)
     {
-        buffer[i]=0;
+        _in &= 0x00;
     }
 }
 
-void COMPASS::run() 
-{
-    write2Bytes(RUN_MSB,RUN_LSB);
-}
-
-void COMPASS::stop() 
-{
-    write2Bytes(STOP_MSB,STOP_LSB);
-}
-
-void COMPASS::resume() 
+void Compass::clearBuffer()
 {
-    write2Bytes(RESUME_MSB,RESUME_LSB);
-}
-
-void COMPASS::reset() 
-{
-    write2Bytes(RST_MSB,RST_LSB);
-}
-
-void COMPASS::write2Bytes(char msb, char lsb) 
-{
-    _serial->putc(lsb);
-    _serial->putc(msb);
-    
+    _in = _out = 0;
 }
 
-void COMPASS::putToBuffer(uint8_t data) 
+int Compass::read()
 {
-    if(buffer_count<_BUFFER_SIZE-10)
-    buffer_count++;
+    printf("Entering Compass::read()...\r\n");
+    printf(" this->_compassSerial %p...\r\n",this->_compassSerial);
+    printf(" temp %p...\r\n",temp);
+
+    printf("_writable()?...\r\n");
+    if(_compassSerial->writeable())
+    {
+       printf("can write..\r\n");
+    }
+    
+    printf("readable()?...\r\n");
+    if(_compassSerial->writeable())
+    {
+       printf("can read..\r\n");
+    }
+    
+    stop();
+    clearBuffer();
+    
+   
+    printf("0\r\n");
+    resume();
+    if(buffer[_out++] == ACK_RESUME_MSB && buffer[_out++] == ACK_RESUME_LSB)
+    {
+    }
     else
-     {
-     printf("Error full buffer \r\n");
-     buffer_count=0;
-     }
-     
-    buffer[buffer_count]=data;
+    {
+        clearBuffer();
+        return -1;
+    }
+    
+    printf("1\r\n");
+    run();
+    if(buffer[_out++] == ACK_RUN_MSB && buffer[_out++] == ACK_RUN_LSB)
+    {
+    }
+    else
+    {
+        clearBuffer();
+        return -1;
+    }
     
+    printf("2\r\n");
+    _MSB = buffer[_out++];
+    _LSB = buffer[_out++];
+    printf("_MSB: %x, _LSB: %x\r\n", _MSB, _LSB);
+                
+    printf("3\r\n");
+    stop();
+    if(buffer[_in - 1] == ACK_STOP_MSB && buffer[_in] == ACK_STOP_LSB)
+    {
+    }
+    else
+    {
+        return -1;
+    }
+    _out = _in;
+    clearBuffer();
+    _degree=0;
+    printf("Exiting Compass::read()...\r\n");
+    return _degree;
 }
-//
-//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);
-//}
+
+void Compass::run() 
+{
+    printf("Entering Compass::run()...\r\n");
+    write2Bytes(RUN_MSB, RUN_LSB);
+}
+
+void Compass::stop() 
+{
+    printf("Entering Compass::stop()...\r\n");
+    write2Bytes(STOP_MSB, STOP_LSB);
+}
+
+void Compass::resume() 
+{
+    printf("Entering Compass::resume()...\r\n");
+    write2Bytes(RESUME_MSB, RESUME_LSB);
+}
+
+void Compass::reset() 
+{
+    printf("Entering Compass::reset()...\r\n");
+    write2Bytes(RST_MSB, RST_LSB);
+}
+
+void Compass::write2Bytes(char msb, char lsb) 
+{
+    printf("before writable...\r\n");
+    if(_compassSerial->writeable())
+    {
+        this->_compassSerial->putc(msb);
+    }
+    printf("_MSB = %x wrote...\r\n", msb);
+    if(_compassSerial->writeable())
+    {
+        this->_compassSerial->putc(lsb);
+    }
+    printf("_LSB = %x wrote...\r\n", lsb);
+    wait(0.1);
+}
\ No newline at end of file