Diff: R1370.cpp

Revision:

5:553c02009ad0

Parent:

4:ffc2a55269f9

--- a/R1370.cpp	Sat May 18 08:42:05 2019 +0000
+++ b/R1370.cpp	Wed Dec 16 08:31:38 2020 +0000
@@ -2,6 +2,7 @@
 
 R1370::R1370(PinName tx, PinName rx) : RawSerial(tx, rx, 115200)
 {
+    zeroPoint = 0;
     attach(callback(this, &R1370::receiveByte));
 }
 
@@ -61,6 +62,17 @@
     return (float)(angle / 100.0);
 }
 
+float R1370::getRelativeAngle()
+{
+    relativeAngle = (float)((angle - zeroPoint) / 100.0);
+    if(relativeAngle > 180.0){
+        return relativeAngle - 360.0;
+    }else if(relativeAngle < -180.0){
+        return relativeAngle + 360.0;
+    }
+    return relativeAngle;
+}
+
 float R1370::getRate()
 {
     return (float)(rate / 100.0);
@@ -71,6 +83,11 @@
     return (float)(angle / 100.0 / 180.0 * 3.141592653589793);
 }
 
+float R1370::getRelativeRadian()
+{
+    return getRelativeAngle() / 180.0 * 3.141592653589793;
+}
+
 int16_t R1370::getAcc(char l)
 {
     if(l == 'x' || l == 'X') {
@@ -116,4 +133,9 @@
 unsigned char R1370::downbit()
 {
     return downbit_;
+}
+
+void R1370::reset()
+{
+    zeroPoint = angle;
 }
\ No newline at end of file