SpeedControl.cpp

[詳解]

#include "SpeedControl.h"

SpeedControl::SpeedControl():
    //pid(0.36, 1.2, 0.027, 30.0), colorSensor( PORT_3 ){
    Pid(0.2, 0.5, 0.0, 30.0), forward(0), preAngleL(0), preAngleR(0){
    for(int i=0; i<25; i++)speed_value[i] = 0;
    speedCount = 0;
    speed_value_all = 0;
    pid_value_old = 0.0;
    distance4ms = 0.0;
    //150のときいい感じ pid(0.8, 1.2, 0.0, 30.0), forward(30){
}

std::int32_t SpeedControl::calculateSpeedForPid(std::int32_t curAngleL, std::int32_t curAngleR) {
    int8_t speed_value_thistime = calcDistance4ms(curAngleL, curAngleR);
    speed_value_all += (speed_value_thistime - speed_value[ speedCount ]);
    speed_value[ speedCount ] = speed_value_thistime;
    calculate((double)speed_value_all);
    double pid_value = - get_output();
    speedCount++;
    if( speedCount >= 25 ) speedCount = 0;
    forward += (pid_value - pid_value_old) / 10; 
    pid_value_old = pid_value;
    return (int)limitOutput(forward);
}

/* 距離更新（4ms間の移動距離を毎回加算している） */
std::int8_t SpeedControl::calcDistance4ms(std::int32_t curAngleL, std::int32_t curAngleR){

    float distance4msFloat = 0.0;        //4msの距離

    // 4ms間の走行距離 = ((円周率 * タイヤの直径) / 360) * (モータ角度過去値　- モータ角度現在値)
    float distance4msL = ((2 * 3.14 * 81) / 360.0) * (float)(curAngleL - preAngleL);  // 4ms間の左モータ距離
    float distance4msR = ((2 * 3.14 * 81) / 360.0) * (float)(curAngleR - preAngleR);  // 4ms間の右モータ距離
    distance4msFloat = (distance4msL + distance4msR) / 2.0; //左右タイヤの走行距離を足して割る

    //モータの回転角度の過去値を更新
    preAngleL = curAngleL;
    preAngleR = curAngleR;

    distance4ms = (int)distance4msFloat;
    // 単位はmm
    return distance4ms;
}

std::int16_t SpeedControl::getSpeed100ms(){
    return speed_value_all;
}