ESP32 Code zur Laser-Steuerung
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

laserpong_main.c 11KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343
  1. /* Hello World Example
  2. This example code is in the Public Domain (or CC0 licensed, at your option.)
  3. Unless required by applicable law or agreed to in writing, this
  4. software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
  5. CONDITIONS OF ANY KIND, either express or implied.
  6. */
  7. #include <stdio.h>
  8. #include <math.h>
  9. #include <driver/gpio.h>
  10. #include "freertos/FreeRTOS.h"
  11. #include "freertos/task.h"
  12. #include "esp_system.h"
  13. #include "esp_spi_flash.h"
  14. #include "driver/ledc.h"
  15. #include "logo.h"
  16. #define GPIO_LEFTRIGHT GPIO_NUM_15
  17. #define GPIO_UPDOWN GPIO_NUM_13
  18. #define GPIO_LASER GPIO_NUM_4
  19. #define GPIO_LASER_SEL GPIO_SEL_4
  20. // some servo-specific constants, those also depend on the way the servos are attached to the horns
  21. #define UPDOWN_STRAIGHT 2550 // -> 0 deg
  22. #define UPDOWN_DOWN 1280 // -> 90 deg = pi/2
  23. #define LEFTRIGHT_LEFT 3200 // max: 3276 -> -90 deg = -pi/2
  24. #define LEFTRIGHT_MIDDLE 1920 // -> 0 deg
  25. #define LEFTRIGHT_RIGHT 900 // -> 90 deg = pi/2
  26. // time to move to the desired position
  27. #define MILLISECONDS_PER_DEGREE (100.0 / 60.0)
  28. // we use 1280 as the required payload difference for 90 degrees, which is more or less correct
  29. #define MILLISECONDS_PER_VALUE ((MILLISECONDS_PER_DEGREE*90.0)/1280.0)
  30. /*
  31. * The position of the laser relative to the projection surface is such that the laser is centered horizontally,
  32. * but sits on the top edge of the surface, so a LEFTRIGHT_MIDDLE and UPDOWN_STRAIGHT point is in the center of
  33. * the top edge
  34. * The values that follow here are the dimensions of the surface and the distance of the laser (same unit)
  35. * such that the corresponding angles for the corners can easily be computed.
  36. *
  37. * coordinates in screen space:
  38. * (0,0) (mx/2,0) (mx,0)
  39. * +-------------------------------------+ ^
  40. * | | |
  41. * | | SCREEN_HEIGHT
  42. * | | |
  43. * | | |
  44. * +-------------------------------------+ v
  45. * (0,my) (mx/2,my) (mx,my)
  46. * <------- SCREEN_WIDTH ---------------->
  47. *
  48. * if resolution is set to 1, mx = SCREEN_WIDTH and my = SCREEN_HEIGHT
  49. */
  50. // distance to the projection surface (all units are cm)
  51. #define SCREEN_DISTANCE 100
  52. #define SCREEN_WIDTH 100
  53. #define SCREEN_HEIGHT 50
  54. // how many "pixels" per unit
  55. #define SCREEN_LEFTRIGHT_RESOLUTION 1
  56. #define SCREEN_UPDOWN_RESOLUTION 1
  57. #define PADDLE_SIZE (SCREEN_HEIGHT/5)
  58. #define BALL_SEGMENTS 4
  59. /*
  60. * Precomputed values for each coordinate
  61. */
  62. uint32_t leftRightValues[SCREEN_WIDTH*SCREEN_LEFTRIGHT_RESOLUTION];
  63. uint32_t upDownValues[SCREEN_HEIGHT*SCREEN_UPDOWN_RESOLUTION];
  64. // current positions (values)
  65. uint32_t currentLeftRightValue, currentUpDownValue;
  66. void precompute_values()
  67. {
  68. double ang;
  69. int i;
  70. for(i=SCREEN_WIDTH*SCREEN_LEFTRIGHT_RESOLUTION/2-1; i>=0; --i) {
  71. ang = -atan2(((double)i)/SCREEN_LEFTRIGHT_RESOLUTION, SCREEN_DISTANCE);
  72. leftRightValues[(SCREEN_WIDTH/2)*SCREEN_LEFTRIGHT_RESOLUTION - 1 - i] = (uint32_t)(LEFTRIGHT_MIDDLE-((LEFTRIGHT_LEFT-LEFTRIGHT_MIDDLE)*ang/M_PI_2));
  73. printf("leftright %d - %f\n", (SCREEN_WIDTH/2)*SCREEN_LEFTRIGHT_RESOLUTION - 1 - i, LEFTRIGHT_MIDDLE-((LEFTRIGHT_LEFT-LEFTRIGHT_MIDDLE)*ang/M_PI_2));
  74. }
  75. for(i=1; i<=SCREEN_WIDTH*SCREEN_LEFTRIGHT_RESOLUTION/2; i++) {
  76. ang = atan2((double)i/SCREEN_LEFTRIGHT_RESOLUTION, SCREEN_DISTANCE);
  77. leftRightValues[(SCREEN_WIDTH/2)*SCREEN_LEFTRIGHT_RESOLUTION + i - 1] = (uint32_t)(LEFTRIGHT_MIDDLE+((LEFTRIGHT_RIGHT-LEFTRIGHT_MIDDLE)*ang/M_PI_2));
  78. printf("leftright %d - %f\n", (SCREEN_WIDTH/2)*SCREEN_LEFTRIGHT_RESOLUTION + i - 1, LEFTRIGHT_MIDDLE+((LEFTRIGHT_RIGHT-LEFTRIGHT_MIDDLE)*ang/M_PI_2));
  79. }
  80. for(i=0; i<SCREEN_HEIGHT*SCREEN_UPDOWN_RESOLUTION; i++) {
  81. ang = atan2((double)i/SCREEN_LEFTRIGHT_RESOLUTION, SCREEN_DISTANCE);
  82. upDownValues[i] = (uint32_t)(UPDOWN_STRAIGHT + ((UPDOWN_DOWN-UPDOWN_STRAIGHT)*ang/M_PI_2));
  83. printf("updown %d - %f\n", i, UPDOWN_STRAIGHT + ((UPDOWN_DOWN-UPDOWN_STRAIGHT)*ang/M_PI_2));
  84. }
  85. }
  86. void setPWM()
  87. {
  88. // updown
  89. ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0, currentUpDownValue);
  90. ledc_set_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1, currentLeftRightValue);
  91. printf("Setting updown value to %d\n", currentUpDownValue);
  92. printf("Setting leftright value to %d\n", currentLeftRightValue);
  93. ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_0);
  94. ledc_update_duty(LEDC_HIGH_SPEED_MODE, LEDC_CHANNEL_1);
  95. }
  96. void go_to(uint8_t x, uint8_t y) {
  97. uint32_t vdiff1 = abs(currentUpDownValue-upDownValues[y]);
  98. uint32_t vdiff2 = abs(currentLeftRightValue-leftRightValues[x]);
  99. uint32_t vdiff = (vdiff1 > vdiff2 ? vdiff1 : vdiff2);
  100. uint32_t time = vdiff * MILLISECONDS_PER_VALUE;
  101. time = 20 * (time / 20 + 1);
  102. //if(time < 20) time = 20; // 20 ms is the minimum for a 50hz pwm...
  103. //if(time < 40) time = 40; // make sure we have at least 2 cycles
  104. //if(time < 10) time = 10;
  105. currentUpDownValue = upDownValues[y];
  106. currentLeftRightValue = leftRightValues[x];
  107. setPWM();
  108. printf("Waiting for %d ms", time);
  109. vTaskDelay(1.5 * time / portTICK_PERIOD_MS);
  110. }
  111. void laser_on()
  112. {
  113. vTaskDelay(20 / portTICK_PERIOD_MS);
  114. gpio_set_level(GPIO_LASER, 1);
  115. }
  116. void laser_off()
  117. {
  118. gpio_set_level(GPIO_LASER, 0);
  119. vTaskDelay(20 / portTICK_PERIOD_MS);
  120. }
  121. void draw_line()
  122. {
  123. for(int i=0; i<10; i++) {
  124. go_to(0, 0);
  125. go_to(0, SCREEN_HEIGHT - 1);
  126. }
  127. }
  128. void draw_paddle(int side)
  129. {
  130. uint8_t x = (side==0?0:(SCREEN_WIDTH-1));
  131. uint8_t y = PADDLE_SIZE;
  132. printf("Draw paddle\n");
  133. go_to(x, y);
  134. laser_on();
  135. vTaskDelay(10 / portTICK_PERIOD_MS);
  136. //for(int i=0;i<2;i++) {
  137. go_to(x, y + PADDLE_SIZE);
  138. vTaskDelay(10 / portTICK_PERIOD_MS);
  139. go_to(x, y);
  140. //}
  141. laser_off();
  142. }
  143. void draw_ball()
  144. {
  145. uint8_t x = SCREEN_WIDTH/2;
  146. uint8_t y = SCREEN_HEIGHT/2;
  147. printf("Draw ball");
  148. go_to(x,y+1);
  149. laser_on();
  150. //go_to(x,y+1);
  151. go_to(x+1,y);
  152. go_to(x,y-1);
  153. go_to(x-1,y);
  154. go_to(x,y+1);
  155. //vTaskDelay(50 / portTICK_PERIOD_MS);
  156. //for(int i=0; i<BALL_SEGMENTS; i++) {
  157. // printf("go to %f %f", x + 5*acos(i * 2.0*M_PI / BALL_SEGMENTS), y + 5*asin(i* 2.0*M_PI / BALL_SEGMENTS));
  158. // go_to(x + 5*cos(i * 2.0*M_PI / BALL_SEGMENTS), y + 5*sin(i* 2.0*M_PI / BALL_SEGMENTS));
  159. //}
  160. //go_to(x,y);
  161. laser_off();
  162. }
  163. void draw_frame()
  164. {
  165. draw_paddle(0);
  166. //vTaskDelay(1 / portTICK_PERIOD_MS);
  167. draw_ball();
  168. //vTaskDelay(1 / portTICK_PERIOD_MS);
  169. draw_paddle(1);
  170. draw_ball();
  171. }
  172. void draw_logo()
  173. {
  174. int l;
  175. int i=0,j;
  176. int p=0;
  177. float x,y;
  178. while( (l=logo_lines[i++]) > 0) {
  179. for(j=0;j<l;j++) {
  180. x = (uint8_t)(logo[p++] * SCREEN_WIDTH);
  181. y = (uint8_t)(logo[p++] * SCREEN_HEIGHT);
  182. if(x >= SCREEN_WIDTH) x = SCREEN_WIDTH -1;
  183. if(x<0) x = 0;
  184. if(y>=SCREEN_HEIGHT) y = SCREEN_HEIGHT-1;
  185. if(y<0) y = 0;
  186. go_to(x,y);
  187. if(j==0) laser_on();
  188. }
  189. laser_off();
  190. }
  191. }
  192. void app_main()
  193. {
  194. printf("Hello world!\n");
  195. precompute_values();
  196. printf("Precompute done.");
  197. gpio_config_t gpio_config_laser;
  198. ledc_channel_config_t ledc_conf_leftright;
  199. ledc_channel_config_t ledc_conf_updown;
  200. /*
  201. * configure the laser gpio as simple output
  202. */
  203. gpio_config_laser.pin_bit_mask = GPIO_LASER_SEL;
  204. gpio_config_laser.mode = GPIO_MODE_OUTPUT;
  205. gpio_config_laser.pull_up_en = GPIO_PULLUP_DISABLE;
  206. gpio_config_laser.pull_down_en = GPIO_PULLDOWN_DISABLE;
  207. gpio_config_laser.intr_type = GPIO_INTR_DISABLE;
  208. gpio_config(&gpio_config_laser);
  209. gpio_set_level(GPIO_NUM_4, 0);
  210. /*
  211. * According to the data sheet, the SG90 9g micro servos
  212. * operate at 0.1s / 60deg
  213. * and require a pwm signal at 50Hz
  214. * with duty length of "about" 1.0 - 1.5 - 2.0 ms
  215. * for all the way left, center, all the way right
  216. */
  217. ledc_timer_config_t timer_conf;
  218. timer_conf.duty_resolution = LEDC_TIMER_15_BIT;
  219. timer_conf.freq_hz = 50;
  220. timer_conf.speed_mode = LEDC_HIGH_SPEED_MODE;
  221. timer_conf.timer_num = LEDC_TIMER_0;
  222. ledc_timer_config(&timer_conf);
  223. // 2nd servo up/down
  224. // 2550 - straight
  225. // 1280 - down
  226. ledc_conf_updown.channel = LEDC_CHANNEL_0;
  227. ledc_conf_updown.duty = UPDOWN_STRAIGHT; //900; //3276; //1638; // to 3276 // 0 - 2088 (actually a bit less than that)
  228. ledc_conf_updown.gpio_num = GPIO_UPDOWN;
  229. ledc_conf_updown.intr_type = LEDC_INTR_DISABLE;
  230. ledc_conf_updown.speed_mode = LEDC_HIGH_SPEED_MODE;
  231. ledc_conf_updown.timer_sel = LEDC_TIMER_0;
  232. ledc_channel_config(&ledc_conf_updown);
  233. // first servo connected to ground
  234. // left-right
  235. // 3276 - 2000 - 900
  236. ledc_conf_leftright.channel = LEDC_CHANNEL_1;
  237. ledc_conf_leftright.duty = LEFTRIGHT_MIDDLE; //900; //3276; //1638; // to 3276 // 0 - 2088 (actually a bit less than that)
  238. ledc_conf_leftright.gpio_num = GPIO_LEFTRIGHT;
  239. ledc_conf_leftright.intr_type = LEDC_INTR_DISABLE;
  240. ledc_conf_leftright.speed_mode = LEDC_HIGH_SPEED_MODE;
  241. ledc_conf_leftright.timer_sel = LEDC_TIMER_0;
  242. ledc_channel_config(&ledc_conf_leftright);
  243. while(1) {
  244. //draw_frame();
  245. draw_logo();
  246. //vTaskDelay(1 / portTICK_PERIOD_MS);
  247. }
  248. go_to(0,0);
  249. vTaskDelay(5000 / portTICK_PERIOD_MS);
  250. laser_on();
  251. go_to(50,0);
  252. vTaskDelay(5000 / portTICK_PERIOD_MS);
  253. //laser_off();
  254. go_to(99,0);
  255. vTaskDelay(5000 / portTICK_PERIOD_MS);
  256. laser_on();
  257. go_to(99,25);
  258. vTaskDelay(5000 / portTICK_PERIOD_MS);
  259. //laser_off();
  260. go_to(50,25);
  261. vTaskDelay(5000 / portTICK_PERIOD_MS);
  262. laser_on();
  263. go_to(0,25);
  264. vTaskDelay(5000 / portTICK_PERIOD_MS);
  265. draw_line();
  266. laser_off();
  267. //laser_off();
  268. /* Print chip information */
  269. esp_chip_info_t chip_info;
  270. esp_chip_info(&chip_info);
  271. printf("This is ESP32 chip with %d CPU cores, WiFi%s%s, ",
  272. chip_info.cores,
  273. (chip_info.features & CHIP_FEATURE_BT) ? "/BT" : "",
  274. (chip_info.features & CHIP_FEATURE_BLE) ? "/BLE" : "");
  275. printf("silicon revision %d, ", chip_info.revision);
  276. printf("%dMB %s flash\n", spi_flash_get_chip_size() / (1024 * 1024),
  277. (chip_info.features & CHIP_FEATURE_EMB_FLASH) ? "embedded" : "external");
  278. for (int i = 0; i < 1000; i++) {
  279. printf("Restarting in %d seconds...\n", i);
  280. vTaskDelay(1000 / portTICK_PERIOD_MS);
  281. //ledc_conf2.duty = LEFTRIGHT_LEFT - (i+1) * (LEFTRIGHT_LEFT - LEFTRIGHT_MIDDLE) / 6;
  282. //ledc_channel_config(&ledc_conf2);
  283. //if(ledc_conf.duty > 1000) {
  284. // ledc_conf.duty = 800;
  285. //} else {
  286. // ledc_conf.duty = 3276;
  287. //}
  288. //ledc_channel_config(&ledc_conf);
  289. }
  290. printf("Restarting now.\n");
  291. fflush(stdout);
  292. esp_restart();
  293. }