MPU6050 with Serial YAW Only not Work Work in Progress
Dependents: R1Arobo_Maika_B 2021Arobo_UMAPYOI 2021Arobo_YUMIPYOI
Serial6050.cpp
00001 #include "Serial6050.h" 00002 00003 Serial6050::Serial6050(PinName tx, PinName rx, PinName resetPin) 00004 : serial(tx, rx, 38400), rst(resetPin), Deg(0), Bias(0) 00005 { 00006 if (resetPin == NC) useResetPin = false; 00007 else useResetPin = true; 00008 } 00009 00010 void Serial6050::init() 00011 { 00012 if (useResetPin) { 00013 rst = 1; 00014 wait(0.1); 00015 rst = 0; 00016 } 00017 while(1) { 00018 serial.putc(100); 00019 if(serial.readable()) { 00020 if(serial.getc()) break; 00021 } 00022 } 00023 serial.attach(this, &Serial6050::intReceive, RawSerial::RxIrq); 00024 wait(1); 00025 reset(); 00026 } 00027 00028 void Serial6050::intReceive() 00029 { 00030 data = serial.getc(); 00031 static uint8_t old = data; 00032 00033 if(data > old && data - old > 127) { 00034 Bias -= 255; 00035 } else if(data < old && old - data > 127) { 00036 Bias += 255; 00037 } else { 00038 00039 } 00040 Deg = data + Bias; 00041 old = data; 00042 } 00043 00044 void Serial6050::reset() 00045 { 00046 resetValue = Deg; 00047 } 00048 00049 00050 00051 float Serial6050::read() 00052 { 00053 return (Deg - resetValue) / 10.0; 00054 }
Generated on Thu Aug 18 2022 00:49:12 by
1.7.2