NIT Fukui / Serial6050Yaw

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   R1Arobo_Maika_B 2021Arobo_UMAPYOI 2021Arobo_YUMIPYOI

Diff: Serial6050.cpp

Revision:
2:00a8daf9203b
Parent:
1:97a3a91ce950
Child:
3:601cfc41e50a
--- a/Serial6050.cpp	Wed Sep 26 00:55:39 2018 +0000
+++ b/Serial6050.cpp	Fri Oct 05 07:34:34 2018 +0000
@@ -1,7 +1,7 @@
 #include "Serial6050.h"
 
 Serial6050::Serial6050(PinName tx, PinName rx, PinName resetPin)
-    : serial(tx, rx, 38400), rst(resetPin)
+    : serial(tx, rx, 38400), rst(resetPin), Deg(0), Bias(0)
 {
 }
 
@@ -21,15 +21,30 @@
     reset();
 }
 
+void Serial6050::intReceive()
+{
+    data = serial.getc();
+    static uint8_t old = data;
+
+    if(data > old && data - old > 127) {
+        Bias -= 255;
+    } else if(data < old && old - data > 127) {
+        Bias += 255;
+    } else {
+        
+    }
+    Deg = data + Bias;
+    old = data;
+}
+
 void Serial6050::reset()
 {
-    resetValue = lower - 127;
+    resetValue = Deg;
 }
 
 
 
 float Serial6050::read()
 {
-    int value = lower;
-    return (value - 127 - resetValue) / 10.0;
+    return (Deg - resetValue) / 10.0;
 }
\ No newline at end of file