//=============================================================================
// Demo program "Hindernisse".
//
// Can be run under control of the ROBO TX Controller
// firmware in download (local) mode.
// 
// Der Abstandssensor ist in einem 45°-Winkel vorne rechts befestigt und an I4 
// angeschlossen. Eine Lampe ist in O8 angeschlossen und zeigt mit ihrer Helligkeit
// die gemessene Entfernung.
// Von ROBO_TX_PRG werden txlamp, txmotor und txdistance verwendet.
// Durch die Warteschleifen (stage = warte... ) ergibt sich ein ruhiges Verhalten. 
//
// 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"

#define TA_IDX          TA_LOCAL

static int duty;
static UINT16 speed;
static enum {zunah, wartezunah, zuweit,  wartezuweit, richtig, warterichtig} stage;

/*-----------------------------------------------------------------------------
 * 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)
)
{
    //TA * p_ta = &p_ta_array[TA_IDX];
    p_ta = &p_ta_array[TA_IDX];
    
    stage = zunah;
    duty = 0;
    speed = 465;
}
void linksbogen(speed)
{txmotor(0,1, (speed * 11) / 10); txmotor(1,1,(speed * 7) / 10);}      

void rechtsbogen(speed)
{txmotor(0,1, (speed * 7) / 10); txmotor(1,1,(speed * 11) / 10);}   

void vorwaerts(speed)
{txmotor(0,1, (speed * 11) / 10); txmotor(1,1,(speed * 11) / 10);}   

/*-----------------------------------------------------------------------------
 * 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.
    //TA * p_ta = &p_ta_array[TA_IDX];
    
    duty = txdistance(3);
    txlamp(7,520 - 5 * duty);
 /*  vor    txmotor(0,1,380); txmotor(1,1,380);
   nachr    txmotor(0,1,speed); txmotor(1,1,120); */
  switch (stage)
        {
            case zunah:
                 if (duty < 14) {  linksbogen(speed);}
                 else  {  stage++;}
                 stage++;
                return rc;

            case wartezunah:
                if (duty > 13) { stage++;}
                return rc;
                
            case zuweit:
                 if (duty > 31) {  rechtsbogen(speed);}
                 else  {  stage++;}
                 stage++;
                return rc;

            case wartezuweit:
                if (duty < 31) { stage++;}
                return rc; 
                 
            case richtig:
                 if ((duty > 13) && (duty < 31)) {  vorwaerts(speed);
                  stage++; }
                 else  {  stage = zunah;}
                return rc;

            case warterichtig:
                if ((duty < 14) || (duty > 30)) { stage = zunah; }
                return rc; 
            default:
                return rc;
        }            
    return rc;
}
