20160814

Fork of CURRENT_CONTROL by LDSC_Robotics_TAs

CURRENT_CONTROL.cpp

Committer:
adam_z
Date:
2016-04-22
Revision:
1:c5973a56d474
Parent:
0:955aa05c968a
Child:
2:562bd14dfd3a

File content as of revision 1:c5973a56d474:

#include "mbed.h"
#include "CURRENT_CONTROL.h"


CURRENT_CONTROL::CURRENT_CONTROL(PinName curChannel,
                                 PinName PwmChannel1,
                                 PinName PwmChannel2,
                                 PWMIndex pwmIndex,
                                 float Kp, float Ki, float Kd,
                                 float samplingTime) : currentAnalogIn(curChannel),
    MotorPlus(PwmChannel1),
    MotorMinus(PwmChannel2),
    pid(Kp,Ki,Kd,samplingTime)
{
    pwmIndex_ = pwmIndex;

    Ts = samplingTime;
    //setup motor PWM parameters
    MotorPlus.period_us(50);//default period equals to 50 microseconds
    MotorPlus.write(0.5);   //duty ratio = 0.5 in complementary output mode -> static
    if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
    else if(pwmIndex_ == PWM2)TIM1->CCER |= 64; //enable complimentary output

}

void CURRENT_CONTROL::Control(float curRef)
{
    curFeedBack = (currentAnalogIn.read() - currentOffset)*analog2Cur;
    pid.Compute(curRef, curFeedBack);
    MotorPlus = 0.5 - pid.output;
    if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
    else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
}

void CURRENT_CONTROL::SetAnalog2Cur(float ratio)//LTS25-NP current sensor working with STM32F446RE : 3.3*8/0.6
{
    analog2Cur = ratio;
}

void CURRENT_CONTROL::SetPWMDuty(float ratio)
{
    MotorPlus = ratio;
    if(pwmIndex_ == PWM1)TIM1->CCER |= 4;
    else if(pwmIndex_ == PWM2)TIM1->CCER |= 64;
}