MPU6050 with Serial YAW Only not Work Work in Progress

Dependents:   R1Arobo_Maika_B 2021Arobo_UMAPYOI 2021Arobo_YUMIPYOI

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers Serial6050.cpp Source File

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 }