C言語でEV3開発(21)-モーター出力の制御

void motor_task(intptr_t unused) {
    while (motor_task_running) {
        //Read current, actual motor output.
        motor_get_power();

        //Calcurate motor output.
        judge_motor_output_limit();
        judge_target_motor_output();
        calc_target_motor_outputLR();
        calc_motor_power();

        //Set motor output data.
        motor_set_power();

        motor_task_count++;
        dly_tsk(MOTOR_TASK_INTERVAL);
    }
}

void calc_motor_power(void) {
    int left_output = 0;
    int right_output = 0;

    left_power.target = target_motor_output_left;
    right_power.target = target_motor_output_right;
    
    left_power.current = left_motor_power_current;
    right_power.current = right_motor_power_current;

    left_output = pid_control(&left_power);
    right_output = pid_control(&right_power);

    /**
     *  Calcurate Finalized motor power.
     *  Due to Hardware or OS (maybe OS), the motor can not drive with
     *  the absolute actual motor power with 50. If the power value exceeds
     *  50 is set, the motor dirve does not change.
     *  And, the case that the value exceeds 50 is set to motor, sometime
     *  the motor can not operate in a precise manner. To avoid this situation,
     *  limits motor output preliminarily.
     */
    //Motor output power(Limitation).
    //Left side motor.
    left_motor_power = limit_int(left_output,
        MOTOR_POWER_MIN, MOTOR_POWER_MAX);
    //Right side motor.
    right_motor_power = limit_int(right_output,
        MOTOR_POWER_MIN, MOTOR_POWER_MAX);
}


/**
 *  @brief  Execute PID control.
 *  @param[in,out]  Value for PID control.
 *  @return Calcurated amount by PID control.
 */
static int pid_control(power_t *power) {
    int ddt = 0;    //derivative element
    int diff = 0;
    int pid = 0;
    int integral = 0;

    if (NULL != power) {
        diff = power->target - power->current;

        integral = power->integral + diff;
        ddt = power->diff - diff;
        pid = (Kp * diff + Ki * integral + Kd * ddt) / 100;

        power->target_prev = power->target;
        power->integral = integral;
        power->diff = diff;
    } else {
        pid = 0;
    }

    return pid;
}