Diff: xyz_sensor_platform.cpp

Revision:

11:f34acfd40dcd

Parent:

10:126524571b17

Child:

12:e2b7e030193c

--- a/xyz_sensor_platform.cpp	Tue Dec 12 08:45:49 2017 +0000
+++ b/xyz_sensor_platform.cpp	Fri Jan 05 06:51:33 2018 +0000
@@ -30,7 +30,7 @@
     accelerometer = sensor->GetAccelerometer();
     magnetometer = sensor->magnetometer;
     temp_sensor = sensor->pt_sensor;
-    //init();
+    init();
 }
 void XYZSensorPlatform::init(void)
 {
@@ -54,18 +54,21 @@
         go_left();
         wait(0.01);
     }
+    go_right();
     step = 0;
     while(yEnd!=0 && step++ < 10) go_forward();
     while(y!=0 && yZero != 0) {
         go_backward();
         wait(0.01);
     }
+    go_forward();
     step = 0;
     while(zEnd!=0 && step++ < 10) go_up();
     while(z!=0) {
         go_down();
         wait(0.01);
     }
+    go_up();
     xEnd.fall(this,&XYZSensorPlatform::x_end);
     yEnd.fall(this,&XYZSensorPlatform::y_end);
     zEnd.fall(this,&XYZSensorPlatform::z_end);
@@ -75,53 +78,40 @@
 void XYZSensorPlatform::x_zero()
 {
     motorX->stop();
-//    wait(0.01);
-    while(xZero!=1) go_right();
     motorX->reset();
     x=0;
-    printf("x_0\n");
 }
 
 void XYZSensorPlatform::y_zero()
 {
     motorY->stop();
-    while(yZero!=1) go_forward();
     motorY->reset();
     y=0;
-    printf("y_0\n");
 }
 
 void XYZSensorPlatform::z_zero()
 {
     motorZ->stop();
-    while(zZero!=1) go_up();
     motorZ->reset();
     z=0;
-    printf("z_0\n");
 }
 
 void XYZSensorPlatform::x_end()
 {
-    printf("x_e\n");
     motorX->stop();
     is_x_end = true;
-//    while(xEnd!=1) go_left();
 }
 
 void XYZSensorPlatform::y_end()
 {
-    printf("y_e\n");
     motorY->stop();
     is_y_end = true;
-//    while(yEnd!=1)  go_backward();
 }
 
 void XYZSensorPlatform::z_end()
 {
-    printf("z_e\n");
     motorZ->stop();
     is_z_end = true;
-//    while(zEnd!=1) go_down();
 }
 
 void XYZSensorPlatform::go_forward()
@@ -132,7 +122,7 @@
 void XYZSensorPlatform::go_backward()
 {
     if(is_y_end) is_y_end = false;
-    motorY->go(NON_MOTOR_SIDE);
+    if(yZero == 1) motorY->go(NON_MOTOR_SIDE);
 }
 
 void XYZSensorPlatform::go_up()
@@ -143,7 +133,7 @@
 void XYZSensorPlatform::go_down()
 {
     if(is_z_end) is_z_end = false;
-    motorZ->go(NON_MOTOR_SIDE);
+    if(zZero == 1) motorZ->go(NON_MOTOR_SIDE);
 }
 
 void XYZSensorPlatform::go_right()
@@ -154,7 +144,7 @@
 void XYZSensorPlatform::go_left()
 {
     if(is_x_end) is_x_end = false;
-    motorX->go(NON_MOTOR_SIDE);
+    if(xZero == 1) motorX->go(NON_MOTOR_SIDE);
 }
 void XYZSensorPlatform::to(float x, float y, float z)
 {
@@ -162,15 +152,15 @@
     position(pos);
     if(pos[0] > x) {
         if(is_x_end) is_x_end = false;
-        motorX->to(x);
+        if(xZero == 1) motorX->to(x);
     } else if(!is_x_end) motorX->to(x);
     if(pos[1] > y) {
         if(is_y_end) is_y_end = false;
-        motorY->to(y);
+        if(yZero == 1)motorY->to(y);
     } else if(!is_y_end) motorY->to(y);
     if(pos[2] > z) {
         if(is_z_end) is_z_end = false;
-        motorZ->to(z);
+        if(zZero == 1) motorZ->to(z);
     } else if(!is_z_end) motorZ->to(z);
 }