Robomiku võistlusel saavutasime 3. koha.
Koos reavahede ja kommentaaridega tuli kokku ligi 200 rida koodi.
Kuna palusin poistel kommentaarid juurde panna siis peaks olema arusaajale inimesele täitsa loetav kood. Kuna kasutasime mitte kõige uuemat versiooni NXC-st siis seetõttu on LineLeader anduriga suhtlev library siia eraldi included. Uuemasse softi (15. märts 2011) on juba kõik need uute andurite driverid sisse kirjutatud.
/*
autor: Ramses Sepp 20/03/11
robomiku 2011 kevad karupäästmise programm
selles programmis:
* sõidab robot mööda joont LineLeader anduri abil
* leiab rohelisi ja hõbedasi teibiribasid mis on kannatanud ja annab nendest märku
* tuvastab IR abil karu ja annab temast märku
* robotil on haarad mille abil ta kannab karu majast välja
*/
#include "LL-Lib.nxc"
#define IR_TOUCH S1
#define I2C_SENSOR S2
#define COLOR S3
#define LIGHT S4
#define LL_ADDR 0x04
long endTime;
bool haaradLahti = FALSE;
bool mulOnKaru = FALSE;
task DriveOnTheLine();
task KeeraRobotit180();
task KeeraRobotit();
//funktsioon ArvutaKiirus tagastab väärtuse, mis jääb -100 ... 100 vahele
int ArvutaKiirus(int x, int min, int max)
{
if (x < min)
return min;
else if (x > max)
return max;
else
return x;
}
//alamprogramm HaaradLahti avab haarad ning seejärel robot sõidab edasi
task HaaradLahti()
{
RotateMotorEx(OUT_A, -100, 6840, 0, FALSE, TRUE);
haaradLahti = TRUE;
//endTime muutujat kasutatakse selleks
//et robot sõidaks peale haarade lahti tegemist 2 sekundit edasi
endTime = CurrentTick() + 1000;
ExitTo(DriveOnTheLine);
}
//alamprogramm HaaradKinni sulgeb haarad ning siis käivitub roboti pööramine
task HaaradKinni()
{
RotateMotorEx(OUT_A, 100, 6840, 0, FALSE, TRUE);
mulOnKaru = TRUE;
haaradLahti = FALSE;
ExitTo(KeeraRobotit180);
}
//alamprogramm KeeraRobotit180 keerab robotit 180 kraadi
//kuni ta on joone keskel
task KeeraRobotit180()
{
int onJoonel;
//hakkame mootoreid keerama, et robot liiguks joonelt ära
RotateMotor(OUT_BC, 40, 360);
OnFwdReg(OUT_B, 40, OUT_REGMODE_SPEED);
OnFwdReg(OUT_C, -40, OUT_REGMODE_SPEED);
Wait(1000);
while (TRUE)
{
OnFwdReg(OUT_B, 40, OUT_REGMODE_SPEED);
OnFwdReg(OUT_C, -40, OUT_REGMODE_SPEED);
//loeme LL andurist kas robot on joone keskel
onJoonel = LL_Read (I2C_SENSOR, LL_ADDR, LL_READ_AVERAGE);
//kui robot on joone keskel siis käivitub uuesti põhiprogramm
if (onJoonel < 50 && onJoonel > 40)
{
Off(OUT_BC);
ExitTo(DriveOnTheLine);
}
}
}
//alamprogramm KeeraRobotit keerab robotit 180 kraadi
task KeeraRobotit()
{
RotateMotor(OUT_BC, 50, 360);
RotateMotorEx(OUT_BC, 50, 1100, 100, TRUE, TRUE);
Off(OUT_BC);
ExitTo(DriveOnTheLine);
}
//alamprogramm DriveOnTheLine on joone jälgimise, kannatanute tuvastamise
//ja karu kandmise programm
task DriveOnTheLine()
{
int steering;
int b_speed;
int c_speed;
int rohelisedMehed;
int hobedasedMehed;
bool naebKaru;
int karuKaugus;
bool onSoitnud2Sek = FALSE;
int baasKiirus;
//lõpmatu tsükkel mille sees toimub enamus programmist
while (TRUE)
{
rohelisedMehed = Sensor(COLOR);
hobedasedMehed = Sensor(LIGHT);
naebKaru = !Sensor(IR_TOUCH);
karuKaugus = SensorUS(I2C_SENSOR);
//kui värviandur näeb rohelist või hõbedast meest põrandal
//annab heliga märku
if (rohelisedMehed == 3 || hobedasedMehed > 70)
{
PlaySound(SOUND_DOUBLE_BEEP);
}
//kui IR_sensor näeb karu, haarad on kinni ja mul ei ole karu
//siis annab sellest heliga märku:P
if (naebKaru && !haaradLahti && !mulOnKaru)
{
Off(OUT_BC);
PlaySound(SOUND_UP);
ExitTo(HaaradLahti);
}
//kui haarad on lahti, ei näe karu ja on sõitnud 2 sekundit
//siis paneb haarad kinni
if (haaradLahti && !naebKaru && onSoitnud2Sek)
{
Off(OUT_BC);
ExitTo(HaaradKinni);
}
//kui robot on haarad lahti teinud siis ta sõidab 2 sekundit joonel edasi
//enne kui hakkab kontrollima kas on tarvis haarasid kinni panna
if (haaradLahti)
{
if (endTime < CurrentTick())
{
onSoitnud2Sek = TRUE;
}
}
//steering muutujasse kirjutatakse LineLeaderist steering väärtus
//mida kasutatakse mootorite juhtimisel
steering = LL_ReadSteering(I2C_SENSOR, LL_ADDR);
//see If lause on vajalik, et steering oleks vahemikus -127 ... 127
if(steering > 127)
{
steering = steering - 256;
}
// kui robot sõidab üles siis on baasKiirus 40, allaminekul 30
if (mulOnKaru)
{
baasKiirus = 30;
steering = steering / 3;
}
else
{
baasKiirus = 40;
}
/*
mootoritele õige kiiruse andmine
igal juhul ei saa olla mootori kiirus suurem kui 100
igal juhul ei saa olla mootori kiirus väiksem kui -100
*/
b_speed = ArvutaKiirus(baasKiirus + steering, -100, 100);
c_speed = ArvutaKiirus(baasKiirus - steering, -100, 100);
//kuna mootorid on tagurpidi siis peame muutma väärtused vastasmärgiliseks
b_speed = 0 - b_speed;
c_speed = 0 - c_speed;
OnFwdReg(OUT_B, b_speed, OUT_REGMODE_SPEED);
OnFwdReg(OUT_C, c_speed, OUT_REGMODE_SPEED);
}
}
task main()
{
SetSensorLight(LIGHT);
SetSensorColorFull(COLOR);
SetSensorLowspeed(I2C_SENSOR);
SetSensorTouch(IR_TOUCH);
Wait(SEC_5);
LL_Write_KP(I2C_SENSOR, LL_ADDR, 40, 32); //katsetada 0...100 //alla:40
LL_Write_KI(I2C_SENSOR, LL_ADDR, 0, 32); //jääb nulliks
LL_Write_KD(I2C_SENSOR, LL_ADDR, 5, 32); //katsetada 0..20 //alla:5
StartTask(DriveOnTheLine);
}