ihsan hadi / CMPS11_Serial

For CMPS11 Serial Mode

CMPS11.cpp@0:804d45caee8d, 2017-08-25 (annotated)

Committer:
alienbernamaihsan
Date:
Fri Aug 25 08:02:16 2017 +0000
Revision:
0:804d45caee8d
CMPS11 for Serial mode

Who changed what in which revision?

UserRevisionLine numberNew contents of line
alienbernamaihsan 0:804d45caee8d 1 #include "CMPS11.h"
alienbernamaihsan 0:804d45caee8d 2
alienbernamaihsan 0:804d45caee8d 3 cmps11::cmps11(PinName tx, PinName rx ) : _cmps11(tx,rx)
alienbernamaihsan 0:804d45caee8d 4 {
alienbernamaihsan 0:804d45caee8d 5 readnow = 0 ;
alienbernamaihsan 0:804d45caee8d 6 readlast = 0 ;
alienbernamaihsan 0:804d45caee8d 7 sudut = 0 ;
alienbernamaihsan 0:804d45caee8d 8 modelast = 0 ;
alienbernamaihsan 0:804d45caee8d 9 modenow = 0 ;
alienbernamaihsan 0:804d45caee8d 10 _cmps11.format(8,SerialBase::None,2);
alienbernamaihsan 0:804d45caee8d 11 }
alienbernamaihsan 0:804d45caee8d 12
alienbernamaihsan 0:804d45caee8d 13 char cmps11::startCalibration()
alienbernamaihsan 0:804d45caee8d 14 {
alienbernamaihsan 0:804d45caee8d 15 _cmps11.putc(0xF0);
alienbernamaihsan 0:804d45caee8d 16 while( !(_cmps11.readable()))
alienbernamaihsan 0:804d45caee8d 17 {}
alienbernamaihsan 0:804d45caee8d 18 if( _cmps11.getc() != 0x55 )
alienbernamaihsan 0:804d45caee8d 19 {
alienbernamaihsan 0:804d45caee8d 20 return 0 ;
alienbernamaihsan 0:804d45caee8d 21 }
alienbernamaihsan 0:804d45caee8d 22
alienbernamaihsan 0:804d45caee8d 23 _cmps11.putc(0xF5);
alienbernamaihsan 0:804d45caee8d 24 while( !(_cmps11.readable()))
alienbernamaihsan 0:804d45caee8d 25 {}
alienbernamaihsan 0:804d45caee8d 26 if( _cmps11.getc() != 0x55 )
alienbernamaihsan 0:804d45caee8d 27 {
alienbernamaihsan 0:804d45caee8d 28 return 0 ;
alienbernamaihsan 0:804d45caee8d 29 }
alienbernamaihsan 0:804d45caee8d 30
alienbernamaihsan 0:804d45caee8d 31 _cmps11.putc(0xF7);
alienbernamaihsan 0:804d45caee8d 32 while( !(_cmps11.readable()))
alienbernamaihsan 0:804d45caee8d 33 {}
alienbernamaihsan 0:804d45caee8d 34 if( _cmps11.getc() != 0x55 )
alienbernamaihsan 0:804d45caee8d 35 {
alienbernamaihsan 0:804d45caee8d 36 return 0 ;
alienbernamaihsan 0:804d45caee8d 37 }
alienbernamaihsan 0:804d45caee8d 38 return 1 ;
alienbernamaihsan 0:804d45caee8d 39 }
alienbernamaihsan 0:804d45caee8d 40
alienbernamaihsan 0:804d45caee8d 41 char cmps11::stopCalibration()
alienbernamaihsan 0:804d45caee8d 42 {
alienbernamaihsan 0:804d45caee8d 43 _cmps11.putc(0xF8);
alienbernamaihsan 0:804d45caee8d 44 while( !(_cmps11.readable()))
alienbernamaihsan 0:804d45caee8d 45 {}
alienbernamaihsan 0:804d45caee8d 46 if( _cmps11.getc() != 0x55 )
alienbernamaihsan 0:804d45caee8d 47 {
alienbernamaihsan 0:804d45caee8d 48 return 0 ;
alienbernamaihsan 0:804d45caee8d 49 }
alienbernamaihsan 0:804d45caee8d 50 cmps11::set0degree() ;
alienbernamaihsan 0:804d45caee8d 51 return 1 ;
alienbernamaihsan 0:804d45caee8d 52 }
alienbernamaihsan 0:804d45caee8d 53
alienbernamaihsan 0:804d45caee8d 54 int cmps11::readCompassAngle16Bit()
alienbernamaihsan 0:804d45caee8d 55 {
alienbernamaihsan 0:804d45caee8d 56 _cmps11.putc(0x13);
alienbernamaihsan 0:804d45caee8d 57 while ( !(_cmps11.readable()) ) ;
alienbernamaihsan 0:804d45caee8d 58 buffer[0] = _cmps11.getc() ;
alienbernamaihsan 0:804d45caee8d 59 while ( !(_cmps11.readable()) ) ;
alienbernamaihsan 0:804d45caee8d 60 buffer[1] = _cmps11.getc() ;
alienbernamaihsan 0:804d45caee8d 61
alienbernamaihsan 0:804d45caee8d 62 return ( ( buffer[0] << 8 ) | buffer[1] ) ;
alienbernamaihsan 0:804d45caee8d 63
alienbernamaihsan 0:804d45caee8d 64 }
alienbernamaihsan 0:804d45caee8d 65
alienbernamaihsan 0:804d45caee8d 66 void cmps11::set0degree()
alienbernamaihsan 0:804d45caee8d 67 {
alienbernamaihsan 0:804d45caee8d 68 sudut = 0 ;
alienbernamaihsan 0:804d45caee8d 69 readnow = cmps11::readCompassAngle16Bit() ;
alienbernamaihsan 0:804d45caee8d 70 readlast = readnow ;
alienbernamaihsan 0:804d45caee8d 71 }
alienbernamaihsan 0:804d45caee8d 72
alienbernamaihsan 0:804d45caee8d 73 int cmps11::readAngle16Bit()
alienbernamaihsan 0:804d45caee8d 74 {
alienbernamaihsan 0:804d45caee8d 75 readnow = cmps11::readCompassAngle16Bit() ;
alienbernamaihsan 0:804d45caee8d 76 bufferx = readnow-readlast ;
alienbernamaihsan 0:804d45caee8d 77 readlast = readnow ;
alienbernamaihsan 0:804d45caee8d 78 sudut += bufferx ;
alienbernamaihsan 0:804d45caee8d 79
alienbernamaihsan 0:804d45caee8d 80 if ( (( sudut < 0 ) || ( sudut > 3599 )))
alienbernamaihsan 0:804d45caee8d 81 {
alienbernamaihsan 0:804d45caee8d 82 modenow = 1 ;
alienbernamaihsan 0:804d45caee8d 83 }
alienbernamaihsan 0:804d45caee8d 84 else
alienbernamaihsan 0:804d45caee8d 85 {
alienbernamaihsan 0:804d45caee8d 86 modenow = 0 ;
alienbernamaihsan 0:804d45caee8d 87 }
alienbernamaihsan 0:804d45caee8d 88
alienbernamaihsan 0:804d45caee8d 89 if ( modelast != modenow )
alienbernamaihsan 0:804d45caee8d 90 {
alienbernamaihsan 0:804d45caee8d 91 if ( sudut < 0 )
alienbernamaihsan 0:804d45caee8d 92 {
alienbernamaihsan 0:804d45caee8d 93 sudut += 3599 ;
alienbernamaihsan 0:804d45caee8d 94 }
alienbernamaihsan 0:804d45caee8d 95 else if ( sudut > 3599 )
alienbernamaihsan 0:804d45caee8d 96 {
alienbernamaihsan 0:804d45caee8d 97 sudut -= 3599 ;
alienbernamaihsan 0:804d45caee8d 98 }
alienbernamaihsan 0:804d45caee8d 99 modelast = modenow ;
alienbernamaihsan 0:804d45caee8d 100 }
alienbernamaihsan 0:804d45caee8d 101 return ( sudut );
alienbernamaihsan 0:804d45caee8d 102 }