SmartWheels self-driving race car. Designed for NXP Cup. Uses FRDM-KL25Z, area-scan camera, and simple image processing to detect and navigate any NXP spec track.

Dependencies:   TSI USBDevice mbed-dev

Fork of SmartWheels by haofan Zheng

Hardwares/ArduTouch.cpp

Committer:
hazheng
Date:
2017-04-19
Revision:
93:8e1bd3602d53
Parent:
81:32bd7a25a699

File content as of revision 93:8e1bd3602d53:

#include "ArduTouch.h"

#include "GlobalVariable.h"
#include "PinAssignment.h"

//#define SW_DEBUG
#include "SWCommon.h"

#ifdef __cplusplus
extern "C" {
#endif

#define TOUCH_PREC 10

#define PixSizeX    13.78
#define PixOffsX    411
#define PixSizeY    4.5
#define PixOffsY    378

static DigitalOut touch_cs(PIN_ATC_TOUCH_CS, 1);
static InterruptIn touch_irq(PIN_ATC_TOUCH_IRQ);
static int16_t value_x = 0;
static int16_t value_y = 0;

static Timeout touch_recover_timer;

static void (*touch_irq_func)(void) = NULL;
static void (*touch_xy_func)(int16_t, int16_t) = NULL;

void ardu_touch_recover();
void ardu_touch_read();
void ardu_touch_irq_handler();

void ardu_touch_recover()
{
    touch_irq.fall(&ardu_touch_irq_handler);
}

inline uint16_t ardu_touch_read_data_16(uint8_t address)
{
    uint16_t result;
    
    touch_cs = 0; 
    g_spi_port.write(address);
    result = static_cast<uint8_t>(g_spi_port.write(0x00)) << 8;
    result |= static_cast<uint8_t>(g_spi_port.write(0x00));
    result = result >> 4;
    touch_cs = 1;

    return result;
}

void ardu_touch_read()
{
    unsigned long tx = 0;
    unsigned long ty = 0;                

    for (int i = 0; i < TOUCH_PREC; ++i)
    {   
        ty+=ardu_touch_read_data_16(0x90);
        tx+=ardu_touch_read_data_16(0xD0);
    }
    
    int touch_x, touch_y;
    touch_x = tx / TOUCH_PREC;
    touch_y = ty / TOUCH_PREC;
    
    value_x = ((touch_y - PixOffsY) / PixSizeY) + 15;
    
    if (value_x < 0)
        value_x = 0;
    else if(value_x > 320)
        value_x = 320;
        
    value_y = (((240 - ((touch_x - PixOffsX) / PixSizeX)) - 150) * 2) + 15;
    
    if (value_y < 0)
        value_y = 0;
    else if(value_y > 240)
        value_x = 240;
        
    //LOGI("#%d, %d#", value_x, value_y);
}

/*
inline uint16_t ardu_touch_read_data_16(uint8_t cmd)
{
    uint16_t result;
    touch_cs = 0;
    g_spi_port.write(cmd);
    wait_us(1);
    result = static_cast<uint8_t>(g_spi_port.write(0x00)) << 8;
    result |= static_cast<uint8_t>(g_spi_port.write(0x00));
    result = result >> 4;
    touch_cs = 1;
    return result;
}

#define LOST_VAL 1
inline uint16_t ardu_touch_read_pos(uint8_t cmd)
{
    uint16_t buf[TOUCH_PREC];
    uint16_t sum = 0;
    uint16_t temp;
    for(int i = 0; i < TOUCH_PREC; ++i)
    {
        buf[i] = ardu_touch_read_data_16(cmd);
    }
    for(int i = 0; i < TOUCH_PREC - 1; ++i)
    {
        for(int j = i + 1; j < TOUCH_PREC; ++j)
        {
            if(buf[i] > buf[j])
            {
                temp = buf[i];
                buf[i] = buf[j];
                buf[j] = temp;
            }
        }
    }
    for(int i = LOST_VAL; i < TOUCH_PREC - LOST_VAL; ++i)
    {
        sum += buf[i];
    }
    return (sum / (TOUCH_PREC - (2 * LOST_VAL)));
}

void ardu_touch_read()
{
    touch_irq.fall(NULL);
    
    touch_x = ardu_touch_read_pos(0X90);
    touch_y = ardu_touch_read_pos(0XD0);
    
    int value_x, value_y;
    
    if (PixSizeY<0)
        value_x = 400 - ((touch_y - PixOffsY) / -PixSizeY);
    else
        value_x = ((touch_y - PixOffsY) / PixSizeY);
    
    if (value_x < 0)
        value_x = 0;
        
    if (PixSizeX>=0)
    {
        value_y = 240 - ((touch_x - PixOffsX) / PixSizeX);
    }
    else
    {
        value_y = (touch_x - PixOffsX) / -(PixSizeX);
    }
    
    if (value_y < 0)
        value_y = 0;
    
    LOGI("#%d, %d#", value_x, value_y);
    
    touch_recover_timer.attach(&ardu_touch_recover, 0.5f);
}
*/

void ardu_touch_irq_handler()
{
    touch_irq.fall(NULL);
    
    if(touch_irq_func)
    {
        (*touch_irq_func)();
    }
    if(touch_xy_func)
    {
        ardu_touch_read();
        (*touch_xy_func)(value_x, value_y);
    }
    /*
    ardu_touch_read();
    ardu_utft_set_xy(value_x, value_y, value_x, value_y);
    ardu_utft_write_DATA(0xF8, 0x00);
    */
    
    touch_recover_timer.attach(&ardu_touch_recover, 0.5f);
}


void ardu_touch_init()
{
    touch_irq.fall(&ardu_touch_irq_handler);
}

void ardu_touch_set_irq_function(void(*irq_func)(void))
{
    touch_irq_func = irq_func;
}

void ardu_touch_set_pos_function(void(*pos_func)(int16_t, int16_t))
{
    touch_xy_func = pos_func;
}

void ardu_touch_get_pos(int16_t* x, int16_t* y)
{
    *x = value_x;
    *y = value_y;
}

#ifdef __cplusplus
}
#endif