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:
- 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