int main(void)
{
initTimer0(); // Timer0 - Lys/Lyd
initTimer1(); // Timer1 - Lyd(OCR1A) og Delays
initTimer2(); // Timer2 - Motorstyring
initBackLEDPort();
initFrontLEDPort();
sei();
allLightsOn(); // Tænd alle lys
while(1)
{
do
{
ReflectionCount();
DriveForward1();
brakeLightOff();
if( antalRefleksbrik == 3 ) // Brems ved bakken.
{
Brake();
brakeLightOn();
}
if( antalRefleksbrik == 6 ) // Bremse og bak.
{
Brake();
_delay_us(5000000);
brakeLightOn();
Reverse();
}
if( antalRefleksbrik == 8 ) // Når den holder ved refleksbrik nr 5.
{
Brake();
_delay_us(5000000);
}
} while ( antalRefleksbrik >=0 && antalRefleksbrik <=11 );
Brake();
brakeLightNormal();
StopLevelComplete();
}
return 0;
}
int main(void)
{
initTimer0();
initTimer1();
initTimer2();
initSwitchPort();
sei();
//int antalRefleksbrik;
while(1)
{
do
{
ReflectionCount();
DriveForward1();
/*if( StartReflection() == 1 ) // True = 1.
{
DetekCoin();
}
*/
if( antal == 3 ) // Brems ved bakken.
{
Brake();
}
if( antal == 6 ) // Bremse og bak.
{
Brake();
_delay_us(5000000);
Reverse();
}
if( antal == 8 ) // Når den holder ved refleksbrik nr 5.
{
Brake();
_delay_us(5000000);
}
} while ( antal >=0 && antal <=11 );
Brake();
}
return 0;
}
void jfolo(unsigned char count){
unsigned char junction_count=0;
unsigned char timer=0;
Forward(); // set motor move forward
// sending display to lcd takes long time
// junction reading and display less frequent.
// use timer variable to count 20, then read once.
while(junction_count<count){ //second junction
timer++;
if(timer>50){
timer=0; //clear timer
motor(0,0); //slow down for lcd display
do{
junction_count=LSA08_GetJunction(); //check junction count
}while(ERR_FLAG||(junction_count>10)); //checking no uart error
lcd_goto(0);
lcd_putchar('J');
lcd_num(junction_count,3);
}
line_follow(); //PID line follow
}
Brake();
__delay_ms(200);
}
inline void EsquivoPared(double distancia){
if (distancia<1){
Accelerate(-0.5);
Brake(1);
RotateAmount(1,666.0,190.0);
}
}
void ICreature::BrakeOnReachState(ICreature& creature, IActionBase::State actionState)
{
if(actionState == IActionBase::State::REACHED)
{
Brake();
}
}
void junction_follow(void){
signed char lineposition=0;
unsigned char i=0;
unsigned char status=0;
lcd_clr();
lcd_goto(0);
lcd_putstr("Junction\nCount");
__delay_ms(500);
lcd_clr();
while(1){
do{
status=LSA08_ClearJunction();
}while(status==0);
jfolo(2);
RotateRight();
motor(180,180);
__delay_ms(500);
do{
lineposition=LSA08_GetPosition();
LED1=1;
}while(lineposition>20);
Brake();
LED1=0;
__delay_ms(100);
RotateRight();
motor(180,180);
__delay_ms(500);
do{
LED1=1;
lineposition=LSA08_GetPosition();
}while(lineposition>20);
Brake();
LED1=0;
__delay_ms(100);
}
}
inline void EsquivoMina(double angulo){
Shoot(0.02*energiadisparo);
if (distancia<20 & ((angulo<90) | (angulo>270)) ){
Accelerate(-0.5);
}else{
Accelerate(2);
}
Brake(1);
}
void UserShip4::Update(float RelativeTime)
{
m_RelativeTime = RelativeTime;
if ((IsKeyDown(VK_NUMPAD8)) && (IsKeyDown(VK_NUMPAD5)))
Brake();
else if (IsKeyDown(VK_NUMPAD8))
MoveForward();
else if (IsKeyDown(VK_NUMPAD5))
MoveBackward();
if ((IsKeyDown(VK_NUMPAD4)) && !(IsKeyDown(VK_NUMPAD6)))
TurnLeft();
if ((IsKeyDown(VK_NUMPAD6)) && !(IsKeyDown(VK_NUMPAD4)))
TurnRight();
SpaceShip::Update(RelativeTime);
}
void UserShip3::Update(float RelativeTime)
{
m_RelativeTime = RelativeTime;
if ((IsKeyDown('I')) && (IsKeyDown('K')))
Brake();
else if (IsKeyDown('I'))
MoveForward();
else if (IsKeyDown('K'))
MoveBackward();
if ((IsKeyDown('J')) && !(IsKeyDown('L')))
TurnLeft();
if ((IsKeyDown('L')) && !(IsKeyDown('J')))
TurnRight();
SpaceShip::Update(RelativeTime);
}
void UserShip1::Update(float RelativeTime)
{
m_RelativeTime = RelativeTime;
if ((IsKeyDown(VK_UP)) && (IsKeyDown(VK_DOWN)))
Brake();
else if (IsKeyDown(VK_UP))
MoveForward();
else if (IsKeyDown(VK_DOWN))
MoveBackward();
if ((IsKeyDown(VK_LEFT)) && !(IsKeyDown(VK_RIGHT)))
TurnLeft();
if ((IsKeyDown(VK_RIGHT)) && !(IsKeyDown(VK_LEFT)))
TurnRight();
SpaceShip::Update(RelativeTime);
}
/*
This task makes the robot follow the line,
turn around corners,
handle crossings
and detect an obstacle.
*/
task Follow(){
int i = 0;
startTask(Sound);
while (true){
status = "active";
/*
Objects are detected below here.
When an object is detected the user can either choose to avoid it or charge.
*/
if(SonarSensor()){
while(speed > 0){
speed = Brake(i,speed);
i--;
Straight(speed);
}
status = "engage";
StopSound();
s = "";
while((s != "FIRE")&&(s != "B")){
}
if (s == "FIRE"){
startTask(Sound);
startTask(AvoidObject);
}
}
else if(speed < max_Speed){
speed = Accelerate(i,speed);
i++;
}
Straight(speed);
/*
Crossings are handled below here.
Once both lightsensors detect the line at the same time the robot will brake unless it was told to go straight.
It will then drive backward to find the line again after which it will continue untill it has crossed it.
The robot will then look at the last given command to determine which way it will go.
*/
if((Convert(SensorValue[S3],minlight,maxlight) < 70) && (Convert(SensorValue[S4],mincolor,maxcolor) < 70)){
if (command != "straight"){
speed = EmergencyBrake(speed);
wait1Msec(1000);
while(Convert(SensorValue[S3],minlight,maxlight) > 30 || Convert(SensorValue[S4],mincolor,maxcolor) > 30){
Straight((-10));
}
if (Convert(SensorValue[S3],minlight,maxlight) < 70 || Convert(SensorValue[S4],mincolor,maxcolor) < 70){
nMotorEncoderTarget[motorB]=(nMotorEncoderTarget[motorC]=105);
motor[motorB]=(motor[motorC]=-10);
while((nMotorRunState[motorB] != runStateIdle) && (nMotorRunState[motorC] != runStateIdle)) wait1Msec(1);
}
if ((s == "FIRE")||(s == "")){
StopSound();
while ((s == "FIRE")||(s == "")){
Straight(0);
}
startTask(Sound);
}
if (command == "left"){
LeftTurn();
}
else if (command == "right"){
RightTurn();
}
}
else{
wait1Msec(200);
}
}
//The code below here makes sure the robot stays on the line.
if(Convert(SensorValue[S3],minlight,maxlight) < 80){
Left(Convert(SensorValue[S3],minlight,maxlight),speed);
}
if(Convert(SensorValue[S4],mincolor,maxcolor) < 80){
Right(Convert(SensorValue[S4],mincolor,maxcolor),speed);
}
wait1Msec(1.5);
}
}
bool vehicle::Progress(double time)
{
if (((p.p.size() == 3) && (progr >= targetProgr)) || (p.p.size() == 2))
{
//3 - end, drove over targetProgr
//2 - jumped over targetProgr to end of road
RemList();
return 1;//end
}
double sNow = speed*(double(time)/1000);
//lanes
if (changingLaneFrom != -1)
{
//continue changing lane
double now = sNow * 0.5;
double left = 1 - changingLaneProgr;
bool end = 0;
if (now >= left)
{
now = left;
end = 1;
}
changingLaneProgr += now;
int rot;
if (lNr > changingLaneFrom)
rot = 90;
else
rot = -90;
k += rot;
Drive(now);
k -= rot;
if (end)
{
changingLaneFrom = -1;
}
}
else
{
//look for better lane
if (BetterAtLane(lNr - 1))
ChangeLane(lNr - 1);
else if (BetterAtLane(lNr + 1))
ChangeLane(lNr + 1);
}
double dist = Length(pos, Destination());
if (dist <= sNow)
{
Drive(dist);
RemList();
p.Skip();
progr = c->GetRoadProgress(p.Prev(), p.Now(), pos);
AddList();
if (p.p.size() >= 3)
UpdateRotation();
//add speed control in here too
if (speed != 0)
{
//reduce avail
double leftDist = sNow - dist;
int leftTime = int((leftDist / (speed)) * 1000);
return Progress(leftTime);
}
}
else
{
Drive(sNow);
double left;//distance to obstacle
left = obs.d;
if (c->Crossroad(p.Now()) && !c->LAvailable(p.Now(), p.Prev()))
{
double distToCrossing = c->DistanceToCrossroad(p.Prev(), p.Now(), pos) - vM->l;
left = min(left, distToCrossing);
}
/*
(0; 1) - Brake
(1; 2) - nieko
(2; +inf) - Accelerate
*/
const double bestDist = 0.5;
const double warnDist = 1.5;
if (left < bestDist)
{
//increase distance
speed = 0;
Brake(time);
}
else if (left < warnDist)
{
speed = obs.speed;
}
else if (left > bestDist)
{
Accelerate(time);
}
}
return 0;
}
void SimpleDrive::Disable() {
Brake();
}
void entradadatos(int sig){
int d=1;
char buffer[128];
liberabloqueos();
read(0,buffer,128);
tipomensaje=analizamensaje(buffer);
//fprintf(stderr,"%s\n",buffer);fflush(stderr);fsync(fileno(stderr));
switch(tipomensaje){
case RADAR:
sscanf(buffer,"Radar %lf %i %lf",&distancia,&tipoobjeto,&angulo);
switch(tipoobjeto){
case ROBOT:
DisparoRobot(distancia);
break;
case SHOT:
ParoDisparo(angulo,distancia);
break;
case WALL:
EsquivoPared(distancia);
break;
case COOKIE:
APorGalleta(angulo);
break;
case MINE:
EsquivoMina(angulo);
}
break;
case COLLISION:
sscanf(buffer,"Collision %i %lf",&tipoobjeto,&angulo);
switch(tipoobjeto){
case WALL:
if(i-tiempocolision>20){
Accelerate(-0.5);
Brake(1);
RotateAmount(1,666.0,angulo+180);
}
tiempocolision=i;
Accelerate(2.0);
break;
case ROBOT:
RotateAmount(1,666,angulo+15);
break;
case SHOT:
RotateAmount(1,666,angulo+15);
break;
case COOKIE:
aquemededico=BUSCANDOANILLO;
break;
case MINE:
break;
}
break;
case INITIALIZE:
if(!inicializado){
printf("Print Ash nazg durbatuluuk, ash nazg gimbatul, ash nazg trakatuluuk agh burzum-ishi krimpatul!\n");fflush(stdout);fsync(fileno(stdout));
printf("Name sauron\n");fflush(stdout);fsync(fileno(stdout));
printf("Colour 00F0F0 010F0F\n");fflush(stdout);fsync(fileno(stdout));
inicializado=1;
}
break;
case ENERGY:
sscanf(buffer,"Energy %lf",&energia);
energiadisparo=30;
if(energia<5){
energiadisparo=0;
}
break;
case ROBOT_INFO:
break;
case ROTATION_REACHED:
Accelerate(2);
Sweep(6,45.2,-0.255,0.25); // Si sabia cuanto girar es que busco algo concreto.
break;
case WARNING:
break;
case 666:
sleep(1);
break;
}
if(tipomensaje==GAME_STARTS){
while(1){
i++;
switch (aquemededico){
case BUSCANDOANILLO:
Accelerate(2.0);
Rotate(1,2.0);
if(!(i%300)){
Brake(0.7);
RotateAmount(1,666.0,90.0);
}else if(!(i%150)){
Brake(0.7);
RotateAmount(1,666.0,-90.0);
}
break;
case GALLETAS:
Sweep(6,45.2,-0.75,0.75);
if (!(time(NULL)-tiempogalleta)) {
aquemededico=BUSCANDOANILLO;
}
break;
case MATAR:
Sweep(6,45.2,-0.01,0.01);
break;
}
liberabloqueos();
sleep(1);
}
}
}