//=============================================================================
// Demo program PIXY.c
//
// Can be run under control of the ROBO TX Controller
// firmware in download (local) mode.
// This example shows how to sense the picture with the I2C Pixy - CMUcam5
// sensor. The port-address is 0x14. After writing some initialisation bytes the program enters
// a loop updating the measured values for the detected objects each 20ms.
// The Roboter trys to follow objekt number 1.
//
// The procedures i2cread, txsinglelamp and txmotor of ROBO_TX_PRG are used.
//
// 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 LIGHT_ON        DUTY_MAX
#define LIGHT_OFF       0

static enum
{   BYTESYNC1,
    WAIT_BYTESYNC1,
    BYTESYNC2,
    WAIT_BYTESYNC2,
    BYTESYNC3,
    SYNC_1,
    WAIT_1,
    SYNC_2,
    LOOP_0,
    WAIT_LOOP_0,
    SEARCH,
    PO,
    WAIT_3,
    PX,
    WAIT_4,
    PY,
    WAIT_5,
    PB,
    WAIT_6,
    PH,
    WAIT_7,
    action,
    Objekt,
    li,
    re,
    vw,
    rw,
    aus,
    sonstige,
} stage;

unsigned int ticks;
unsigned int next_action = 0;
UINT16 sync = 0xaa55;    
UINT16 chk, po, px, py, pb, ph; 
UINT16 status, value, speed;
 
 
/*-----------------------------------------------------------------------------
 * Function Name       : I2cCallback
 *
 * This callback function is called to inform the program about result (status)
 * of execution of any I2c command.
 *-----------------------------------------------------------------------------*/
static void I2cCallback
(
    TA * p_ta_array,
    I2C_CB * p_data
)
{
    status = p_data->status;
    value  = p_data->value;
    
                   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];
    
    ticks = 0;
    stage = BYTESYNC1;  
    speed = 480;
}

void links(speed)
{txmotor(0,1, speed / 4); txmotor(1,2,speed / 4);}      

void rechts(speed)
{txmotor(0,2, speed / 4); txmotor(1,1,speed / 4);}   

void vorwaerts(speed)
{txmotor(0,1, speed); txmotor(1,1,speed);} 

void zurueck(speed)
{txmotor(0,2, speed); txmotor(1,2,speed);} 

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

/*-----------------------------------------------------------------------------
 * 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];
  
    char str[64];
     
    ticks++;

    
    while(1)
    {
        switch(stage)
        {    case BYTESYNC1:
                i2cread(0x14, 0x00, 0xc4);         
                stage++;
                return rc;
                
            case WAIT_BYTESYNC1:
                // waiting for callback    
                return rc;
                
           case BYTESYNC2:
                if (value != 0x55) {stage = BYTESYNC1;}
                else {
                i2cread(0x14, 0x00, 0xc4);         
                stage++;}
                return rc;
                
            case WAIT_BYTESYNC2:
                // waiting for callback    
                return rc;
                    
            case BYTESYNC3:
                if (value != 0xaa) {stage = BYTESYNC1;}
                else {
                stage++;}
                return rc;
                
            case SYNC_1:
                i2cread(0x14, 0x00, 0xdc);         
                stage++;
                return rc;
                
            case WAIT_1:
                // waiting for callback    
                return rc;
                
            case SYNC_2:
                if (value == 0xaa55) {stage = SYNC_1;}
                else {stage++;}
                return rc;
                
            case LOOP_0:
                 if (value == 0) {
                 po = 0; px = 0; py = 0; pb = 0; ph = 0;
                 i2cread(0x14, 0x00, 0xcc);     //cc
                 stage++; }  
                 else {stage = PO;} 
                 return rc;
                 
            case WAIT_LOOP_0:
                 // waiting for callback    
                return rc;
                
            case SEARCH: 
            if (value == 0) {  stage = LOOP_0; }
            else {stage = SYNC_1;} 
            //     stage = SYNC_1;   
                 return rc;             
                
            case PO:
                i2cread(0x14, 0x00, 0xdc);
                stage++;
                return rc;
                
            case WAIT_3:
                // waiting for callback    
                return rc;
                
            case PX:
                po = value;
                i2cread(0x14, 0x00, 0xdc);
                stage++;
                return rc;
                
            case WAIT_4:
                // waiting for callback    
                return rc;
                
            case PY:
                px = value;
                i2cread(0x14, 0x00, 0xdc);
                stage++;
                return rc;
                
            case WAIT_5:
                // waiting for callback    
                return rc;
                
            case PB:
                py = value; 
                i2cread(0x14, 0x00, 0xdc);      
                stage++;
                return rc;
                
            case WAIT_6:
                // waiting for callback    
                return rc; 
                  
            case PH:
                pb = value;
                i2cread(0x14, 0x00, 0xcc);       
                stage++;
                return rc;
                
            case WAIT_7:
                // waiting for callback    
                return rc;                               
                
            case action:
                ph = value;
                if(ticks < next_action)  
                { stage = SYNC_1;}
                else {stage++ ;}
                 return rc;   
            case Objekt: 
                 next_action = ticks + 20;    
                 if (po == 1) 
                 {txsinglelamp(6, 510);
                  stage++ ;}
                 else {stage = sonstige;} 
                 return rc;
            case li:         
                 if (px < 115) {links (speed * 3); stage = SYNC_1;}   
                 else {stage++ ;}
                 return rc;
             case re:         
                 if (px > 205) {rechts (speed * 3); stage = SYNC_1;}   
                 else {stage++ ;}
                 return rc;  
             case vw:         
                 if (pb < 35) {vorwaerts(speed); stage = SYNC_1;}   
                 else {stage++ ;}
                 return rc; 
             case rw:         
                 if (pb > 60) {zurueck(speed); stage = SYNC_1;}   
                 else {stage++ ;}
                 return rc;
             case aus:         
                 stopp(); 
                 stage = SYNC_1;   
                 return rc;    
             case sonstige:         
                 if (po == 2) {txsinglelamp(7, 490); } 
                 else {if (po == 3) {txsinglelamp(5, 400); }
                       else {txlamp(5,0); txlamp(6,0); txlamp(7,0);  }
                      }
                 stopp();
                 stage = SYNC_1;   
                 return rc;
        }
    }    
    return rc;
} 