NXTruck Software: verschil tussen versies

Uit RobotMC.be
Ga naar: navigatie, zoeken
(Nieuwe pagina aangemaakt met 'Alle code is geschreven in NXC. == T-Tijd == <pre><nowiki> #define NEAR 15 #define STOP 12 #define TURN 90 #define THRESHOLD 45 int toeren; int toeren100, to...')
 
Geen bewerkingssamenvatting
 
Regel 1: Regel 1:
[[category:robot]]
Alle code is geschreven in [[NXC]].
Alle code is geschreven in [[NXC]].



Huidige versie van 29 dec 2017 om 22:45

Alle code is geschreven in NXC.

T-Tijd

#define NEAR 15
#define STOP 12
#define TURN 90
#define THRESHOLD 45

int toeren;
int toeren100, toeren50, toerenwrecht;

task main()
{
    OnFwd(OUT_B, 100);
    SetSensorLowspeed(IN_4);
    
    toeren = 0;
    ResetRotationCount (OUT_B);

    NumOut(0, LCD_LINE1, SensorUS(IN_4));

    until(SensorUS(IN_4) < NEAR);

    toeren100 = MotorRotationCount(OUT_B);
    NumOut(0, LCD_LINE1, SensorUS(IN_4));

    OnFwd(OUT_B, 50);
    until(SensorUS(IN_4) < STOP);

    toeren50 = MotorRotationCount(OUT_B);
    NumOut(0, LCD_LINE1, SensorUS(IN_4));

    OnRev(OUT_B, 0);

    toeren = MotorRotationCount(OUT_B);
    NumOut(0, LCD_LINE1, toeren100);
    NumOut(0, LCD_LINE2, toeren50);
    NumOut(0, LCD_LINE3, toeren);
    ResetRotationCount (OUT_B);

    OnRev(OUT_B, 100);

    until(abs(MotorRotationCount(OUT_B)) > ((toeren) / 4));
    
    RotateMotor(OUT_A, 100, TURN);

    RotateMotor(OUT_B, 100, -(360*5));
    
    OnRev(OUT_B, 100);
    RotateMotor(OUT_A, 100, -TURN);

    SetSensorLight(IN_2);
    until(Sensor(IN_2) > THRESHOLD);
    ResetSensor(IN_2);
    
    RotateMotor(OUT_B, 100, -(360 + 180));
    OnFwd(OUT_B, 0);
    
    RotateMotor(OUT_B, 100, 4 * 360);

    RotateMotor(OUT_A, 100, -TURN);
    RotateMotor(OUT_B, 100, (360*5));
    
    OnFwd(OUT_B, 100);
    RotateMotor(OUT_A, 100, TURN);
    
    until(SensorUS(IN_4) < NEAR);
    OnFwd(OUT_B, 50);
    until(SensorUS(IN_4) < STOP);
    OnRev(OUT_B, 0);
}

Heen en weer

#define NEAR 15
#define STOP 12

int toeren;
int toeren100, toeren50;

task main()
{
    OnFwd(OUT_B, 100);
    SetSensorLowspeed(IN_4);
    toeren = 0;
    ResetRotationCount (OUT_B);
    
    NumOut(0, LCD_LINE1, SensorUS(IN_4));
    
    until(SensorUS(IN_4) < NEAR);
    
    toeren100 = MotorRotationCount(OUT_B);
    NumOut(0, LCD_LINE1, SensorUS(IN_4));
    
    OnFwd(OUT_B, 50);
    until(SensorUS(IN_4) < STOP);

    toeren50 = MotorRotationCount(OUT_B);
    NumOut(0, LCD_LINE1, SensorUS(IN_4));

    OnRev(OUT_B, 0);

    toeren = MotorRotationCount(OUT_B);
    NumOut(0, LCD_LINE1, toeren100);
    NumOut(0, LCD_LINE2, toeren50);
    NumOut(0, LCD_LINE3, toeren);
    ResetRotationCount (OUT_B);
    
    OnRev(OUT_B, 100);

    until(abs(MotorRotationCount(OUT_B)) > toeren100);
    OnRev(OUT_B, 50);
    
    until(abs(MotorRotationCount(OUT_B)) > toeren50);
    OnRev(OUT_B, 0);
}

Objectdetectie

#define LROT 134
#define RROT -134
#define FULLLEFT 100
#define FULLRIGHT -100
#define SPEED 75
#define SCANSPEED 25
#define LOWSPEED  50
#define NEAR005 5
#define NEAR015 15
#define NEAR030 30
#define NEAR100 100

/* Turn at 100cm */
#define TURNAT     100
#define TURNAMOUNT 92

mutex engine;
mutex mturn;



int totalrot = 0;
int numrot = 0;
void turn(int rot)
{
    if(rot < 0)
        numrot++;

    int r = (rot / 100) * LROT;

    totalrot += r;
    Acquire(mturn);
    RotateMotor(OUT_A, 50, r);
    Release(mturn);
}

void reset()
{
     RotateMotor(OUT_A, 50, -totalrot);
     totalrot = 0;
}

task touchSensor()
{
     while(true)
     {
         if(SENSOR_1 == 1)
         {
              Acquire(engine);
              OnFwd(OUT_B, 0);
              Release(engine);
         }
     }
}

task stopAt5()
{
     while(true)
     {
         if(SensorUS(IN_4) <= NEAR005)
             OnFwd(OUT_B, 0);
     }
}

void sensorMonitor()
{
    int near;
    near = NEAR030 + (numrot * 2);

    while (SensorUS(IN_4) <= near)
    {
        Acquire(engine);
        OnFwd(OUT_B, 0);
        turn(FULLRIGHT);
        OnRev(OUT_B, LOWSPEED);
        Wait(1000);

        if(numrot == 3)
        {
             reset();
             OnFwd(OUT_B, LOWSPEED);
             turn(FULLLEFT);
             numrot = 0;
        }

        Release(engine);
        reset();
    }
    Acquire(engine);
    OnFwd(OUT_B, LOWSPEED);
    Release(engine);
}

task over1meter ()
{
     while (true)
     {
           SetSensorLowspeed(IN_4);
           if (SensorUS(IN_4) <= NEAR100)
           {
              Acquire(engine);
              OnFwd(OUT_B, LOWSPEED);
              Release(engine);
              sensorMonitor();
           }
            else
            {
              Acquire(engine);
              OnFwd(OUT_B, SPEED);
              Release(engine);
            }
     }

}

task drive()
{
     Acquire(engine);
     OnFwd(OUT_B, SPEED);
     Release(engine);
}

task main()
{
     SetSensorLowspeed(IN_4);
     SetSensorTouch(IN_1);
     Precedes (over1meter, drive, touchSensor);
}