PICによるRC戦車制御２

/**
 * モーターを約40Hzの周波数でPWM制御するために使うカウンタ
 * このカウンタはカウントダウンする形で使用する。
 */
static uint8_t MTR_counter100u;

/**
 * TMR2の割込み発生時の処理
 *
 * 約100us毎にコールされる。
 */
void MTR_HandlerForTMR2(void)
{
    // 100usカウンタをカウントダウンする。
    MTR_counter100u--;

    // モーターの制御状態が変わったイベントを発生する。
    EventRise(Event_MTR_ChangeAnyStatus);
}

/**
 * 右モーターのスピード
 */
static uint8_t MTR_RMOTOR_SPD;

/**
 * 右モーターの方向 true:前進 false:後進
 */
static __bit MTR_RMOTOR_FOR_OR_BACK;

/**
 * 左モーターのスピード
 */
static uint8_t MTR_LMOTOR_SPD;

/**
 * 左モーターの方向 true:前進 false:後進
 */
static __bit MTR_LMOTOR_FOR_OR_BACK;

/**
 * モーター制御のための更新処理
 */
void MTR_Update(void)
{
    // MTR_LATCにセットする値を作成するための作業変数
    OUT_DATA_t control_value;
    control_value.UINT8 = 0;

    // この関数は、各種変数に応じて、LATCの更新する。
    // LATCにセットする値を control_valueで作成していき、
    // 最後にOUT_SetMotor()を使ってLATCを更新する。

    // 右モーターの制御信号を変更する
    if (MTR_counter100u < MTR_RMOTOR_SPD)
    {
        if (MTR_RMOTOR_FOR_OR_BACK)
        {
            control_value.RMTR_FOR = true;
        }
        else
        {
            control_value.RMTR_BACK = true;
        }
    }

    // 左モーターの制御信号を変更する
    if (MTR_counter100u < MTR_LMOTOR_SPD)
    {
        if (MTR_LMOTOR_FOR_OR_BACK)
        {
            control_value.LMTR_FOR = true;
        }
        else
        {
            control_value.LMTR_BACK = true;
        }
    }

    // セットする値が完成したので、出力する。
    OUT_SetMotor(control_value.UINT8);
}

#define MTR_SPEED(left, right) (MTR_L##left | MTR_R##right)
#define MTR_RSTOP 0        // 右モーターSTOP
#define MTR_RFOR1 (1 << 0) // 右モーターFOR1
#define MTR_RFOR2 (1 << 1) // 右モーターFOR2
#define MTR_RBACK (1 << 2) // 右モーターBACK
#define MTR_LSTOP 0        // 左モーターSTOP
#define MTR_LFOR1 (1 << 3) // 左モーターFOR1
#define MTR_LFOR2 (1 << 4) // 左モーターFOR2
#define MTR_LBACK (1 << 5) // 左モーターBACK

const uint8_t MTR_SPEED_TABLE[12] = {
    MTR_SPEED(STOP, STOP),
    MTR_SPEED(FOR1, FOR1),
    MTR_SPEED(FOR2, FOR2),
    MTR_SPEED(BACK, BACK),

    MTR_SPEED(FOR1, BACK),
    MTR_SPEED(FOR1, STOP),
    MTR_SPEED(FOR2, FOR1),
    MTR_SPEED(STOP, BACK),

    MTR_SPEED(BACK, FOR1),
    MTR_SPEED(STOP, FOR1),
    MTR_SPEED(FOR1, FOR2),
    MTR_SPEED(BACK, STOP),
};

/**
 * モーター制御のパラメーター
 */
static uint8_t MTR_ControlParameter;

/**
 * モーターを制御するパラメーターをセットする。
 *
 * @param[in] parameter
 *
 * parameterの下位4bitを用いてモーターを制御する。
 * 上位4bitは無視する。
 *
 * @param parameterの1-0ビット
 * 速度（0:停止, 1:前進(遅い）, 2:前進(早), 3:後進)
 *
 * @param parameterの3-2ビット
 * 旋回（0: 直進, 1: 車体を右に向ける, 2: 車体を左に向ける, 3: error）
 *
 */
void MTR_SetControlParameter(uint8_t parameter)
{
    // parameterは 2進数で0000～1011までの値しか有効ではない
    // (2-3ビットが11はエラー)
    // そのため 12以上になった場合は、無視する。

    if (parameter >= 12)
    {
        return;
    }

    MTR_ControlParameter = parameter;
    // 12未満の場合は、モーター制御パラメーターが変化したイベントを発生する。
    EventRise(Event_MRT_ControlParameter_Changed);
}

// FOR1やBACKのときの速度（PWMのDuty比:45%）
#define MTR_FOR1_SPEED (256 * 45 / 100)
// FOR2のときの速度(PWMのDuty比: 90%)
#define MTR_FOR2_SPEED (256 * 90 / 100)

/**
 * MTR_SetControlParameter()の後処理を行う関数
 *
 * MRT_ControlParameter_Changed イベントによってコールされる。
 * MTR_SetControlParameter()の処理内容（アセンブル後の処理命令数）が多かったので、
 * 小分けに別関数にしている。
 *
 * MTR_ControlParameterに応じて、モーターの回転方向や速度を決定する。
 *
 */
void MTR_SetControlParameter_PostProcess(void)
{
    // 変換テーブルを使って、paramterを制御値に変換する
    uint8_t speed_table = MTR_SPEED_TABLE[MTR_ControlParameter];

    // 右モーター：回転方向
    MTR_RMOTOR_FOR_OR_BACK = !(speed_table & MTR_RBACK);
    // 左モーター：回転方向
    MTR_LMOTOR_FOR_OR_BACK = !(speed_table & MTR_LBACK);

    // 右モーター速度
    if (speed_table & MTR_RFOR2)
    {
        MTR_RMOTOR_SPD = MTR_FOR2_SPEED;
    }
    else if (speed_table & (MTR_RFOR1 | MTR_RBACK))
    {
        MTR_RMOTOR_SPD = MTR_FOR1_SPEED;
    }
    else
    {
        MTR_RMOTOR_SPD = 0;
    }

    // 左モーター：速度
    if (speed_table & MTR_LFOR2)
    {
        MTR_LMOTOR_SPD = MTR_FOR2_SPEED;
    }
    else if (speed_table & (MTR_LFOR1 | MTR_LBACK))
    {
        MTR_LMOTOR_SPD = MTR_FOR1_SPEED;
    }
    else
    {
        MTR_LMOTOR_SPD = 0;
    }

    // モーターの制御状態が変わったイベントを発生する。
    EventRise(Event_MTR_ChangeAnyStatus);
}