DCマウスでの速度制御を行なうプログラムの一例です。
char speed_control_en; //速度制御有効無効フラグfloat now_target_speed; //現在の目標速度[mm/s]float target_speed; //目標速度[mm/s]float acceleration; //目標加速度 常に正の値を設定する[m/s^2]float now_acc; //現在の加速度 加速中なら正,減速中なら負[m/s^2]float speed_integral; //速度の差の積分値float center_speed; //エンコーダから得た中心速度[mm/s]float k_t; //FF加速度項ゲインfloat k_e; //FF速度項ゲインfloat k_f_l; //FF摩擦項ゲイン(左)float k_f_r; //FF摩擦項ゲイン(右)float speed_P; //FB比例項ゲインfloat speed_I; //FB積分項ゲインfloat speed_D; //FB微分項ゲインvoid speed_control(void){ float speed_diff; //現在の目標速度とエンコーダから得た実速度との差分[mm/s] static float pre_speed_diff; //1周期前の速度の差分[mm/s] //現在の目標速度の更新 if(now_target_speed < target_speed){ //加速中 now_acc = acceleration; //現在の加速度を正の値にする now_target_speed += now_acc; //現在の目標速度を目標加速度分上げる if(now_target_speed > target_speed){ //現在の目標速度が目標速度を上回ったら now_target_speed = target_speed; //現在の目標速度を目標速度にする now_acc = 0.0; //現在の加速度を0にする } } else if(now_target_speed > target_speed){ //減速中 now_acc = -acceleration; //現在の加速度を負の値にする now_target_speed += now_acc; //現在の目標速度を目標加速度分下げる if(now_target_speed < target_speed){ //現在の目標速度が目標速度を下回ったら now_target_speed = target_speed; //現在の目標速度を目標速度にする now_acc = 0.0; //現在の加速度を0にする } } else{ //現在の目標速度=目標速度で加減速の必要なし now_acc = 0.0; } float speed_vol_l, speed_vol_r; //電圧[mV] //速度制御が有効なら if(speed_control_en){ //FF計算 speed_vol_l = speed_vol_r = (k_t * now_acc + k_e * now_target_speed); //FF加速度項と速度項の計算 if(now_target_speed){ //現在の目標速度が0でないなら speed_vol_l += k_f_l; //摩擦項も計算 speed_vol_r += k_f_r; } speed_diff = (now_target_speed - center_speed); //速度差分計算 speed_integral += (speed_diff + pre_speed_diff)/2.0*; //積分値計算 //本来はサンプル時間0.001をかけるが、I項の係数が大きくなりすぎるためかけていない //FB計算 float speed_vol_temp = 0.0; speed_vol_temp += speed_diff * speed_P; //FB比例項計算 speed_vol_temp += speed_integral * speed_I; //FB積分項計算 speed_vol_temp += (speed_diff - pre_speed_diff) * speed_D; //FB微分項計算 speed_vol_l += speed_vol_temp; //FF項に足す speed_vol_r += speed_vol_temp; pre_speed_diff = speed_diff; //今回の差分を前回の差分に代入 } else{ speed_vol_l = 0.0; speed_vol_r = 0.0; speed_integral = 0.0; } /* 角速度も同じように計算 */ change_motor_vol((short)(speed_vol_l + angle_speed_vol_l), (short)(speed_vol_r + angle_speed_vol_r));}