0
$\begingroup$

I have a skid steer robot where I have four individual DC motors with an H bridge so I can control speed and direction. There are also four quadrature encoders with 6000 ticks per revolution. The motors are 12v and I am controlling them with a 10bit PWM (0 to 1023). First test I did was to raise all wheels and test the minimum value where the wheels start moving, which is around 500.

I am able to measure the current speed of the wheels correctly by using a counter where the quadrature encoder set it to count up or down. Therefore, by reading the current count every 50ms (20Hz), I can measure the current speed:

       encoders_curr.FR = COUNTER_mReadReg(ENC0_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.FL = COUNTER_mReadReg(ENC1_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.RR = COUNTER_mReadReg(ENC2_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);
       encoders_curr.RL = COUNTER_mReadReg(ENC3_BASEADDR, COUNTER_S00_AXI_SLV_REG1_OFFSET);

       encoders_diff.FR = encoders_curr.FR - encoders_prev.FR;
       encoders_diff.FL = encoders_curr.FL - encoders_prev.FL;
       encoders_diff.RL = encoders_curr.RL - encoders_prev.RL;
       encoders_diff.RR = encoders_curr.RR - encoders_prev.RR;

       encoders_prev = encoders_curr;

       speed_measured.FR = (2*M_PI*WHEEL_RADIUS*encoders_diff.FR) / (0.05*TICKS);
       speed_measured.FL = (2*M_PI*WHEEL_RADIUS*encoders_diff.FL) / (0.05*TICKS);
       speed_measured.RR = (2*M_PI*WHEEL_RADIUS*encoders_diff.RR) / (0.05*TICKS);
       speed_measured.RL = (2*M_PI*WHEEL_RADIUS*encoders_diff.RL) / (0.05*TICKS);

Which I convert to X, Y and Z with:

       // Inverse Kinematics: http://robotsforroboticists.com/drive-kinematics/
       kinematics_measured.linear.x = (speed_measured.FR+speed_measured.FL+speed_measured.RR+speed_measured.RL)*(WHEEL_RADIUS/4);
       kinematics_measured.linear.y = (-speed_measured.FR+speed_measured.FL+speed_measured.RR-speed_measured.RL)*(WHEEL_RADIUS/4);
       kinematics_measured.angular.z = (-speed_measured.FR+speed_measured.FL-speed_measured.RR+speed_measured.RL)*(WHEEL_RADIUS/(4*(WHEEL_SEPARATION_WIDTH+WHEEL_SEPARATION_LENGTH)));

Now for the control part, to set the individual velocities of each of the four wheels, I am using these equations, as my input is linear x, linear Y and angular Z speeds (rad/seg):

       // Forward kinematics: http://robotsforroboticists.com/drive-kinematics/
       speed_desired.FL = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x-kinematics_desired.linear.y-kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.FR = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x+kinematics_desired.linear.y+kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.RL = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x+kinematics_desired.linear.y-kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));
       speed_desired.RR = (1/WHEEL_RADIUS)*(kinematics_desired.linear.x-kinematics_desired.linear.y+kinematics_desired.angular.z*(WHEEL_SEPARATION_LENGTH+WHEEL_SEPARATION_WIDTH));

The PID function that I have implemented is the following:

float integral=0.0, prev_error=0.0;
#define dt  0.05    // in seconds
#define Kp  1
#define Ki  0
#define Kd  0
float pid(float desired, float measured)
{
    if(measured<0)
        measured = (-1)*measured;

    // Calculate error
    float error = desired-measured;

    // Proportional term
    float Pout = Kp*error;

    // Integral term
    integral += error*dt;
    float Iout = Ki*integral;

    // Derivate term
    float derivate = (error-prev_error)/dt;
    float Dout = Kd*derivate;

    return (Pout+Iout+Dout);
}

So, after measuring the current speed, I can call it to obtain the speed I need to set the duty of each motor.

My questions:

  1. The sign of the velocity indicates the direction of rotation (clockwise=forward, counterclockwise=backwards). Do I need to pass to the pid function the desired velocity as a signed or unsigned value?
  2. What should I do with velocity values that would mean a lower pwm so the motor will not spin?
  3. To convert from linear velocity that the pid will dictate to duty cycle, I am calculating the equation that passes over two points. For this, I measured the speeds at the minimum and maximum duties where the wheels rotation (500 and 1023) giving (0.81 and 1.03). I use these numbers to get the equation: p = 511 + (uint16_t)(((1023-511)/(1.02-0.8))*(v-0.8));. Should I consider that the speed can be negative to compute a "negative" duty which will later represent only the direction to set for the motors? If so, it means that I have to consider that the two points for the equation are (-1023, -1.03) and (1023, 1.03)?
  4. As I have four motors, they all show different velocities. So, to simplify the process, even thought there will be 4 PIDs, I got for the lowest duty cycle the maximum velocity value of all motors and for the highest duty cycle the lowest velocity of all motors. Would that be correct?
  5. Do I need to include the speed limits for the PID?
  6. Considering the signs, won't be a problem if the measured speed is higher than the desired, the pid would compute a negative value and the fact that the rotation switches directions might cause an issue, or not?

Thank you for the help.

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.