File content as of revision 6:41d7cf11fdb1:

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


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


int MOTION_CONTROL::limit_motor(uint8_t dirction)
{

    if(_limit_up) {
        //while(_limit_up!=0) {
        motor.SetMotor(2);
        //}
        return 1;
    } else if(_limit_down) {

        motor.SetMotor(1);

        // }
        //motor.StopMotor();
        return -1;
    } else {
        motor.SetMotor(dirction);
        return 0;//Normally

    }

//motor.SetMotor(dirction);
//return 0;
}

int8_t MOTION_CONTROL::position_control(uint16_t target)
{
    uint16_t current =Scale(_position.read_u16());
    //target = Scale(target);
    if(target > scale) {
        return -3;
    }

    error = target-current;
    if(error >scale || error < -scale) {
        // pc.printf("data error\n");
        return -2;
    } 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()
{
    int state=0;
    uint32_t sum=0;

    do {
        state = limit_motor(1);
    } while(state==0);

    do {
        state = limit_motor(2);
    } while(state!=1);
    wait_ms(200);
    motor.StopMotor();
    wait_ms(500);

    sum=0;
    for(int a=0; a<LOOP; a++) {
        sum += _position.read_u16();
    }
    MAX_POSITION = sum/LOOP;

    do {
        state = limit_motor(2);
    } while(state==0);
    //motor.StopMotor();
    do {
        state = limit_motor(1);
    } while(state!=0);
    wait_ms(100);
    motor.StopMotor();

    sum=0;
    for(int a=0; a<LOOP; a++) {
        sum += _position.read_u16();
    }
    MIN_POSITION = sum/LOOP;
}

int MOTION_CONTROL::GetLimitUp()
{
    return _limit_up;
}

int MOTION_CONTROL::GetLimitDown()
{
    return _limit_down;
}

void MOTION_CONTROL::SetMargin(int16_t data)
{
    MARGIN = data;
}
int16_t MOTION_CONTROL::GetMargin()
{
    return MARGIN;
}

uint16_t MOTION_CONTROL::GetMaxPosition()
{
    return MAX_POSITION;
}

uint16_t MOTION_CONTROL::GetMinPosition()
{
    return MIN_POSITION;
}

void MOTION_CONTROL::SetMaxPosition(uint16_t value)
{
    MAX_POSITION = value;
}
void MOTION_CONTROL::SetMinPosition(uint16_t value)
{
    MIN_POSITION = value;
}

uint16_t MOTION_CONTROL::Scale(uint16_t data)
{
    return ((float)(data-MIN_POSITION)/(MAX_POSITION - MIN_POSITION))*scale;
}

uint16_t MOTION_CONTROL::GetAnalog()
{
    return _position.read_u16();
}

uint16_t MOTION_CONTROL::GetPosition()
{
    return Scale(_position.read_u16());
}

void MOTION_CONTROL::stop()
{
    motor.StopMotor();
}