斜め走行は通常の直進より幅が狭く、制御なしでは３,４区画程度が限界かと思います。
しかし第３１回全国大会ハーフサイズ競技では最速経路に１７区画斜めが存在するなど、長い斜めを走り切るためには制御が必須となります。

通常の直進のように横センサで制御するのは厳しいため、左右２個の前センサを用いて制御をかけます。
閾値を用い、センサ値が閾値以上の場合のみP制御にて制御をかけます。

if(front_right_sensor_now>=FRONT_RIGHT_SENS_REF){
	diagonal_control=Kp*(front_right_sensor_now-FRONT_RIGHT_SENS_REF);
}else if(front_left_sensor_now>=FRONT_LEFT_SENS_REF){
	diagonal_control=Kp*(FRONT_LEFT_SENS_REF-front_left_sensor_now);
}

その他ありましたら追記下さい