Diff: xyz_sensor_platform.h

Revision:

14:1363e4b2fbac

Parent:

13:1aa1d5ec949b

Child:

15:db753c75e53f

--- a/xyz_sensor_platform.h	Tue Mar 06 08:35:33 2018 +0000
+++ b/xyz_sensor_platform.h	Mon Jul 02 07:41:35 2018 +0000
@@ -7,7 +7,7 @@
 #include "motor.h"
 #include "motor_targets.h"
 #define MOTOR_INITIAL
-#define CALIBRATION_TIME 400
+#define CALIBRATION_TIME 100
 
 /*** Typedefs ----------------------------------------------------------------- ***/
 typedef struct {
@@ -79,19 +79,18 @@
     
     void setSensorI2cFrequency(int hz){ sensor->setI2cFrequency(hz); }
 private:
-    void x_zero();
-    void x_end();
-    void y_zero();
-    void y_end();
-    void z_zero();
-    void z_end();
     void checkMovedTimes(){
-        if(movedTimes++>=CALIBRATION_TIME) reset();
+        if(movedTimes++>=CALIBRATION_TIME)
+        {
+            float p[3] = {0.0};
+            position(p);
+            reset();
+            to(p[0],p[1],p[2]);
+        }
     }
     Motor *motorX, *motorY, *motorZ;
     float x,y,z;
     int movedTimes;
-    bool is_x_end, is_y_end, is_z_end, is_x_zero, is_y_zero, is_z_zero;
     InterruptIn xZero, xEnd, yZero, yEnd, zZero, zEnd;
     AltIMU_10_v5 *sensor;
     GyroSensor *gyroscope;