Похоже LEGO Mindstorms NXT уже всеми забыт и в мире пластиковой робототехники безраздельно правит EV3. Однако мы не спешим списывать его со счетов и в нашем сегодняшнем проекте он еще даст жару!
NXT for Speed - гоночный аркадный автомат с использованием руля и педалей. Основная цель проекта - изучение основных принципов работы с динамической векторной графикой, закрепление навыков программирования многопоточных приложений на С.
В нашем проекте мы реализовали:
- динамический расчет конфигурации трассы
- физическую модель, учитывающую силу нажатия на педаль газа, сопротивление покрышек, сопротивление воздуха. Автомобиль способен двигаться накатом, тормозить с учетом силы нажатия на педаль тормоза.
- вид от третьего лица с изменением угла зрения на автомобиль
- "плавающий" горизонт с задним фоном (горы)
- дорожные знаки на обочинах дороги
- управление с использованием руля (контроль угла поворота) и педалей (газ-тормоз)
- реализован автовозврат педалей - они жесткие и возвращаются в исходную позицию, если их отпустить
- обратная связь от препятствий - при съезде на обочины руль трясет
- обратная связь от скорости движения - чем выше скорость тем сложнее резко повернуть руль, на стоящей машине руль поворачивается легко
- цифровой спидометр в углу экрана
- подсчет очков
Инструкцию по сборке в формате LEGO Digital Designer можно скачать по ссылке.
Программа написана на языке NXC и выглядит следующим образом:
Код:
int ohki=0;
int trig=1;
int r=23;
int ry=0;
int rx=0;
int mx=0;
int my;
float rz=2;
int l=0;
int c=0;
long t;
float m=500;
int f=4;
float x1=25,x2=75,y1=0,y2=0;
int rt=0;
int o=-10000000000,p=100000000;
int u=0;
float k=0.90;
int tmp =0;
float turn[48];
int ch=0;
float a=50;
int b=0;
int e=0;
int Sign(int x, int y,int v,int c)
{
if(c==0){
LineOut(x,y,x,y+v/2,DRAW_OPT_NORMAL);
RectOut(x-v/4,y+v/2,v/2,v/2,DRAW_OPT_NORMAL);
}
else{
LineOut(x,y,x,y+v/2,DRAW_OPT_CLEAR);
RectOut(x-v/4,y+v/2,v/2,v/2,DRAW_OPT_CLEAR);
}
}
int lines(int x, int y,int color)
{
mx=x;
if(color==0){
RectOut(x-6,y+4,11,3,DRAW_OPT_NORMAL);
RectOut(x-5,y+2,2,2,DRAW_OPT_NORMAL);
RectOut(x+2,y+2,2,2,DRAW_OPT_NORMAL);
LineOut(x+5,y+7,x+3,y+10,DRAW_OPT_NORMAL);
LineOut(x-6,y+7,x-4,y+10,DRAW_OPT_NORMAL);
LineOut(x-4,y+10,x+3,y+10,DRAW_OPT_NORMAL);
if(CurrentTick()-t>500){
TextOut(0,0," ");
e=(500-m)/5;
NumOut(0,0,e);
t=CurrentTick();
}
if(MotorRotationCount(OUT_A) > 15){
LineOut(x+5,y+6,x+9,y+9,DRAW_OPT_NORMAL);
LineOut(x+3,y+10,x+7,y+12,DRAW_OPT_NORMAL);
LineOut(x-4,y+10,x-2,y+12,DRAW_OPT_NORMAL);
LineOut(x,y+12,x+7,y+12,DRAW_OPT_NORMAL);
LineOut(x+7,y+12,x+10,y+5,DRAW_OPT_NORMAL);
LineOut(x+6,y+3,x+10,y+5,DRAW_OPT_NORMAL);
LineOut(x+10,y+4,x+10,y+3,DRAW_OPT_NORMAL);
LineOut(x+9,y+3,x+10,y+3,DRAW_OPT_NORMAL);
if(x+10>=p+10){
TextOut(35,20,"Over");
rt=1;
}
}
else if(MotorRotationCount(OUT_A) <-15){
LineOut(x+3,y+10,x+1,y+12,DRAW_OPT_NORMAL);
LineOut(x-4,y+10,x-7,y+12,DRAW_OPT_NORMAL);
LineOut(x+1,y+12,x-8,y+12,DRAW_OPT_NORMAL);
LineOut(x-6,y+7,x-9,y+10,DRAW_OPT_NORMAL);
LineOut(x-8,y+11,x-11,y+7,DRAW_OPT_NORMAL);
LineOut(x-11,y+7,x-7,y+4,DRAW_OPT_NORMAL);
LineOut(x-11,y+7,x-11,y+4,DRAW_OPT_NORMAL);
LineOut(x-11,y+4,x-9,y+4,DRAW_OPT_NORMAL);
if(x-11<=o-10){
TextOut(35,20,"Over");
rt=1;
}
}
else{
LineOut(x+3,y+10,x+1,y+12,DRAW_OPT_NORMAL);
LineOut(x-4,y+10,x-2,y+12,DRAW_OPT_NORMAL);
LineOut(x-2,y+12,x+1,y+12,DRAW_OPT_NORMAL);
LineOut(x-5,y+11,x-5,y+11,DRAW_OPT_NORMAL);
LineOut(x+4,y+11,x+4,y+11,DRAW_OPT_NORMAL);
//if(x+10<=p || x-6>=o){
// TextOut(0,0,"Over");
// rt=1;
//}
}
Wait(10);
}
else{
RectOut(x-6,y+4,11,3,DRAW_OPT_CLEAR);
RectOut(x-5,y+2,2,2,DRAW_OPT_CLEAR);
RectOut(x+2,y+2,2,2,DRAW_OPT_CLEAR);
LineOut(x+5,y+7,x+3,y+10,DRAW_OPT_CLEAR);
LineOut(x-6,y+7,x-4,y+10,DRAW_OPT_CLEAR);
LineOut(x-4,y+10,x+3,y+10,DRAW_OPT_CLEAR);
LineOut(x+5,y+6,x+9,y+9,DRAW_OPT_CLEAR);
LineOut(x+3,y+10,x+7,y+12,DRAW_OPT_CLEAR);
LineOut(x-4,y+10,x,y+12,DRAW_OPT_CLEAR);
LineOut(x,y+12,x+7,y+12,DRAW_OPT_CLEAR);
LineOut(x+7,y+12,x+10,y+5,DRAW_OPT_CLEAR);
LineOut(x+6,y+3,x+10,y+5,DRAW_OPT_CLEAR);
LineOut(x+10,y+4,x+10,y+3,DRAW_OPT_CLEAR);
LineOut(x+9,y+3,x+10,y+3,DRAW_OPT_CLEAR);
LineOut(x+3,y+10,x+1,y+12,DRAW_OPT_CLEAR);
LineOut(x-4,y+10,x-7,y+12,DRAW_OPT_CLEAR);
LineOut(x+1,y+12,x-8,y+12,DRAW_OPT_CLEAR);
LineOut(x-6,y+7,x-9,y+10,DRAW_OPT_CLEAR);
LineOut(x-8,y+11,x-11,y+7,DRAW_OPT_CLEAR);
LineOut(x-11,y+7,x-7,y+4,DRAW_OPT_CLEAR);
LineOut(x-11,y+7,x-11,y+4,DRAW_OPT_CLEAR);
LineOut(x-11,y+4,x-9,y+4,DRAW_OPT_CLEAR);
}
return 0;
}
task mot()
{
Wait(3000);
float P=0.7;
//P=0.3*e;
float Kp=e/100;
float P2=0.7;
float P3=0.7;
float ERR1=0;
float ERR2=0;
float ERR3=0;
float u1=0;
float u2=0;
float u3=0;
while(1){
if(rt==1){
break;
}
float Kp=e/100;
ERR1=0-MotorRotationCount(OUT_A);
u1=P*ERR1*Kp;
ERR2=0-MotorRotationCount(OUT_B);
u2=P2*ERR2;
ERR3=0-MotorRotationCount(OUT_C);
u3=P3*ERR3;
if(mx-11<=o+3){
u1=Random(70)-35;
}
if(mx+10>=p-3){
u1=Random(70)-35;
}
if(u1>100)u1=100;
if(u1<-100)u1=-100;
if(u1<0){
OnRev(OUT_A,-1*u1);
}
else{
OnFwd(OUT_A,u1);
}
if(u2>100)u2=100;
if(u2<-100)u2=-100;
if(u2<0){
OnRev(OUT_B,-1*u2);
}
else{
OnFwd(OUT_B,u2);
}
if(u3>100)u3=100;
if(u3<-100)u3=-100;
if(u3<0){
OnRev(OUT_C,-1*u3);
}
else{
OnFwd(OUT_C,u2);
}
Wait(100);
}
}
task road()
{
for(int i =0;i<47;i++){
turn[i]=0;
}
while(true){
ch++;
f--;
if(f==0){
f=4;
}
l+=1;
for(int i =0;i<23;i+=1){
LineOut(25+turn[i]+i*0.9,i*2,25+turn[i+1]+(i+1)*0.9,i*2+3,DRAW_OPT_NORMAL);
LineOut(75+turn[i]-i*0.9,i*2,75+turn[i+1]-(i+1)*0.9,i*2+3,DRAW_OPT_NORMAL);
if(i==r){
if(trig==1){
rx=25+turn[i]+i*0.9-5;
ry=i*2;
}
else{
rx=75+turn[i]-i*0.9+5;
ry=i*2;
}
}
if(i%4==f && i>5){
LineOut(50+turn[i],i*2,50+turn[i+1],i*2+3,DRAW_OPT_NORMAL );
}
}
Sign(rx,ry,rz,0);
m+=5;
m+=m*0.2;
if(m>500){
m=500;
}
if(m<0){
m=0;
}
if(abs(MotorRotationCount(OUT_B))> 5){
if(m<=500){
m-=1.7*abs(0-abs(MotorRotationCount(OUT_B))*2);
}
if(m>500){
m=500;
}
if(m<0){
m=0;
}
}
if(abs(MotorRotationCount(OUT_C))> 5){
if(m>=0){
m+=1.7*abs(0-abs(MotorRotationCount(OUT_C)));
}
if(m>500){
m=500;
}
if(m<0){
m=0;
}
}
Wait(m);
for(int i =0;i<23;i+=1){
LineOut(25+turn[i]+i*0.9,i*2,25+turn[i+1]+(i+1)*0.9,i*2+3,DRAW_OPT_CLEAR);
LineOut(75+turn[i]-i*0.9,i*2,75+turn[i+1]-(i+1)*0.9,i*2+3,DRAW_OPT_CLEAR);
if(i%4==f && i>5)
{
LineOut(50+turn[i],i*2,50+turn[i+1],i*2+3,DRAW_OPT_CLEAR);
}
if(i==3){
o=25+turn[i];
p=75+turn[i];
}
}
Sign(rx,ry,rz,1);
rz+=0.5;
r--;
if(rz>=13){
rz=2;
r=23;
trig=trig*-1;
}
for(int i =0;i<47;i++){
turn[i]=turn[i+1];
}
if(ch==23){
ohki+=1;
ch=0;
u = Random(100);
turn[47]=u-50;
for(int i =23;i<47;i++){
turn[i]=turn[i-1]*k+turn[47]*(1-k);
}
}
if(rt==1){
break;
}
if(l==1){
LineOut(70,0,70,35,DRAW_OPT_NORMAL);
LineOut(70,35,30,35,DRAW_OPT_NORMAL);
LineOut(70,17,30,17,DRAW_OPT_NORMAL);
LineOut(70,0,30,0,DRAW_OPT_NORMAL);
Wait(1000);
LineOut(70,0,70,35,DRAW_OPT_CLEAR);
LineOut(70,35,30,35,DRAW_OPT_CLEAR);
LineOut(70,17,30,17,DRAW_OPT_CLEAR);
LineOut(70,0,30,0,DRAW_OPT_CLEAR);
LineOut(30,0,70,35,DRAW_OPT_NORMAL);
LineOut(30,35,70,35,DRAW_OPT_NORMAL);
LineOut(70,0,30,0,DRAW_OPT_NORMAL);
Wait(1000);
LineOut(30,0,70,35,DRAW_OPT_CLEAR);
LineOut(30,35,70,35,DRAW_OPT_CLEAR);
LineOut(70,0,30,0,DRAW_OPT_CLEAR);
LineOut(30,17,70,35,DRAW_OPT_NORMAL);
LineOut(70,0,70,35,DRAW_OPT_NORMAL);
Wait(1000);
LineOut(30,17,70,35,DRAW_OPT_CLEAR);
LineOut(70,0,70,35,DRAW_OPT_CLEAR);
}
}
}
task main()
{
t=CurrentTick();
SetSensorTouch(IN_4);
start road;
start mot;
ClearScreen();
LineOut(0,48,100,48,DRAW_OPT_NORMAL);
lines(a,b,0);
while(true){
NumOut(80,0,ohki);
c=30-((a-50)/2);
LineOut(c-50,49,c-30,53,DRAW_OPT_NORMAL);
LineOut(c-10,49,c-30,53,DRAW_OPT_NORMAL);
LineOut(c-10,49,c,55,DRAW_OPT_NORMAL);
LineOut(c+10,49,c,55,DRAW_OPT_NORMAL);
LineOut(c+5,51,c+15,55,DRAW_OPT_NORMAL);
LineOut(c+25,51,c+15,55,DRAW_OPT_NORMAL);
LineOut(c+20,51,c+35,55,DRAW_OPT_NORMAL);
LineOut(c+40,49,c+35,55,DRAW_OPT_NORMAL);
LineOut(c+40,51,c+55,55,DRAW_OPT_NORMAL);
LineOut(c+60,49,c+55,55,DRAW_OPT_NORMAL);
LineOut(c+60,51,c+85,55,DRAW_OPT_NORMAL);
LineOut(c+100,49,c+85,55,DRAW_OPT_NORMAL);
lines(a,b,1);
if(abs(MotorRotationCount(OUT_A))>=15){
a+=(MotorRotationCount(OUT_A))/100.0;
}
LineOut(c-10,49,c,55,DRAW_OPT_CLEAR);
LineOut(c+10,49,c,55,DRAW_OPT_CLEAR);
LineOut(c+5,51,c+15,55,DRAW_OPT_CLEAR);
LineOut(c+25,51,c+15,55,DRAW_OPT_CLEAR);
LineOut(c+20,51,c+35,55,DRAW_OPT_CLEAR);
LineOut(c+40,49,c+35,55,DRAW_OPT_CLEAR);
LineOut(c+40,51,c+55,55,DRAW_OPT_CLEAR);
LineOut(c+60,49,c+55,55,DRAW_OPT_CLEAR);
LineOut(c+60,51,c+85,55,DRAW_OPT_CLEAR);
LineOut(c+100,49,c+85,55,DRAW_OPT_CLEAR);
LineOut(c-50,49,c-30,53,DRAW_OPT_CLEAR);
LineOut(c-10,49,c-30,53,DRAW_OPT_CLEAR);
lines(a,b,0);
if(rt==1){
break;
}
}
Wait(1500);
}
