Code:
task main(){
int ng,gn=0,nw=0,rp=0,rg,lrp=0,rwi=0,md;
int GyroBiasCount,Gyro_value,GyroBias=0, batt;
long timer1;
string message, messageA, messageB;
timer1 = CurrentTick(); // set timer1
// Calculate the Giro Bias for the Hitechnic Gyro sensor.
// by measuring the Gyro value many times in 3 secondes.
// Gyro Bias is avarage Gyro value.
GyroBiasCount = 0;
while (CurrentTick() < (3000+timer1)) {
// filter the sensor output
Gyro_value = SensorRaw(S3);
Wait(150);
GyroBiasCount = GyroBiasCount + 1;
gn = gn + Gyro_value;
PlayTone(TONE_B7, 5);
}
GyroBias = gn / GyroBiasCount;
gn=GyroBias;
// Measure the battery voltage.
batt=BatteryLevel();
// Display calculated GyroBias an battery voltage on the NXT screen;
message = "Gbias Bat ";
messageA = NumToStr(gn);
messageB = NumToStr(batt);
message = StrReplace(message, 5, messageA);
message = StrReplace(message, 12, messageB);
TextOut(0, LCD_LINE1, message, false);
PlayTone(TONE_B7, 5);
while(true){
ng=SensorRaw(S3)-gn; nw+=ng; //body angle
rp=MotorTachoCount(OUT_B); //wheel position
rg=rp-lrp; lrp=rp; rwi+=rg; //wheel speed
if (abs(rwi)>15){gn-=sign(rwi); rwi=0;} //Gyro drift comp.
// md=(nw+ng*10+rp*4+rg*200)>>4; //pd contr. NXT small wheels
md=(nw+ng*12+rp*5+rg*360)>>5;
// md=(nw+ng*12+rp*5+rg*360)>>5; //pd contr. RXC large wheels
md+=sign(md)*48; //friction compensation default = 48
SetOutput(OUT_AB, Power, md, OutputMode,OUT_MODE_MOTORON,
RunState,OUT_RUNSTATE_RUNNING,
UpdateFlags,UF_UPDATE_MODE+UF_UPDATE_SPEED);
Wait(8); //loop about 100 times/sec
}
}
i has made a script with Java for my Lego NXT set
and a gyro sensor
i like to hear what you think of this
remember its my first script
well not the first ever but the first try (for me ) on a balancing bot
Bookmarks