//=============================================================================
// Demo program Licht.c
//
// Can be run under control of the ROBO TX Controller
// firmware in download (local) mode.
// An I7 und I8 sind Fotowiderstände (LDR) angeschlossen.
// Eine helle Glühbirne dienst als Ziel.
// Der Roboter dreht sich zum Licht und fährt in die Richtung.
// 
// Benutzte Prozeduren: txanalog und txmotor.
//
// 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"

UINT16 lr, ll , tick;
static enum {neu, warte} 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_LOCAL];
    p_ta = &p_ta_array[TA_LOCAL];
    tick = 0;
    stage = neu;
}

int Lichtr()
{  int rg = 0, i;
   for (i=0; i<10; i++) {
   rg += txanalog(6);}
   return rg / 10;   //114 * rg / 1000
}  
 
 int Lichtl()
{  int rg = 0, i;
   for (i=0; i<10; i++) {
   rg += txanalog(7);}
   return  rg / 10;
}   
void drehe()
{txmotor(0,1, 450); txmotor(1,2,450);}      

void vor(UINT16 h)
{ if (h > 800) {txmotor(0,1, 512); txmotor(1,1,512);}
else {if (h < 500) {txmotor(0,2, 512); txmotor(1,2,512); }
         else {stopp();}
         }
}         
  

void rechts()
{txmotor(0,1, 312); txmotor(1,1,512);}   

void links()
{txmotor(0,1, 512); txmotor(1,1,312);} 

void stopp()
{txmotor(0,0, 400); txmotor(1,0,400);} 


/*-----------------------------------------------------------------------------
 * 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_LOCAL];

  tick++;
  switch (stage)
        {
            case neu:
                  txlamp(7,500); 
         lr = Lichtr(); ll= Lichtl();
          if ((lr + ll) > 3000) {drehe();}
            else { if (lr > (ll + 80)) {links();}
                else { if (ll > (lr + 80)) {rechts();}
                  else { if ((abs (lr - ll)) < 160) {vor (lr+ll);}
                  }}} 
            stage++;
            return rc;

            case warte:
                if (tick > 60) { tick = 0; stage = neu;}
                return rc;
            default:
                return rc;
        }     
    return rc;
} 