File content as of revision 14:c9611cf036ad:

#include "DC_Motor_Controller.h"

//************************************************//
// COMMENT NEXT LINE TO ENABLE/DISABLE DEBUG MODE //
//                                                //
//                #define DEBUG_MODE                //
//                                                //
//************************************************//



void DC_Motor_On_Off::goto_pulse(int tar_pulse, float speed){      //  Move motor to specific angle from home point
    int cur_pulse = dc_motor.getPulses();
    int direction = 0;
    
    #ifdef DEBUG_MODE 
    Serial device(USBTX, USBRX);  // tx, rx
    device.baud(9600);
    device.printf("\n------ current %d -> target %d -----\n", cur_pulse, tar_pulse);
    #endif
    
    while ( tar_pulse != cur_pulse ){
        direction = tar_pulse > cur_pulse; // cw 1 ; acw 0
      
        // if margin of error is less than +-3 (+- 1 degree), give up 
        if(abs(tar_pulse - cur_pulse) < 10)
        {
            #ifdef DEBUG_MODE 
            device.printf("GIVEUP ");
            #endif
            break;
        }
        
        // future direction diff from current motion, pause for 0.5 to prevent unexpected outputs
        else if (direction != (out1 > 0)) 
        {
            out1 = out2 = 0;
            #ifdef DEBUG_MODE 
            device.printf("PAUSE ");
            #endif
            wait(0.3); 
        }
      
        out1 = direction * speed;
        out2 = !direction * speed;
        
        cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
        
        #ifdef DEBUG_MODE 
        device.printf("[%d] ", cur_pulse);
        #endif

    }
    #ifdef DEBUG_MODE 
    device.printf("\n----- done -----\n");
    #endif
    out1 = 0;       //  Stop motor
    out2 = 0;
};

void DC_Motor_PID::set_pid(double Kp, double Ki, double Kd)
{
    #ifdef DEBUG_MODE 
    Serial device(USBTX, USBRX);  // tx, rx
    device.baud(9600);
    device.printf("Kp: %.3f, Ki: %.3f, Kd: %.3f", Kp, Ki, Kd);
    #endif
    this->pid=new PID(0.01,1,0.05,Kp,Kd,Ki);
}


void DC_Motor_PID::goto_pulse(int tar_pulse, float placeholder){      //  Move motor to specific angle from home point
    int cur_pulse = dc_motor.getPulses();
    
    #ifdef DEBUG_MODE 
    Serial device(USBTX, USBRX);  // tx, rx
    device.baud(9600);
    device.printf("\n------ PID current %d -> target %d -----\n", cur_pulse, tar_pulse);
    #endif
    
    while ( tar_pulse != cur_pulse ){
        double speed = this->pid->calculate(tar_pulse,cur_pulse);
        
        #ifdef DEBUG_MODE 
        if(!this->pid)
            device.printf("PID NOT SET BUT CALLED! NULL POINTER ");
        #endif
        // if margin of error is less than +-3 (+- 1 degree), give up 
        if(abs(tar_pulse - cur_pulse) < 0)
        {
            #ifdef DEBUG_MODE 
            device.printf("GIVEUP ");
            #endif
            break;
        }
      
        out1 = speed > 0 ? speed : 0;
        out2 = speed > 0 ? 0 : -speed;
        
        #ifdef DEBUG_MODE 
        device.printf("%.3f ", speed);
        #endif
        
        cur_pulse = dc_motor.getPulses();   //  Get increment encoder current pulse number
        
        #ifdef DEBUG_MODE 
        device.printf("[%d] ", cur_pulse);
        #endif

    }
    #ifdef DEBUG_MODE 
    device.printf("\n----- PID done -----\n");
    #endif
    out1 = 0;       //  Stop motor
    out2 = 0;
};



DC_Motor_Controller::DC_Motor_Controller(PinName out_M1, PinName out_M2, PinName in_A, PinName in_B, int PPR) : 
            out1(out_M1), out2(out_M2), dc_motor(in_A, in_B, NC, PPR)
{
    _ppr = PPR;
    out1.period(0.002);
    out2.period(0.002);
}

void DC_Motor_Controller::goto_angle(int angle, float speed){      //  Move motor to specific angle from home point
        
    goto_pulse(((double) angle / 360.0)* (2.0 * _ppr), speed);    //  Calculating target pulse number
};
    
void DC_Motor_Controller::reset(){       // Setting home point for increment encoder
    out1 = 0;
    out2 = 0;
    dc_motor.reset();       //  Reset pulse_
    wait(0.3);
};

void DC_Motor_Controller::move_angle(int angle, float speed){      //  move for relative distance
    
    goto_pulse(get_pulse() + ((double) angle / 360.0)* (2.0 * _ppr), speed); 
};

void DC_Motor_Controller::set_out(float a, float b)
{ 
    out1 = a;
    out2 = b;
}

int DC_Motor_Controller::get_pulse()
{
    return dc_motor.getPulses();
}