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-18
Branch:
Drift
Revision:
79:bdbac82c979b
Parent:
78:9f20bf037db6
Child:
80:c85cb93713b3

File content as of revision 79:bdbac82c979b:

#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 uint16_t touch_x = 0;
static uint16_t touch_y = 0;

static Timeout touch_recover_timer;

void ardu_touch_recover();
void ardu_touch_read();

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

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()
{
    touch_irq.fall(NULL);
    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);
    }

    touch_x = tx / TOUCH_PREC;
    touch_y = ty / TOUCH_PREC;
    
    int value_x, value_y;
    
    value_x = ((touch_y - PixOffsY) / PixSizeY);
    
    if (value_x < 0)
        value_x = 0;
    else if(value_x > 320)
        value_x = 320;
        
    value_y = ((240 - ((touch_x - PixOffsX) / PixSizeX)) - 150) * 2;
    
    if (value_y < 0)
        value_y = 0;
    else if(value_y > 240)
        value_x = 240;
        
    LOGI("#%d, %d#", value_x, value_y);
    
    touch_recover_timer.attach(&ardu_touch_recover, 0.5f);
}

/*
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_init()
{
    touch_irq.fall(&ardu_touch_read);
}

void ardu_touch_get_pos(int16_t* x, int16_t* y)
{
    
}

#ifdef __cplusplus
}
#endif