BETADeliverer.nxc

#include "NXCDefs.h"
#define NEAR 15 //cm
#define __NOTETIME   10
#define __WAITTIME   12

int val = 0;
mutex moveMutex;
 /*Turns robot to the left 90degrees*/
/* task waiting()
 {
      Wait(4000);
     Break;
 }
 task counting()
 {
    while(true)
    if(sensor==1)
    {
    val++
    Wait(300);
    }
 } */



inline void move_fwd(int a)
{
int i;
 OnFwdReg(OUT_AC, 75, OUT_REGMODE_SYNC);
 i = SensorUS(IN_4);
 while(i>a)
 {
 i = SensorUS(IN_4);

 }
 Off(OUT_AC);
}

inline void hitta()
{

    int s=SensorUS(IN_4);
    OnFwd(OUT_A, 40);
    OnRev(OUT_C, 40);
    while(s>70)
    {
    s=SensorUS(IN_4);
    NumOut(4, LCD_LINE1, SensorUS(IN_4));
    }
    Off(OUT_AC);
 int v=SensorUS(IN_4);

 OnFwd(OUT_A, 50);
 OnRev(OUT_C, 50);
 Wait(300);
 Off(OUT_AC);
 int h=SensorUS(IN_4);

     while(v>h&&v!=h)     //&&v!=h
     {
     OnFwd(OUT_A, 50);
     OnRev(OUT_C, 50);
     Wait(200);
     Off(OUT_AC);
     v = h;
     h = SensorUS(IN_4);
     NumOut(10, LCD_LINE2, v);
     NumOut(10, LCD_LINE3, h);
     }
OnRev(OUT_A, 50);
OnFwd(OUT_C, 50);
Wait(500);
Off(OUT_AC);
NumOut(10, LCD_LINE2, v);
NumOut(10, LCD_LINE3, h);
Wait(2000);

}

inline void avl()//Avlämning av bok
{
       RotateMotor(OUT_B, 20, -150);
       Wait(500);
       OnRevReg(OUT_AC, 50, OUT_REGMODE_SYNC);
       Wait(500);
       Off(OUT_AC);
       RotateMotor(OUT_B, 20, 150);
}

inline void paket1()   // Höger 2.5m
{


     RotateMotor(OUT_A, 75, 360);
     Wait(1000);
     Off(OUT_A);
     OnFwdReg(OUT_AC, 100, OUT_REGMODE_SPEED);
     Wait(8000);
      move_fwd(20);
     Off(OUT_AC);
     avl();
    //klar
}

inline int paket2()   // Vänster 2.5m
{
          //Snurra vänster
      OnFwdReg(OUT_AC, 50, OUT_REGMODE_SYNC);
      Wait(4000);
      Off(OUT_AC);
      hitta();
      avl();
      //klar
}

inline int paket3()    // Höger 2.5m tvärs över väggen
{
        //snurra höger
     OnFwdReg(OUT_AC, 50, OUT_REGMODE_SYNC);
     Wait(4000);
     Off(OUT_AC);
       //snurra runt
     Off(OUT_AC);
     //Åk rakt fram tills vägg
     //Dropoff
}

inline int paket4()   // Vänster 2.5m tvärs över väggen
{
       //Snurra vänster
     OnFwdReg(OUT_AC, 50, OUT_REGMODE_SYNC);
     Wait(4000);
     Off(OUT_AC);
    //Snurra runt
     Off(OUT_AC);
     //Åk rakt fram tills vägg
     //Dropoff
}


task main()
{
     SetSensorLowspeed(IN_4);
     hitta();
     move_fwd(40);
     paket1();
     Wait(2000);
}