//信奥在高中最后的用途
# 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;
}