File content as of revision 1:5b313fd2ca6f:

#include "mbed.h"
#include "motion_control.h"
#include "motor_relay.h"

//int16_t MOTION_CONTROL::error=0;
//int16_t MOTION_CONTROL::MARGIN=500;


MOTION_CONTROL::MOTION_CONTROL(PinName dirA, PinName dirB, PinName limitUp, PinName limitDown, PinName vr): _limit_up(limitUp), _limit_down(limitDown), _position(vr)
{
    MOTOR_RELAY *motor = new MOTOR_RELAY(dirA,dirB);
    error=0;
    MARGIN=500;
}


int8_t MOTION_CONTROL::limit_motor(uint8_t dirction)
{
    if(_limit_up) {
        motor->StopMotor();
        return 1;
    } else {
        if(_limit_down) {
            motor->StopMotor();
            return -1;
        } else {
            motor->SetMotor(dirction);
            return 0;//Normally
        }
    }
}

int8_t MOTION_CONTROL::position_control(uint16_t current, uint16_t target)
{
    error = target-current;
    if(error >scale || error < -scale) {
        //pc.printf("data error\n");
        
    } else {
        if(error > MARGIN) {
            if(limit_motor(1)==0 ) { //limit sens
                //pc.printf("motor[%d]=limit error\n",id);
                return limit_motor(1);
            }
        } else if(error < -MARGIN) {
            if(limit_motor(2)!=0 ) { //limit sens
                //pc.printf("motor[%d]=limit error\n",id);
                return limit_motor(2);
            }
        } else {    //in zone
            motor->StopMotor();
            //pc.printf("motor[%d]=complete\n",id);
            return 2; //in zone complete
        }

        //pc.printf("motor[%d]=in process\n",id);
        return 0; //in process
    }
}

void MOTION_CONTROL::calibration()
{
    //pc.printf("motor[1] run up\n");
    do {
        if(_limit_up == 0) {
            motor->StopMotor();
        } else {
            motor->SetMotor(1);
        }
    } while(_limit_up ==0);

    //pc.printf("motor[1] stop up\n");
    wait_ms(500);
    do {
        motor->SetMotor(2);
    } while(_limit_up);
    motor->StopMotor();
    wait_ms(500);
    //pc.printf("motor[1] read position\n");

    MAX_POSITION = _position.read_u16();

    //pc.printf("max_pos_LU= %d\n",max_pos_LU);
    //pc.printf("motor[1] run down\n");

    do {
        if(_limit_down == 0) {
            motor->StopMotor();
        } else {
            motor->SetMotor(2);
        }
    } while(_limit_down==0) ;
    //pc.printf("motor[1] stop down\n");
    do {
        motor->SetMotor(1);
    } while(_limit_down);
    motor->StopMotor();
    wait_ms(500);
    //pc.printf("motor[1] read position\n");
    MIN_POSITION = _position.read_u16();
    //pc.printf("min_pos_LU= %d\n",min_pos_LU);

}