//=============================================================================
// Demo program "Spurensucher".
//
// Can be run under control of the ROBO TX Controller
// firmware in download (local) mode.
//
// Der Roboter folgt einer 20cm breiten, schwarzen Spur. 
// Dabei ist der Spurensensor an I1 und I2 angschlossen.
// Es werden die Prozeduren txtrack, txmotor und txsinglelamp aus der Erweiterung 
// von ROBO_TX_PRG.h verwendet.
// Durch unterschiedliche Geschwindigkeiten wird ein schnellerer Ablauf erreicht.
// An O5 bis O7 angeschlossene Lampen zeigen die Bewegungsrichtung an.
//
// Disclaimer - Exclusion of Liability
//
// This software is distributed in the hope that it will be useful, but WITHOUT
// ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
// FITNESS FOR A PARTICULAR PURPOSE. It can be used and modified by anyone
// free of any license obligations or authoring rights.
//=============================================================================

#include "ROBO_TX_PRG.h"

static enum {drauf, PAUSE_drauf, runter, PAUSE_runter, nachr, PAUSE_nachr, nachl, PAUSE_nachl} stage;
UINT16 tick; speed; lampspeed; lampnr;

/*-----------------------------------------------------------------------------
 * Function Name       : PrgInit
 *
 * This it the program initialization.
 * It is called once.
 *-----------------------------------------------------------------------------*/
void PrgInit
(
    TA * p_ta_array,    // pointer to the array of transfer areas
    int ta_count        // number of transfer areas in array (equal to TA_COUNT)
)
{
    stage = drauf;
    p_ta = &p_ta_array[TA_LOCAL];
    lampnr = 7;  speed = 440; tick = 0;  lampspeed = 350;
 }

/*-----------------------------------------------------------------------------
 * Function Name       : PrgTic
 *
 * This is the main function of this program.
 * It is called every tic (1 ms) realtime.
 *-----------------------------------------------------------------------------*/
int PrgTic
(
    TA * p_ta_array,    // pointer to the array of transfer areas
    int ta_count        // number of transfer areas in array (equal to TA_COUNT)
)
{
    int rc = 0x7FFF; // return code: 0x7FFF - program should be further called by the firmware;
                     //              0      - program should be normally stopped by the firmware;
                     //              any other value is considered by the firmware as an error code
                     //              and the program is stopped.

     INT16 track1;
     INT16 track2;
   
   // while (1)
     txsinglelamp(lampnr,lampspeed);
     tick++;    
    track1 = txtrack(0) ;   // false: schwarz  true: weiß
    track2 = txtrack(1) ;
    switch (stage)
        {
            case drauf:
                 if (!track1  && !track2) {
                 lampnr = 7; tick = 0; 
                 txmotor(0,1,380); txmotor(1,1,380); }
                 else  {  stage++;}
                 stage++;
                return rc;

            case PAUSE_drauf:
                if (tick = 100) {
                txmotor(0,1,480); txmotor(1,1,480);
                speed = 230; lampspeed = 180;}
                if (track1  || track2)
                {  stage++; }
                return rc;
 
            case runter:
            if (track1  && track2) {
            lampnr = 4; 
            txmotor(0,2,380); txmotor(1,2,380); speed = 512; }
                 else  {  stage++;}
                 stage++;
                return rc;

            case PAUSE_runter:
                 if (!track1  || !track2)
                {  stage++; }
                return rc;
            case nachr:
                 if (track1  && !track2) {
                 lampnr = 6; tick = 0;
                 txmotor(0,1,speed); txmotor(1,1,120); }
                 else  {  stage++;}
                 stage++;
                 return rc;

            case PAUSE_nachr:
                 if (tick = 220) {speed = 512; txmotor(0,1,speed); lampspeed = 480;}
                 if (!track1  || track2)
                 {  stage++; }
                 return rc;
 
            case nachl:
            if (!track1  && track2) {
            lampnr = 5;  tick = 0;
            txmotor(0,1,180); txmotor(1,1,speed); }
                 else  {  stage = drauf;}
                 stage++;
                 return rc;

            case PAUSE_nachl:
                 if (tick = 220) {speed = 512; txmotor(1,1,speed); lampspeed = 480;}
                 if (track1  || !track2)
                { stage = drauf; }
                return rc;

            default:
                return rc;
        }
     
    return rc;
}
