C 带油箱踏板的PID直线跟随器

我做了一个（相当糟糕的）跟班

这是一张草图，大致了解机器人的形状以及踏板和传感器的位置

[-] 0  0 [-] // 0 = color sensor
[-]------[-] // - = robot body
[-]------[-] // [-] = tank tread
[-]      [-] 

它的作用如下：

• 获取红色、绿色和蓝色，对传感器1的读数进行平均，对传感器2执行相同操作

• 减去得到值

• 该值将通过PID部分

• 使用计算转向进行转向

• 重复（所有这些都在一个循环中）

我使用RGB，而不是反射强度（这是常用的），因为有时我需要检测传感器下是否有绿色（如果有，转动）

真正的问题在于转向部分。不幸的是，它只会加速马达，这意味着在非常急转弯时，我们会失去线路

最佳情况下，它应该补偿一点其他电机（可能朝另一个方向？），但我不知道如何计算电机的速度，也不知道如何执行这一非常严格的线路跟随政策

下面是代码（我也非常感谢关于如何清理代码的任何提示！这是我在C:D中的第一个项目）。我不要求阅读全部内容（它相当长），您也可以只看转向功能，然后返回到

rawFollowLine

，这应该有希望缩短代码

void rawFollowLine(int speed, float Kp, float Ki, float Kd){

    _checkInit();

    set_sensor_mode(sn_lx_color, "RGB-RAW");
    set_sensor_mode(sn_rx_color, "RGB-RAW");
    //printAllSensors();

    int wasBlackCounter = 0;
    int wasBlack = 0;
    int lastBlack = 0;

    for (int i = 0; i < 2000; i++)
    {

        if (isTerminating == 1)
        {
            killMotors(0);
            break;
        }

        int greenCheck = rawGreenCheck(&wasBlack, &wasBlackCounter, &lastBlack);


        if (wasBlack == 1){
            wasBlackCounter++;
            if (wasBlackCounter > 50){
                wasBlackCounter = 0;
                wasBlack = 0;
            }
        }
        if (greenCheck == 1)
        {
            // lx is green
            killMotors(1);
            usleep(400 * 1000);
            drive(200, 70);
            waitIfMotorIsRunning();
            killMotors(1);
            pivotTurn(-90);
        } 
        else if (greenCheck == 2)
        {
            // rx is green
            killMotors(1);
            usleep(400 * 1000);
            drive(200, 70);
            waitIfMotorIsRunning();
            killMotors(1);
            pivotTurn(90);
        }
        else if (greenCheck == 3)
        {
            // both rx and lx are green
            killMotors(1);
            turn(180);
        }
        else if (greenCheck == 5) 
        {
            if(lastBlack == 2)
            {
                lastBlack = 0;
                drive(100, -200);
                //pivotTurn(50);
            }
            else if (lastBlack == 1)
            {
                lastBlack = 0;
                drive(100, -200);
                //pivotTurn(-50);
            } else {
                pidLineRaw(speed, Kp, Ki, Kd, &lastBlack);  
            }
        }
        else
        {
            pidLineRaw(speed, Kp, Ki, Kd, &lastBlack);  
        }

    }
    killMotors(1);

}


int rawGreenCheck(int *wasBlack, int *wasBlackCounter, int *lastBlack)
{
    // Some documentation
    // return nums:
    // 3 = double green
    // 2 = right green
    // 1 = left green
    // 0 = no green 
    int lx_red;
    int lx_green;
    int lx_blue;

    int rx_red;
    int rx_green;
    int rx_blue;

    get_sensor_value(0, sn_lx_color, &lx_red);
    get_sensor_value(0, sn_rx_color, &rx_red);

    get_sensor_value(1, sn_lx_color, &lx_green);
    get_sensor_value(1, sn_rx_color, &rx_green);

    get_sensor_value(2, sn_lx_color, &lx_blue);
    get_sensor_value(2, sn_rx_color, &rx_blue);

    //printf("rx_red %d\n", rx_red);
    rx_red = (rx_red * rx_ratio_r);
    rx_green = (rx_green * rx_ratio_g);
    rx_blue = (rx_blue * rx_ratio_b);
    //printf("rx_red (again) %d\n", rx_red);

    if(
        lx_red < 55 && 
        lx_green > 90 && 
        lx_blue < 55 && 

        rx_red < 55 && 
        rx_green > 90 && 
        rx_blue < 55
        )
    {
        // rx and lx see green
        if (*wasBlack == 1)
        {
            // Apparently we crossed an intersection!
            printf("Apparently we crossed an intersection!\n");
            // We need to go straight.
            *wasBlack = 0;
            *wasBlackCounter = 0;
            return 0;
        } 
        else
        {
            return 3;
        }
    }
    else if(lx_red < 55 && lx_green > 90 && lx_blue < 55)
    {
        // lx sees green
        return 1;
    }
    else if(rx_red < 55 && rx_green > 90 && rx_blue < 55)
    {
        // rx sees green
        return 2;
    }
    else if(rx_red < 50 && rx_green < 50 && rx_blue < 50 && lx_red < 50 && lx_green < 50 && lx_blue < 50)
    {
        // rx and lx see black
        // this is needed if the intersection has the green tiles after the black line
        printf("We are on the line? Is this an intersection?\n");
        *wasBlack = 1;
        return 0;
    }
    else if(lx_red < 55 && lx_green < 55 && lx_blue < 55)
    {
        // lx = right sees black
        // this is needed if the intersection has the green tiles after the black line
        //printf("We are on the line? Is this an intersection?\n");
        killMotor(1, motor[R]);
        rotateTillBlack(motor[L], sn_rx_color);
        //printf("ASS2\n");
        return 0;
    }
    else if(rx_red < 55 && rx_green < 55 && rx_blue < 55)
    {
        // rx = left sees black
        killMotor(1, motor[L]);
        rotateTillBlack(motor[R], sn_lx_color);
        //printf("ASS1\n");
        return 0;
    }

    //*lx_color_status = 0;
    //*rx_color_status = 0;
    *lastBlack = 0;
    return 0;
}

void pidLineRaw(int speed, float Kp, float Ki, float Kd, int *lastBlack)
{

    int red_lx_color;
    int red_rx_color;

    int green_lx_color;
    int green_rx_color;

    int blue_lx_color;
    int blue_rx_color;

    int lx_color;
    int rx_color;

    int last_error = 0;

    int integral = 0;
    int derivative = 0;

    //float Kp = 0.1;
    //float Ki = 0;
    //float Kd = 0;

    //set_sensor_mode(sn_lx_color, "COL-REFLECT");
    //set_sensor_mode(sn_rx_color, "COL-REFLECT");

    get_sensor_value(0, sn_lx_color, &red_lx_color);
    get_sensor_value(0, sn_rx_color, &red_rx_color);

    get_sensor_value(1, sn_lx_color, &green_lx_color);
    get_sensor_value(1, sn_rx_color, &green_rx_color);

    get_sensor_value(2, sn_lx_color, &blue_lx_color);
    get_sensor_value(2, sn_rx_color, &blue_rx_color);

    lx_color = (red_lx_color + green_lx_color+ blue_lx_color)/3;
    rx_color = ( (red_rx_color*rx_ratio_r) + (green_rx_color*rx_ratio_g) + (blue_rx_color*rx_ratio_b))/3;

    if(*lastBlack == 0)
    {
        int error = lx_color - rx_color;
        integral = integral + error;
        derivative = error - last_error;
        last_error = error;

        int steering_val = (error * Kp) + (integral * Ki) + (derivative * Kd);
        // printf("error: %d\nsteering: %d\n",error, steering_val);
        move_steering(-steering_val, speed, 1, 0);
    } else if (*lastBlack == 1)
    {
        printf("lx_color_status\n");
        move_steering(35, speed, 1, 0);
        move_steering(-2, speed, 1, 0);
    } 
    else if (*lastBlack == 2)
    {
        printf("rx_color_status\n");
        move_steering(-35, speed, 1, 0);
        move_steering(2, speed, 1, 0);
    }
    else
    {
        printf("HMMM: %d\n", *lastBlack);
        exit(666);
    }
}
static void _getSteeringSpeed(int speed, int *lx_speed, int *rx_speed, int steering)
{
    if(steering > 100 || steering < -100)
    {
        printf("Yo wtf steering is %d\n", steering);
    } 
    else 
    {
        int speed_factor = (50 - abs(steering)) / 50;
        *lx_speed = speed;
        *rx_speed = speed;

        if(steering >= 0)
        {
            *rx_speed = *rx_speed * speed_factor;
        } 
        else 
        {
            *lx_speed = *lx_speed * speed_factor;
        }
    }
}

void rawFollowLine（整数速度、浮点Kp、浮点Ki、浮点Kd）{
_checkInit（）；
设置传感器模式（sn\U lx\U颜色，“RGB-RAW”）；
设置传感器模式（sn接收颜色，“RGB-RAW”）；
//打印所有传感器（）；
int-wasbackcounter=0；
int=0；
int lastBlack=0；
对于（int i=0；i<2000；i++）
{
如果（isTerminating==1）
{
killMotors（0）；
打破
}
int greenCheck=rawGreenCheck（&wasback，&wasbackcounter，&lastback）；
如果（wasBlack==1）{
wasbackcounter++；
如果（wasBlackCounter>50）{
wasBlackCounter=0；
wasBlack=0；
}
}
如果（绿色支票==1）
{
//lx是绿色的
killMotors（1）；
usleep（400*1000）；
驱动器（200、70）；
waitIfMotorIsRunning（）；
killMotors（1）；
枢轴转动（-90）；
} 
否则如果（绿色检查==2）
{
//rx是绿色的
killMotors（1）；
usleep（400*1000）；
驱动器（200、70）；
waitIfMotorIsRunning（）；
killMotors（1）；
旋转（90）；
}
否则如果（绿色检查==3）
{
//rx和lx都是绿色的
killMotors（1）；
转（180）；
}
否则如果（绿色检查==5）
{
如果（lastBlack==2）
{
lastBlack=0；
驱动器（100，-200）；
//旋转（50）；
}
else if（lastBlack==1）
{
lastBlack=0；
驱动器（100，-200）；
//枢轴转动（-50）；
}否则{
pidLineRaw（速度、Kp、Ki、Kd和lastBlack）；
}
}
其他的
{
pidLineRaw（速度、Kp、Ki、Kd和lastBlack）；
}
}
killMotors（1）；
}
int-rawGreenCheck（int*wasback，int*wasbackcounter，int*lastback）
{
//一些文件
//返回nums：
//3=双绿
//2=右绿色
//1=左绿色
//0=没有绿色
int lx_红色；
int lx_绿色；
int lx_蓝；
国际红新月会；
国际绿色；
int rx_蓝；
获取传感器值（0、sn\U lx\U颜色和lx\U红色）；
获取传感器值（0、序列号接收颜色和接收红色）；
获取传感器值（1，sn\U lx\U颜色和lx\U绿色）；
获取传感器值（1、序列号接收颜色和接收绿色）；
获取传感器值（2，sn\U lx\U颜色和lx\U蓝色）；
获取传感器值（2，序列号接收颜色和接收蓝色）；
//printf（“接收红色%d\n”，接收红色）；
rx_red=（rx_red*rx_ratio_r）；
rx_绿色=（rx_绿色*rx_比率g）；
rx_蓝=（rx_蓝*rx_比b）；
//printf（“接收红色（再次）%d\n”，接收红色）；
如果(
lx_红<55&&
lx_绿色>90&&
lx_蓝<55&&
rx_红色<55&&
rx_绿色>90&&
rx_蓝<55
)
{
//rx和lx见绿色
如果（*wasBlack==1）
{
//显然我们穿过了一个十字路口！
printf（“显然我们穿过了十字路口！\n”）；
//我们得直走。
*wasBlack=0；
*wasBlackCounter=0；
返回0；
} 
其他的
{
返回3；
}
}
否则如果（lx_红色<55和&lx_绿色>90和&lx_蓝色<55）
{
//lx看到绿色
返回1；
}
否则如果（接收红色<55和接收绿色>90和接收蓝色<55）
{
//rx看到绿色
返回2；
}
否则，如果（rx_红色<50&&rx_绿色<50&&rx_蓝色<50&&lx_红色<50&&lx_绿色<50&&lx_蓝色<50）
{
//rx和lx见黑色
//如果交点在黑线后有绿色平铺，则需要此选项
printf（“我们在这条线上？这是十字路口吗？\n”）；
*wasBlack=1；
返回0；
}
否则，如果（lx_红色<55和&lx_绿色<55和&lx_蓝色<55）
{
//lx=右侧为黑色
//如果交点在黑线后有绿色平铺，则需要此选项
//printf（“我们在这条线上？这是十字路口吗？\n”）；
基尔马达（1，马达[R]）；
旋转黑色（电机[L]，黑色）；
//printf（“ASS2\n”）；
返回0；
}
否则如果（rx_红色<55和rx_绿色<55和rx_蓝色<55）
{
//rx=左侧显示黑色
基尔马达（1，马达[L]）；
旋转黑色（电机[R]，黑色）；
//printf（“ASS1\n”）；
返回0；
}
//*lx_颜色_状态=0；
//*rx_颜色_状态=0；
*lastBlack=0；
返回0；
}
void pidLineRaw（整型速度、浮点Kp、浮点Ki、浮点Kd、整型*最后黑色）