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
Hardwares/ArduTouch.cpp
- Committer:
- hazheng
- Date:
- 2017-04-18
- Branch:
- Drift
- Revision:
- 81:32bd7a25a699
- Parent:
- 80:c85cb93713b3
- Child:
- 93:8e1bd3602d53
File content as of revision 81:32bd7a25a699:
#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) { } #ifdef __cplusplus } #endif