Jordan Brack / SmartWheelsPublic

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@100:ffbeefc9e218, 2017-04-20 (annotated)

Committer:
hazheng
Date:
Thu Apr 20 21:04:10 2017 +0000
Revision:
100:ffbeefc9e218
Parent:
93:8e1bd3602d53 
Better version of Intersection detection.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hazheng 78:9f20bf037db6 1 #include "ArduTouch.h"
hazheng 78:9f20bf037db6 2
hazheng 79:bdbac82c979b 3 #include "GlobalVariable.h"
hazheng 79:bdbac82c979b 4 #include "PinAssignment.h"
hazheng 79:bdbac82c979b 5
hazheng 81:32bd7a25a699 6 //#define SW_DEBUG
hazheng 79:bdbac82c979b 7 #include "SWCommon.h"
hazheng 78:9f20bf037db6 8
hazheng 78:9f20bf037db6 9 #ifdef __cplusplus
hazheng 78:9f20bf037db6 10 extern "C" {
hazheng 78:9f20bf037db6 11 #endif
hazheng 78:9f20bf037db6 12
hazheng 79:bdbac82c979b 13 #define TOUCH_PREC 10
hazheng 78:9f20bf037db6 14
hazheng 79:bdbac82c979b 15 #define PixSizeX 13.78
hazheng 79:bdbac82c979b 16 #define PixOffsX 411
hazheng 79:bdbac82c979b 17 #define PixSizeY 4.5
hazheng 79:bdbac82c979b 18 #define PixOffsY 378
hazheng 79:bdbac82c979b 19
hazheng 79:bdbac82c979b 20 static DigitalOut touch_cs(PIN_ATC_TOUCH_CS, 1);
hazheng 79:bdbac82c979b 21 static InterruptIn touch_irq(PIN_ATC_TOUCH_IRQ);
hazheng 80:c85cb93713b3 22 static int16_t value_x = 0;
hazheng 80:c85cb93713b3 23 static int16_t value_y = 0;
hazheng 79:bdbac82c979b 24
hazheng 79:bdbac82c979b 25 static Timeout touch_recover_timer;
hazheng 79:bdbac82c979b 26
hazheng 80:c85cb93713b3 27 static void (*touch_irq_func)(void) = NULL;
hazheng 80:c85cb93713b3 28 static void (*touch_xy_func)(int16_t, int16_t) = NULL;
hazheng 80:c85cb93713b3 29
hazheng 79:bdbac82c979b 30 void ardu_touch_recover();
hazheng 79:bdbac82c979b 31 void ardu_touch_read();
hazheng 80:c85cb93713b3 32 void ardu_touch_irq_handler();
hazheng 79:bdbac82c979b 33
hazheng 79:bdbac82c979b 34 void ardu_touch_recover()
hazheng 79:bdbac82c979b 35 {
hazheng 80:c85cb93713b3 36 touch_irq.fall(&ardu_touch_irq_handler);
hazheng 79:bdbac82c979b 37 }
hazheng 79:bdbac82c979b 38
hazheng 79:bdbac82c979b 39 inline uint16_t ardu_touch_read_data_16(uint8_t address)
hazheng 79:bdbac82c979b 40 {
hazheng 79:bdbac82c979b 41 uint16_t result;
hazheng 79:bdbac82c979b 42
hazheng 79:bdbac82c979b 43 touch_cs = 0;
hazheng 79:bdbac82c979b 44 g_spi_port.write(address);
hazheng 79:bdbac82c979b 45 result = static_cast<uint8_t>(g_spi_port.write(0x00)) << 8;
hazheng 79:bdbac82c979b 46 result |= static_cast<uint8_t>(g_spi_port.write(0x00));
hazheng 79:bdbac82c979b 47 result = result >> 4;
hazheng 79:bdbac82c979b 48 touch_cs = 1;
hazheng 79:bdbac82c979b 49
hazheng 79:bdbac82c979b 50 return result;
hazheng 79:bdbac82c979b 51 }
hazheng 79:bdbac82c979b 52
hazheng 79:bdbac82c979b 53 void ardu_touch_read()
hazheng 79:bdbac82c979b 54 {
hazheng 79:bdbac82c979b 55 unsigned long tx = 0;
hazheng 79:bdbac82c979b 56 unsigned long ty = 0;
hazheng 79:bdbac82c979b 57
hazheng 79:bdbac82c979b 58 for (int i = 0; i < TOUCH_PREC; ++i)
hazheng 79:bdbac82c979b 59 {
hazheng 79:bdbac82c979b 60 ty+=ardu_touch_read_data_16(0x90);
hazheng 79:bdbac82c979b 61 tx+=ardu_touch_read_data_16(0xD0);
hazheng 79:bdbac82c979b 62 }
hazheng 80:c85cb93713b3 63
hazheng 80:c85cb93713b3 64 int touch_x, touch_y;
hazheng 79:bdbac82c979b 65 touch_x = tx / TOUCH_PREC;
hazheng 79:bdbac82c979b 66 touch_y = ty / TOUCH_PREC;
hazheng 79:bdbac82c979b 67
hazheng 80:c85cb93713b3 68 value_x = ((touch_y - PixOffsY) / PixSizeY) + 15;
hazheng 79:bdbac82c979b 69
hazheng 79:bdbac82c979b 70 if (value_x < 0)
hazheng 79:bdbac82c979b 71 value_x = 0;
hazheng 79:bdbac82c979b 72 else if(value_x > 320)
hazheng 79:bdbac82c979b 73 value_x = 320;
hazheng 79:bdbac82c979b 74
hazheng 80:c85cb93713b3 75 value_y = (((240 - ((touch_x - PixOffsX) / PixSizeX)) - 150) * 2) + 15;
hazheng 79:bdbac82c979b 76
hazheng 79:bdbac82c979b 77 if (value_y < 0)
hazheng 79:bdbac82c979b 78 value_y = 0;
hazheng 79:bdbac82c979b 79 else if(value_y > 240)
hazheng 79:bdbac82c979b 80 value_x = 240;
hazheng 79:bdbac82c979b 81
hazheng 80:c85cb93713b3 82 //LOGI("#%d, %d#", value_x, value_y);
hazheng 79:bdbac82c979b 83 }
hazheng 79:bdbac82c979b 84
hazheng 79:bdbac82c979b 85 /*
hazheng 79:bdbac82c979b 86 inline uint16_t ardu_touch_read_data_16(uint8_t cmd)
hazheng 79:bdbac82c979b 87 {
hazheng 79:bdbac82c979b 88 uint16_t result;
hazheng 79:bdbac82c979b 89 touch_cs = 0;
hazheng 79:bdbac82c979b 90 g_spi_port.write(cmd);
hazheng 79:bdbac82c979b 91 wait_us(1);
hazheng 79:bdbac82c979b 92 result = static_cast<uint8_t>(g_spi_port.write(0x00)) << 8;
hazheng 79:bdbac82c979b 93 result |= static_cast<uint8_t>(g_spi_port.write(0x00));
hazheng 79:bdbac82c979b 94 result = result >> 4;
hazheng 79:bdbac82c979b 95 touch_cs = 1;
hazheng 79:bdbac82c979b 96 return result;
hazheng 79:bdbac82c979b 97 }
hazheng 79:bdbac82c979b 98
hazheng 79:bdbac82c979b 99 #define LOST_VAL 1
hazheng 79:bdbac82c979b 100 inline uint16_t ardu_touch_read_pos(uint8_t cmd)
hazheng 79:bdbac82c979b 101 {
hazheng 79:bdbac82c979b 102 uint16_t buf[TOUCH_PREC];
hazheng 79:bdbac82c979b 103 uint16_t sum = 0;
hazheng 79:bdbac82c979b 104 uint16_t temp;
hazheng 79:bdbac82c979b 105 for(int i = 0; i < TOUCH_PREC; ++i)
hazheng 79:bdbac82c979b 106 {
hazheng 79:bdbac82c979b 107 buf[i] = ardu_touch_read_data_16(cmd);
hazheng 79:bdbac82c979b 108 }
hazheng 79:bdbac82c979b 109 for(int i = 0; i < TOUCH_PREC - 1; ++i)
hazheng 79:bdbac82c979b 110 {
hazheng 79:bdbac82c979b 111 for(int j = i + 1; j < TOUCH_PREC; ++j)
hazheng 79:bdbac82c979b 112 {
hazheng 79:bdbac82c979b 113 if(buf[i] > buf[j])
hazheng 79:bdbac82c979b 114 {
hazheng 79:bdbac82c979b 115 temp = buf[i];
hazheng 79:bdbac82c979b 116 buf[i] = buf[j];
hazheng 79:bdbac82c979b 117 buf[j] = temp;
hazheng 79:bdbac82c979b 118 }
hazheng 79:bdbac82c979b 119 }
hazheng 79:bdbac82c979b 120 }
hazheng 79:bdbac82c979b 121 for(int i = LOST_VAL; i < TOUCH_PREC - LOST_VAL; ++i)
hazheng 79:bdbac82c979b 122 {
hazheng 79:bdbac82c979b 123 sum += buf[i];
hazheng 79:bdbac82c979b 124 }
hazheng 79:bdbac82c979b 125 return (sum / (TOUCH_PREC - (2 * LOST_VAL)));
hazheng 79:bdbac82c979b 126 }
hazheng 79:bdbac82c979b 127
hazheng 79:bdbac82c979b 128 void ardu_touch_read()
hazheng 79:bdbac82c979b 129 {
hazheng 79:bdbac82c979b 130 touch_irq.fall(NULL);
hazheng 79:bdbac82c979b 131
hazheng 79:bdbac82c979b 132 touch_x = ardu_touch_read_pos(0X90);
hazheng 79:bdbac82c979b 133 touch_y = ardu_touch_read_pos(0XD0);
hazheng 79:bdbac82c979b 134
hazheng 79:bdbac82c979b 135 int value_x, value_y;
hazheng 79:bdbac82c979b 136
hazheng 79:bdbac82c979b 137 if (PixSizeY<0)
hazheng 79:bdbac82c979b 138 value_x = 400 - ((touch_y - PixOffsY) / -PixSizeY);
hazheng 79:bdbac82c979b 139 else
hazheng 79:bdbac82c979b 140 value_x = ((touch_y - PixOffsY) / PixSizeY);
hazheng 79:bdbac82c979b 141
hazheng 79:bdbac82c979b 142 if (value_x < 0)
hazheng 79:bdbac82c979b 143 value_x = 0;
hazheng 79:bdbac82c979b 144
hazheng 79:bdbac82c979b 145 if (PixSizeX>=0)
hazheng 79:bdbac82c979b 146 {
hazheng 79:bdbac82c979b 147 value_y = 240 - ((touch_x - PixOffsX) / PixSizeX);
hazheng 79:bdbac82c979b 148 }
hazheng 79:bdbac82c979b 149 else
hazheng 79:bdbac82c979b 150 {
hazheng 79:bdbac82c979b 151 value_y = (touch_x - PixOffsX) / -(PixSizeX);
hazheng 79:bdbac82c979b 152 }
hazheng 79:bdbac82c979b 153
hazheng 79:bdbac82c979b 154 if (value_y < 0)
hazheng 79:bdbac82c979b 155 value_y = 0;
hazheng 79:bdbac82c979b 156
hazheng 79:bdbac82c979b 157 LOGI("#%d, %d#", value_x, value_y);
hazheng 79:bdbac82c979b 158
hazheng 79:bdbac82c979b 159 touch_recover_timer.attach(&ardu_touch_recover, 0.5f);
hazheng 79:bdbac82c979b 160 }
hazheng 79:bdbac82c979b 161 */
hazheng 80:c85cb93713b3 162
hazheng 80:c85cb93713b3 163 void ardu_touch_irq_handler()
hazheng 80:c85cb93713b3 164 {
hazheng 80:c85cb93713b3 165 touch_irq.fall(NULL);
hazheng 80:c85cb93713b3 166
hazheng 80:c85cb93713b3 167 if(touch_irq_func)
hazheng 80:c85cb93713b3 168 {
hazheng 80:c85cb93713b3 169 (*touch_irq_func)();
hazheng 80:c85cb93713b3 170 }
hazheng 80:c85cb93713b3 171 if(touch_xy_func)
hazheng 80:c85cb93713b3 172 {
hazheng 80:c85cb93713b3 173 ardu_touch_read();
hazheng 80:c85cb93713b3 174 (*touch_xy_func)(value_x, value_y);
hazheng 80:c85cb93713b3 175 }
hazheng 80:c85cb93713b3 176 /*
hazheng 80:c85cb93713b3 177 ardu_touch_read();
hazheng 80:c85cb93713b3 178 ardu_utft_set_xy(value_x, value_y, value_x, value_y);
hazheng 80:c85cb93713b3 179 ardu_utft_write_DATA(0xF8, 0x00);
hazheng 80:c85cb93713b3 180 */
hazheng 80:c85cb93713b3 181
hazheng 80:c85cb93713b3 182 touch_recover_timer.attach(&ardu_touch_recover, 0.5f);
hazheng 80:c85cb93713b3 183 }
hazheng 80:c85cb93713b3 184
hazheng 80:c85cb93713b3 185
hazheng 79:bdbac82c979b 186 void ardu_touch_init()
hazheng 79:bdbac82c979b 187 {
hazheng 80:c85cb93713b3 188 touch_irq.fall(&ardu_touch_irq_handler);
hazheng 80:c85cb93713b3 189 }
hazheng 80:c85cb93713b3 190
hazheng 80:c85cb93713b3 191 void ardu_touch_set_irq_function(void(*irq_func)(void))
hazheng 80:c85cb93713b3 192 {
hazheng 80:c85cb93713b3 193 touch_irq_func = irq_func;
hazheng 80:c85cb93713b3 194 }
hazheng 80:c85cb93713b3 195
hazheng 80:c85cb93713b3 196 void ardu_touch_set_pos_function(void(*pos_func)(int16_t, int16_t))
hazheng 80:c85cb93713b3 197 {
hazheng 80:c85cb93713b3 198 touch_xy_func = pos_func;
hazheng 79:bdbac82c979b 199 }
hazheng 79:bdbac82c979b 200
hazheng 79:bdbac82c979b 201 void ardu_touch_get_pos(int16_t* x, int16_t* y)
hazheng 79:bdbac82c979b 202 {
hazheng 93:8e1bd3602d53 203 *x = value_x;
hazheng 93:8e1bd3602d53 204 *y = value_y;
hazheng 79:bdbac82c979b 205 }
hazheng 78:9f20bf037db6 206
hazheng 78:9f20bf037db6 207 #ifdef __cplusplus
hazheng 78:9f20bf037db6 208 }
hazheng 78:9f20bf037db6 209 #endif