Revision 1:edc6b5cc7112, committed 2019-01-07

Comitter:

hober

Date:

Mon Jan 07 03:32:36 2019 +0000

Parent:

0:9e6e3dc903c0

Commit message:

20190107 for motor control

Changed in this revision

--- a/XYZ_sensor_Platform/motor/linear_slide.cpp	Sat Dec 22 01:07:34 2018 +0000
+++ b/XYZ_sensor_Platform/motor/linear_slide.cpp	Mon Jan 07 03:32:36 2019 +0000
@@ -31,11 +31,12 @@
 {
     if(moving() || (pos == _pos)) return;
     float diffPos = pos-_pos;
-    Direction dir = (diffPos > 0 ? CCW : CW);
+//    if(diffPos < 0.01f && diffPos > -0.01f) return;
+    Direction dir = (diffPos > 0.0f ? CCW : CW);
     _dir = (dir == CCW ? MOTOR_SIDE : NON_MOTOR_SIDE);
     if(!isInBound()) return;
-    diffPos = (diffPos > 0 ? diffPos : -diffPos);
-    int pulseCount = countPulseNumber(diffPos); // 1mm
+    diffPos = (diffPos > 0.0f ? diffPos : -diffPos);
+    int pulseCount = countPulseNumber(diffPos); 
     _p_motor->setRotateFinishEvent(callback(this,&LinearSlide::goCallback));
     _p_motor->setRotateCheckEvent(callback(this,&LinearSlide::isInBound));
     _p_motor->setSpeed((int)(_vel*60.0f/(float)(_pitch)));
@@ -53,6 +54,11 @@
 
 float LinearSlide::setSpeed(float cmps)
 {
+    if(moving()){
+        _resetVel = cmps;
+        return;
+    }
+    _resetVel = cmps;
     _vel = cmps;
     int rpm = (int)(_vel*60.0f/(float)(_pitch));
     int rpm_set = _p_motor->setSpeed(rpm);
@@ -72,6 +78,7 @@
 
 void LinearSlide::toZero()
 {
+    if(!isInBound()) return;
     _dir = NON_MOTOR_SIDE;
     int pulseCount = countPulseNumber(10); // 10cm
     _p_motor->setRotateFinishEvent(callback(this,&LinearSlide::reset));

--- a/XYZ_sensor_Platform/motor/linear_slide.h	Sat Dec 22 01:07:34 2018 +0000
+++ b/XYZ_sensor_Platform/motor/linear_slide.h	Mon Jan 07 03:32:36 2019 +0000
@@ -18,7 +18,7 @@
     bool moving(){ return _p_motor->moving(); }
     void reset(){
         setPosition(0);
-        _vel = 0;
+        setSpeed(_resetVel);
     }
     float setSpeed(float cmps);
     void setPosition(float cm)
@@ -46,6 +46,7 @@
     float _pitch; // in cm
     float _pos;
     float _vel; // in cm/s
+    float _resetVel;
     int _dir;
     DigitalIn _zero, _end;
     Callback<void()> _nextEvent;

--- a/XYZ_sensor_Platform/motor/motor.cpp	Sat Dec 22 01:07:34 2018 +0000
+++ b/XYZ_sensor_Platform/motor/motor.cpp	Mon Jan 07 03:32:36 2019 +0000
@@ -25,26 +25,28 @@
 void Motor::toggleOn(void)
 {
     _pulse = 1;
+    if(_pulseCounter > _goalPulseCount && _rotateDirection == CCW)
+    {
+        _rotateDirection = CW;
+        _dir = _cw;
+    }
+    else if(_pulseCounter < _goalPulseCount && _rotateDirection == CW)
+    {
+        _rotateDirection = CCW;
+        _dir = _ccw;
+    }
     _pulseCounter += (_rotateDirection==CCW?1:-1);
-    if(_pulseCounter != _goalPulseCount)
+    
+    if( _pulseCounter != _goalPulseCount)
     {
         bool check = true;
         if(_rotateCheckEvent) check = _rotateCheckEvent();
-        if(check) _pulseTimeout.attach_us(callback(this, &Motor::toggleOff), _onDelay);
-        else
-        {
-            _lock = false;
-            if(_rotateFinishEvent) _rotateFinishEvent();
-            _rotateFinishEvent = NULL;
-            _rotateCheckEvent = NULL;
+        if(check){
+            _pulseTimeout.attach_us(callback(this, &Motor::toggleOff), _onDelay);
+            return;
         }
     }
-    else {
-        _lock = false;
-        if(_rotateFinishEvent) _rotateFinishEvent();
-        _rotateFinishEvent = NULL;
-        _rotateCheckEvent = NULL;
-    }
+    stop();
 }
 
 void Motor::toggleOff(void)
@@ -56,8 +58,10 @@
 void Motor::stop(void)
 {
     _pulseTimeout.detach();
-//    _rotateTimeout.detach();
     _lock = false;
+    if(_rotateFinishEvent) _rotateFinishEvent();
+    _rotateFinishEvent = NULL;
+    _rotateCheckEvent = NULL;
 }
 void Motor::rotate(Direction direction, int pulseCount)
 {
@@ -65,6 +69,7 @@
         return;
     }
     _lock = true;
+    pulseCount = (pulseCount > 0 ? pulseCount : -pulseCount);
     _pulseTimeout.detach();
     _pulse = 0;
     _rotateDirection = direction;

--- a/XYZ_sensor_Platform/motor/motor.h	Sat Dec 22 01:07:34 2018 +0000
+++ b/XYZ_sensor_Platform/motor/motor.h	Mon Jan 07 03:32:36 2019 +0000
@@ -47,6 +47,7 @@
     
 protected:
     void set_cw_signal(int cw){
+        cw = (cw > 0 ? 1 : 0);
         _cw = cw;
         _ccw = 1-_cw;
     }

--- a/XYZ_sensor_Platform/xyz_sensor_platform.cpp	Sat Dec 22 01:07:34 2018 +0000
+++ b/XYZ_sensor_Platform/xyz_sensor_platform.cpp	Mon Jan 07 03:32:36 2019 +0000
@@ -66,39 +66,6 @@
 }    
 void XYZSensorPlatform::reset() {
     /** Motor Initial **/
-    /*
-    _movedTimes = 0;
-    int step = 0;
-    while(xEnd!=0 && step++ < 10)
-    {
-        go_right();
-        _movedTimes = 0;
-    }
-    while(xZero!=0) {
-        go_left();
-        _movedTimes = 0;
-        wait(0.01);
-    }
-    go_right();
-    step = 0;
-    while(yEnd!=0 && step++ < 10) go_forward();
-    while(yZero!=0 && yZero != 0) {
-        go_backward();
-        _movedTimes = 0;
-        wait(0.01);
-    }
-    go_forward();
-    step = 0;
-    while(zEnd!=0 && step++ < 10) go_up();
-    while(zZero!=0) {
-        go_down();
-        _movedTimes = 0;
-        wait(0.01);
-    }
-    go_up();
-    xSlide->reset();
-    ySlide->reset();
-    zSlide->reset();*/
     xSlide->toZero();
     ySlide->toZero();
     zSlide->toZero();
@@ -106,66 +73,36 @@
 
 void XYZSensorPlatform::go_forward()
 {
-//    checkMovedTimes();
-    if(yEnd == 1) ySlide->go(MOTOR_SIDE);
+    ySlide->go(MOTOR_SIDE);
 }
 
 void XYZSensorPlatform::go_backward()
 {
-//    checkMovedTimes();
-    if(yZero == 1) ySlide->go(NON_MOTOR_SIDE);
+    ySlide->go(NON_MOTOR_SIDE);
 }
 
 void XYZSensorPlatform::go_up()
 {
-//    checkMovedTimes();
-    if(zEnd == 1) zSlide->go(MOTOR_SIDE);
+    zSlide->go(MOTOR_SIDE);
 }
 
 void XYZSensorPlatform::go_down()
 {
-//    checkMovedTimes();
-    if(zZero == 1) zSlide->go(NON_MOTOR_SIDE);
+    zSlide->go(NON_MOTOR_SIDE);
 }
 
 void XYZSensorPlatform::go_right()
 {
-//    checkMovedTimes();
-    if(xEnd != 0) xSlide->go(MOTOR_SIDE);
-//    printf("P");
-//    }else { printf("S"); }
+    xSlide->go(MOTOR_SIDE);
 }
 
 void XYZSensorPlatform::go_left()
 {
-//    checkMovedTimes();
-    if(xZero == 1) xSlide->go(NON_MOTOR_SIDE);
+    xSlide->go(NON_MOTOR_SIDE);
 }
 void XYZSensorPlatform::to(float x, float y, float z)
 {
-//    checkMovedTimes();
-    float pos[3];
-    position(pos);
-    if(pos[2] > z) {
-        if(zZero == 1) zSlide->to(z);
-    } else if(zEnd == 1) zSlide->to(z);
-    if(pos[0] > x) {
-        if(xZero == 1) xSlide->to(x);
-    } else if(xEnd == 1) xSlide->to(x);
-    if(pos[1] > y) {
-        if(yZero == 1) ySlide->to(y);
-    } else if(yEnd == 1) ySlide->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;
-//    }
-//}
-
+    zSlide->to(z);
+    xSlide->to(x); 
+    ySlide->to(y);
+}
\ No newline at end of file

--- a/XYZ_sensor_Platform/xyz_sensor_platform.h	Sat Dec 22 01:07:34 2018 +0000
+++ b/XYZ_sensor_Platform/xyz_sensor_platform.h	Mon Jan 07 03:32:36 2019 +0000
@@ -7,7 +7,7 @@
 #include "linear_slide.h"
 #include "motor_targets.h"
 #define MOTOR_INITIAL
-#define CALIBRATION_TIME 100
+//#define CALIBRATION_TIME 100
 #define MOTOR_RESOLUTION 200
 
 /*** Typedefs ----------------------------------------------------------------- ***/

--- a/ui.cpp	Sat Dec 22 01:07:34 2018 +0000
+++ b/ui.cpp	Mon Jan 07 03:32:36 2019 +0000
@@ -5,6 +5,7 @@
 #define BIG_CHAR_MASK 0x1F
 #define BAUD 921600
 #define Fs 1e2 // sampling rate -- max: 1kHz
+#define NORMAL_SPEED 2
 
 typedef unsigned char byte;
 uint8_t* dataToSend;
@@ -32,6 +33,7 @@
 int downCount = 0;
 int forwardCount = 0;
 int backwardCount = 0;
+float speed = (float)NORMAL_SPEED;
 bool commandToDo = false;
 int recordTime;
 bool isEcho = false;
@@ -75,7 +77,7 @@
     tracker.setBufferLength(100);
     pattern.state = NONE;
     pc.format(8,SerialBase::None,1);
-    platform.set_speed(2.5);
+    platform.set_speed(speed);
     platform.setSensorSpiFrequency(SPI_FREQUENCY);
 // Setup a serial interrupt function to receive data
     pc.attach(&Rx_interrupt, Serial::RxIrq);
@@ -83,10 +85,10 @@
     while(1) {
         if(platform.isMoving()) continue;
         if(isReset){
+            isReset = false;
             platform.set_speed(1);
             platform.reset();
-            isReset = false;
-            platform.set_speed(2.5);
+            platform.set_speed(speed);
         }
         if(pattern.state != NONE)
         {
@@ -204,6 +206,7 @@
             getMag--;
         }
         if(commandToDo) {
+//            printf("%f, %f, %f\n", x,y,z);
             platform.to(x,y,z);
             isEcho = true;
             commandToDo = false;
@@ -261,8 +264,10 @@
     char tmp[] = {typ, p_data[0]>>8, p_data[0], p_data[1]>>8, p_data[1], p_data[2]>>8, p_data[2]};
     command->setEnvelopeData(tmp,7);
     dataToSend = (uint8_t*)(command->getEnvelopeArray());
-    if(pc.writeable()&&isWriteNow)
+    if(isWriteNow)
     {
+       int i = 0;
+       while(!pc.writeable() && i < 10000) i++;
        for(int i = 0; i < 10; i++) pc.putc(dataToSend[i]);
     }
     return dataToSend;
@@ -335,9 +340,9 @@
                     break;
                 case 'C': // command
                     if(commandToDo) break;
-                    x=(float)((dataArray[1]<<8)+dataArray[2])/10.0f;
-                    y=(float)((dataArray[3]<<8)+dataArray[4])/10.0f;
-                    z=(float)((dataArray[5]<<8)+dataArray[6])/10.0f;
+                    x=(float)((int(dataArray[1])<<8)+int(dataArray[2]))/10.0f;
+                    y=(float)((int(dataArray[3])<<8)+int(dataArray[4]))/10.0f;
+                    z=(float)((int(dataArray[5])<<8)+int(dataArray[6]))/10.0f;
                     commandToDo = true;
                     break;
                 case 'X': // move in X direction
@@ -356,7 +361,7 @@
                     getMag++;
                     magSel = dataArray[1];
                     isTimeToRecord = true;
-                    pc.putc('M');
+//                    pc.putc('M');
                     break;
                 case 'R': // record
                     recordTime = dataArray[1];
@@ -380,10 +385,14 @@
                 case 'N': // new set up
                     isReset = true;
                     break;
-                case 'B':
+                case 'B': // reset magnetometers
                     magSel = dataArray[1];
                     isMagReset = true;
                     break;
+                case 'V': // set speed
+                    speed = float(dataArray[1])/10.0f;
+                    platform.set_speed(speed);
+                    break;
                 default:
                     break;
             } // end switch