Shih-Ho Hsieh / XYZ_sensor_Platform

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.

Files at this revision

API Documentation at this revision

Revision 15:5f791bd5dd97, committed 2018-08-08

Comitter:
hober
Date:
Wed Aug 08 07:58:15 2018 +0000
Parent:
14:1363e4b2fbac
Commit message:
2018/08/08 branch 5 magnetometers.

Changed in this revision

motor/motor.h Show annotated file Show diff for this revision Revisions of this file
xyz_sensor_platform.cpp Show annotated file Show diff for this revision Revisions of this file
xyz_sensor_platform.h Show annotated file Show diff for this revision Revisions of this file 
--- a/motor/motor.h	Mon Jul 02 07:41:35 2018 +0000
+++ b/motor/motor.h	Wed Aug 08 07:58:15 2018 +0000
@@ -23,13 +23,23 @@
         return (float)_TXcm_s/(_onDelay+_offDelay);
     }
     void reset() {
+        if(_lock){
+            return;
+        }
         _pos=0;
     }
     void to(float p);
     float position() {
+        if(_lock){
+            return -1;
+        }
         return _pos;
     }
+    bool moving(){ return _lock; }
     void setPosition(float p) {
+        if(_lock){
+            return;
+        }
         _pos = p;
     }
     void stop(void);

--- a/xyz_sensor_platform.cpp	Mon Jul 02 07:41:35 2018 +0000
+++ b/xyz_sensor_platform.cpp	Wed Aug 08 07:58:15 2018 +0000
@@ -7,7 +7,7 @@
     yEnd(MOTOR_Y_PINT_END),
     zZero(MOTOR_Z_PINT_ZERO),
     zEnd(MOTOR_Z_PINT_END),
-    movedTimes(0)
+    _movedTimes(0)
 {
     motorX = new CD_2D34M("motorX\0", \
                           MOTOR_X_POUT_DIRECTION, \
@@ -44,14 +44,19 @@
 }    
 void XYZSensorPlatform::reset() {
     /** Motor Initial **/
-    movedTimes = 0;
+    _movedTimes = 0;
     x = 1;
     y = 1;
     z = 1;
     int step = 0;
-    while(xEnd!=0 && step++ < 10) go_right();
+    while(xEnd!=0 && step++ < 10)
+    {
+        go_right();
+        _movedTimes = 0;
+    }
     while(xZero!=0) {
         go_left();
+        _movedTimes = 0;
         wait(0.01);
     }
     go_right();
@@ -59,6 +64,7 @@
     while(yEnd!=0 && step++ < 10) go_forward();
     while(yZero!=0 && yZero != 0) {
         go_backward();
+        _movedTimes = 0;
         wait(0.01);
     }
     go_forward();
@@ -66,6 +72,7 @@
     while(zEnd!=0 && step++ < 10) go_up();
     while(zZero!=0) {
         go_down();
+        _movedTimes = 0;
         wait(0.01);
     }
     go_up();
@@ -125,3 +132,14 @@
     } else if(yEnd == 1) motorY->to(y);
 }
 
+void XYZSensorPlatform::checkMovedTimes()
+{
+    if(_movedTimes++>=CALIBRATION_TIME)
+    {
+        float p[3] = {0.0};
+//        position(p);
+        reset();
+//        to(p[0],p[1],p[2]);
+//        _movedTimes = 0;
+    }
+}

--- a/xyz_sensor_platform.h	Mon Jul 02 07:41:35 2018 +0000
+++ b/xyz_sensor_platform.h	Wed Aug 08 07:58:15 2018 +0000
@@ -30,6 +30,8 @@
     void go_right();
     void go_left();
     
+    bool isMoving(){ return motorX->moving() || motorY->moving() || motorZ->moving(); }
+    
     void set_speed(float speed) {
         motorX->setSpeed(speed);
         motorY->setSpeed(speed);
@@ -79,18 +81,10 @@
     
     void setSensorI2cFrequency(int hz){ sensor->setI2cFrequency(hz); }
 private:
-    void checkMovedTimes(){
-        if(movedTimes++>=CALIBRATION_TIME)
-        {
-            float p[3] = {0.0};
-            position(p);
-            reset();
-            to(p[0],p[1],p[2]);
-        }
-    }
+    void checkMovedTimes();
     Motor *motorX, *motorY, *motorZ;
     float x,y,z;
-    int movedTimes;
+    int _movedTimes;
     InterruptIn xZero, xEnd, yZero, yEnd, zZero, zEnd;
     AltIMU_10_v5 *sensor;
     GyroSensor *gyroscope;