// 车运行
private static void Car_run()
{
var temp = new float[4]; //四个电机的速度
int v, vdif, v_f; //车速度和差速
var coefficient = 0;
while (true)
{
if (Trans_error_count >= tansError)
{
Trans_Error(); //通信失常
}
if (buffer[0] != 0x01 && buffer[0] != 0x02 && buffer[0] != 0x03 && buffer[0] != 0x04)
{
break; //跳出循环判断
}
v = buffer[2];
if (v >= 99)
{
v = 99;
}
if (v <= 2)
{
v = 0;
}
vdif = buffer[3] * v / 90;
switch (buffer[0] * 10 + buffer[1]) //10,11,12,13,14,20,21,22,23,24,30,32,34,40,42,44
{
case 10:
case 20:
case 30:
case 40:
temp[0] = 0;
temp[1] = 0;
temp[2] = 0;
temp[3] = 0;
break;
case 11:
case 21:
temp[0] = v - vdif;
temp[1] = v - vdif;
temp[2] = v * -1;
temp[3] = v * -1;
break;
case 12:
case 22:
temp[0] = v;
temp[1] = v;
temp[2] = (v - vdif) * -1;
temp[3] = (v - vdif) * -1;
break;
case 13:
case 23:
v_f = v * 8 / 10;
temp[0] = -v_f;
temp[1] = -v_f;
temp[2] = (-v_f + vdif) * -1;
temp[3] = (-v_f + vdif) * -1;
break;
case 14:
case 24:
v_f = v * 8 / 10;
temp[0] = -v_f + vdif;
temp[1] = -v_f + vdif;
temp[2] = -v_f * -1;
temp[3] = -v_f * -1;
break;
case 32:
case 42:
temp[0] = v;
temp[1] = v;
temp[2] = v;
temp[3] = v;
break;
case 34:
case 44:
temp[0] = -v;
temp[1] = -v;
temp[2] = -v;
temp[3] = -v;
break;
default:
temp[0] = 0;
temp[1] = 0;
temp[2] = 0;
temp[3] = 0;
break;
}
switch (buffer[0])
{
case 1:
case 3:
coefficient = velocityMax;
break;
case 2:
case 4:
coefficient = 2 * velocityMax;
break;
}
//写入速度
for (byte add = 0x01; add < 0x05; add++)
{
try
{
com.Write(CarControl.CarSpeedWrite(add, coefficient * temp[add - 1]), 0, 8);
}
catch (Exception e)
{
mainLog.WriteLog("串口写入失败", e.ToString());
}
Thread.Sleep(serial_time);
}
Thread.Sleep(cpu_time); //延时降低CPU负担
#if DEBUG
//读取电流
for (byte add = 0x01; add < 0x05; add++)
{
try
{
com.Write(CarControl.CurrentRead(add), 0, 8);
}
catch (Exception e)
{
mainLog.WriteLog("串口写入失败", e.ToString());
}
Thread.Sleep(serial_time);
}
#endif
}
}
