生物创新科技素养大赛小车代码

//信奥在高中最后的用途

# include <crobert.h>

double Mod(double x){
    if(x > 180) return x - 360;
    if(x < -180) return x + 360;
    return x;
}

void Show(double x){
    rbText(23, x);
}

void SetRight(double spd){
    rbMotor(1, spd);
      rbMotor(2, spd);
      rbMotor(3, -spd);
      rbMotor(4, -spd);
}

void SetLeft(double spd){
    rbMotor(1, -spd);
      rbMotor(2, -spd);
      rbMotor(3, spd);
      rbMotor(4, spd);
}

void SetRun(double spd){
    rbMotor(1, spd);
      rbMotor(2, spd);
      rbMotor(3, spd);
      rbMotor(4, spd);
}

void TurnRight(double ang){
    double ori = rbAnalog(22, 6, 1);
    while(Mod(rbAnalog(22, 6, 1) - ori) < ang)
        SetRight(30);
    while(Mod(rbAnalog(22, 6, 1) - ori) > ang)
        SetLeft(10);
    while(Mod(rbAnalog(22, 6, 1) - ori) < ang)
        SetRight(2);
}

void TurnLeft(double ang){
    double ori = rbAnalog(22, 6, 1);
    while(Mod(ori - rbAnalog(22, 6, 1)) < ang)
        SetLeft(30);
    while(Mod(ori - rbAnalog(22, 6, 1)) > ang)
        SetRight(10);
    while(Mod(ori - rbAnalog(22, 6, 1)) < ang)
        SetLeft(2);
}

void RunInTime(double t){
    t *= 1000, rbReset();
    while(rbSecond() < t)
        SetRun(100);
}

void BackInTime(double t){
    t *= 1000, rbReset();
    while(rbSecond() < t)
        SetRun(-100);
}

void FollowLine(int col){
    if(rbAnalog(25, 1, 1) == col && rbAnalog(24, 1, 1) == col) SetRun(50);
    else if(rbAnalog(25, 1, 1) != col) SetRight(30);
    else SetLeft(30);
}

void RunUntil(int col,int spd){
    while(rbAnalog(23, 1, 1) != col && rbAnalog(24, 1, 1) != col)
        SetRun(spd);
}

void Judge(double t){
    while(Mod(rbAnalog(22, 6, 1) - t) > 0)
        SetLeft(1);
    while(Mod(t - rbAnalog(22, 6, 1)) > 0)
        SetRight(1);
}

void Rest(){
    rbMotor(1, 0);
      rbMotor(2, 0);
      rbMotor(3, 0);
      rbMotor(4, 0);
}

void Touch(){
    rbMotor(5, 5);
    rbSleep(1000);
    rbMotor(5, -5);
    rbSleep(1000);
    rbMotor(5, 0);
}

int main(){
    TurnLeft(30);
    RunInTime(0.3);
    TurnRight(30);
    RunInTime(0.8);
    TurnRight(30);
    RunUntil(59, 50);
    TurnLeft(30);
    Judge(180);
    RunInTime(1.7, 80);
    Rest();
    Touch();
    TurnLeft(30);
    RunInTime(0.3);
    TurnRight(30);
    Judge(180);
    RunUntil(167, 100);
    rbSleep(500);
    TurnLeft(90);
    Judge(90);
    RunInTime(2.5);
    BackInTime(0.5);
    TurnRight(90);
    TurnRight(90);
    Judge(270);
    RunUntil(167, 100);
    RunInTime(3);
    TurnRight(90);
    TurnRight(90);
    Judge(90);
    RunUntil(167, 100);
    rbSleep(500);
    TurnRight(90);
    RunUntil(170, 100);
    rbSleep(500);
    TurnRight(90);
    Judge(270);
    RunInTime(3.6);
    RunUntil(164, 50);
    TurnRight(90);
    RunInTime(1);
    Judge(360);
    BackInTime(1);
    TurnRight(90);
    Judge(90);
    RunInTime(8);
    Judge(90);
    RunUntil(164, 50);
    TurnLeft(90);
    RunInTime(10);
    Rest();
    return 1;
}