Viel Spaß mit der MAYA!

SUBMITTED BY: bitcoinsachen

DATE: April 26, 2017, 5 p.m.

FORMAT: C

SIZE: 11.6 kB

Raw Download

HITS: 341

Report
  1. #include "stm32f30x_conf.h"
  2. #include <stdlib.h>
  3. #include <stdio.h>
  4. #include "thetable.c"
  6. #define PI 3.14159265358979323846
  7. #define BUFFERSIZE 64
  8. #define UNCERT 20
  9. #define PERIOD 20
  11. // IN DIESEM PROJEKT GILT:
  12. // PROZESSORTAKT = 72 MHz
  13. // AHB Prescaler = 1
  14. // APB1 Prescaler = 2
  15. // APB2 Prescaler = 1
  17. volatile char buffer[BUFFERSIZE];
  18. volatile int bufferread = 0;
  20. volatile int vertstate = 0;
  21. volatile int horistate = 0;
  23. volatile int pos = 0;
  24. volatile char directions = 0;
  26. volatile char photo [4];
  28. volatile double rps = 0;
  30. void vertstep(int direction);
  31. void horistep(int direction);
  33. int main(void){
  34. // ---------------------------------------------------------------------
  35. // Enabling clocks
  36. RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA, ENABLE);
  37. RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOB, ENABLE);
  38. RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOC, ENABLE);
  39. RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2,ENABLE);
  40. RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1,ENABLE);
  41. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
  42. RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
  43. RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE);
  46. // ---------------------------------------------------------------------
  47. // Init GPIO (A) for USART1 (Maya serial) and USART2 (PC serial)
  48. GPIO_InitTypeDef GPIO_Init_Struct;
  49. GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_AF;
  50. GPIO_Init_Struct.GPIO_OType = GPIO_OType_PP;
  51. GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_2 | GPIO_Pin_3;
  52. GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_NOPULL;
  53. GPIO_Init_Struct.GPIO_Speed = GPIO_Speed_50MHz;
  55. GPIO_Init(GPIOA, &GPIO_Init_Struct);
  56. GPIO_PinAFConfig(GPIOA,GPIO_PinSource9,GPIO_AF_7);
  57. GPIO_PinAFConfig(GPIOA,GPIO_PinSource10,GPIO_AF_7);
  58. GPIO_PinAFConfig(GPIOA,GPIO_PinSource2,GPIO_AF_7);
  59. GPIO_PinAFConfig(GPIOA,GPIO_PinSource3,GPIO_AF_7);
  60. // ---------------------------------------------------------------------
  61. // Init GPIO (A) for debugging with LED2 and extra PA6 for osci
  63. // GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_OUT;
  64. // GPIO_Init_Struct.GPIO_OType = GPIO_OType_PP;
  65. // GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6;
  66. // GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_NOPULL;
  67. // GPIO_Init_Struct.GPIO_Speed = GPIO_Speed_50MHz;
  68. //
  69. // GPIO_Init(GPIOA,&GPIO_Init_Struct);
  71. // ---------------------------------------------------------------------
  72. // Init GPIO (B) for Vertical Stepper
  73. GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_OUT;
  74. GPIO_Init_Struct.GPIO_OType = GPIO_OType_PP;
  75. GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7 | GPIO_Pin_8 | GPIO_Pin_9;
  76. GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_NOPULL;
  77. GPIO_Init_Struct.GPIO_Speed = GPIO_Speed_50MHz;
  79. GPIO_Init(GPIOB, &GPIO_Init_Struct);
  80. GPIOB->ODR &= 0xF<<6;
  82. // ---------------------------------------------------------------------
  83. // Init GPIO (B) for Lichtschranken
  84. GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_IN;
  85. GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_3 | GPIO_Pin_4;
  86. GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_DOWN;
  88. GPIO_Init(GPIOB, &GPIO_Init_Struct);
  90. // ---------------------------------------------------------------------
  91. // Init GPIO (C) for Horizontal Stepper
  92. GPIO_Init_Struct.GPIO_Mode = GPIO_Mode_OUT;
  93. GPIO_Init_Struct.GPIO_OType = GPIO_OType_PP;
  94. GPIO_Init_Struct.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10 | GPIO_Pin_11 | GPIO_Pin_12;
  95. GPIO_Init_Struct.GPIO_PuPd = GPIO_PuPd_NOPULL;
  96. GPIO_Init_Struct.GPIO_Speed = GPIO_Speed_50MHz;
  98. GPIO_Init(GPIOC, &GPIO_Init_Struct);
  99. GPIOC->ODR = 0;
  101. // ---------------------------------------------------------------------
  102. // Init TIM2 for Stepping every PERIOD ms
  103. TIM_TimeBaseInitTypeDef TIM_Init_Struct;
  104. TIM_Init_Struct.TIM_Prescaler = 7200 - 1; // 100 mcs per tick (0.1ms)
  105. TIM_Init_Struct.TIM_Period = PERIOD - 1;
  107. TIM_TimeBaseInit(TIM2, &TIM_Init_Struct);
  108. TIM_ITConfig(TIM2, TIM_IT_Update, ENABLE); //IT enable
  109. TIM_Cmd(TIM2,ENABLE);
  110. // ---------------------------------------------------------------------
  111. // Init TIM3 for rps calculation
  112. TIM_Init_Struct.TIM_Prescaler = 7200 - 1; // 100 mcs per tick (0.1ms)
  113. TIM_Init_Struct.TIM_Period = 0xFFFF;
  115. TIM_TimeBaseInit(TIM3, &TIM_Init_Struct);
  116. TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE);
  117. TIM_Cmd(TIM3,ENABLE);
  118. // ---------------------------------------------------------------------
  119. // Init USART1 (Maya serial)
  120. USART_InitTypeDef USART_Init_Struct;
  121. USART_Init_Struct.USART_BaudRate = 115200;
  122. USART_Init_Struct.USART_Parity = USART_Parity_No;
  123. USART_Init_Struct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  124. USART_Init_Struct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  125. USART_Init_Struct.USART_StopBits = USART_StopBits_1;
  126. USART_Init_Struct.USART_WordLength = USART_WordLength_8b;
  128. USART_Init(USART1,&USART_Init_Struct);
  129. USART_ITConfig(USART1, USART_IT_RXNE, ENABLE); // RXNE = ReceiveDataRegister Not Empty
  130. USART_ITConfig(USART1, USART_IT_ORE, DISABLE);
  131. USART_Cmd(USART1,ENABLE);
  132. // ---------------------------------------------------------------------
  133. // Init USART2 (PC serial)
  134. USART_Init_Struct.USART_BaudRate = 115200;
  135. USART_Init_Struct.USART_Parity = USART_Parity_No;
  136. USART_Init_Struct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  137. USART_Init_Struct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  138. USART_Init_Struct.USART_StopBits = USART_StopBits_1;
  139. USART_Init_Struct.USART_WordLength = USART_WordLength_8b;
  141. USART_Init(USART2,&USART_Init_Struct);
  142. USART_ITConfig(USART2, USART_IT_TC, DISABLE);
  143. USART_Cmd(USART2,ENABLE);
  145. // ---------------------------------------------------------------------
  146. // Init EXTI for Lichtschranke MAYA
  147. SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOB, EXTI_PinSource4);
  149. EXTI_InitTypeDef EXTI_Init_Struct;
  150. EXTI_Init_Struct.EXTI_Line = EXTI_Line4;
  151. EXTI_Init_Struct.EXTI_LineCmd = ENABLE;
  152. EXTI_Init_Struct.EXTI_Mode = EXTI_Mode_Interrupt;
  153. EXTI_Init_Struct.EXTI_Trigger = EXTI_Trigger_Rising_Falling;
  155. EXTI_Init(&EXTI_Init_Struct);
  157. // ---------------------------------------------------------------------
  158. // NVIC
  159. NVIC_InitTypeDef NVIC_Init_Struct;
  160. // USART2 (PC serial)
  161. NVIC_Init_Struct.NVIC_IRQChannel = USART2_IRQn;
  162. NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
  163. NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 4;
  164. NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 4;
  165. NVIC_Init(&NVIC_Init_Struct);
  166. // EXTI LICHTSCHRANKE
  167. NVIC_Init_Struct.NVIC_IRQChannel = EXTI4_IRQn;
  168. NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
  169. NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 3;
  170. NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 3;
  171. NVIC_Init(&NVIC_Init_Struct);
  172. // TIM3 (rps measurement)
  173. NVIC_Init_Struct.NVIC_IRQChannel = TIM3_IRQn;
  174. NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
  175. NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 2;
  176. NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 2;
  177. NVIC_Init(&NVIC_Init_Struct);
  178. // TIM2 (Stepping every PERIOD ms)
  179. NVIC_Init_Struct.NVIC_IRQChannel = TIM2_IRQn;
  180. NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
  181. NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 1;
  182. NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 1;
  183. NVIC_Init(&NVIC_Init_Struct);
  184. // USART1 (Maya serial)
  185. NVIC_Init_Struct.NVIC_IRQChannel = USART1_IRQn;
  186. NVIC_Init_Struct.NVIC_IRQChannelCmd = ENABLE;
  187. NVIC_Init_Struct.NVIC_IRQChannelPreemptionPriority = 0;
  188. NVIC_Init_Struct.NVIC_IRQChannelSubPriority = 0;
  189. NVIC_Init(&NVIC_Init_Struct);
  191. // ---------------------------------------------------------------------
  192. // Wrapup
  193. // GPIO_ResetBits(GPIOA, GPIO_Pin_5);
  194. // GPIO_ResetBits(GPIOA, GPIO_Pin_6);
  197. // ---------------------------------------------------------------------
  198. /* Infinite loop */
  199. while (1);
  200. }
  202. void USART1_IRQHandler(void) {
  203. // GPIO_SetBits(GPIOA, GPIO_Pin_6);
  204. char recvd1 = USART_ReceiveData(USART1);
  205. photo[(int)recvd1>>0x6] = recvd1 & 0x3F; // recvd>>0x6 = 0b 0000 00xx // 0x3F = 0b 0011 1111
  207. USART_ClearFlag(USART1, USART_FLAG_ORE);
  208. // GPIO_ResetBits(GPIOA, GPIO_Pin_6);
  209. }
  211. void USART2_IRQHandler(void) {
  212. if (buffer[bufferread] == '\r') {
  213. bufferread = 0;
  214. USART_SendData(USART2, '\r');
  215. USART_ITConfig(USART2, USART_IT_TC, DISABLE);
  216. }else {
  217. USART_SendData(USART2, buffer[bufferread++]);
  218. }
  219. }
  221. void EXTI4_IRQHandler(void){
  222. //GPIO_SetBits(GPIOA, GPIO_Pin_5);
  223. int ls1 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_3);
  224. int ls2 = GPIO_ReadInputDataBit(GPIOB,GPIO_Pin_4);
  226. directions = directions<<1;
  228. if (ls1 == ls2) {
  229. pos++;
  230. directions |= 1;
  231. }
  232. else
  233. pos--;
  235. char last3 = directions & 7; // 7 = 0b0111
  236. if ((last3 == 5) || (last3 == 2)) { // 5 = 0b0101 // 2 = 0b0010 // if NULLPOSITION
  238. long temp = TIM_GetCounter(TIM3);
  239. if (temp != 0){
  240. rps = (10000) / (double)temp;
  241. TIM_SetCounter(TIM3, 0);
  242. sprintf(buffer, "%lf rps \r", rps);
  243. USART_ITConfig(USART2, USART_IT_TC, ENABLE);
  244. }else {
  245. TIM_Cmd(TIM3, ENABLE);
  246. rps = 0;
  247. sprintf(buffer, "%lf rps \r", rps);
  248. USART_ITConfig(USART2, USART_IT_TC, ENABLE);
  249. }
  251. if (pos >= 0)
  252. pos = 0;
  253. else
  254. pos = 229;
  255. }
  257. //GPIO_ResetBits(GPIOA, GPIO_Pin_5);
  258. EXTI_ClearITPendingBit(EXTI_Line4);
  259. }
  261. void TIM3_IRQHandler(void) {
  262. TIM_Cmd(TIM3, DISABLE);
  263. TIM_SetCounter(TIM3, 0);
  264. TIM_ClearITPendingBit(TIM3, TIM_IT_Update);
  265. }
  267. void TIM2_IRQHandler(void) {
  268. // GPIO_SetBits(GPIOA, GPIO_Pin_5);
  270. // int x = photo[0] + photo[1] - photo[2] - photo[3];
  271. // int y = photo[0] - photo[1] + photo[2] - photo[3];
  272. //
  273. // double abs = sqrt(x*x+y*y);
  274. // double angle = atan(y/x);
  275. // if (x < 0)
  276. // angle += PI;
  277. // double rot = -pos * 2 * PI / 230;
  278. //
  279. // x = (int)(abs * cos(angle + rot));
  280. // y = (int)(abs * sin(angle + rot));
  282. double x1 = photo[0] + photo[1] - photo[2] - photo[3];
  283. double y1 = photo[0] - photo[1] + photo[2] - photo[3];
  285. double x = x1 * cos_lookup[pos] + y1 * sin_lookup[pos];
  286. double y = y1 * cos_lookup[pos] - x1 * sin_lookup[pos];
  288. //INVERSION IS INSIDE HERE AS WELL
  289. if (x > UNCERT)
  290. horistep(1);
  291. else if (x < -UNCERT)
  292. horistep(-1);
  294. if (y > UNCERT)
  295. vertstep(1);
  296. else if (y < -UNCERT)
  297. vertstep(-1);
  299. // GPIO_ResetBits(GPIOA, GPIO_Pin_5);
  300. TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
  301. }
  305. void vertstep(int direction){ // POSITIVE direction = DOWN
  306. vertstate += direction;
  308. if(vertstate > 7)
  309. vertstate = 0;
  310. if(vertstate < 0)
  311. vertstate = 7;
  313. switch(vertstate){
  314. case 0:
  315. GPIOB->ODR = 1<<9;
  316. break;
  317. case 1:
  318. GPIOB->ODR = 1<<9 | 1<<8;
  319. break;
  320. case 2:
  321. GPIOB->ODR = 1<<8;
  322. break;
  323. case 3:
  324. GPIOB->ODR = 1<<8 | 1<<7;
  325. break;
  326. case 4:
  327. GPIOB->ODR = 1<<7;
  328. break;
  329. case 5:
  330. GPIOB->ODR = 1<<7 | 1<<6;
  331. break;
  332. case 6:
  333. GPIOB->ODR = 1<<6;
  334. break;
  335. case 7:
  336. GPIOB->ODR = 1<<6 | 1<<9;
  337. break;
  338. default:
  339. GPIOB->ODR &= 0xF<<6;
  340. break;
  341. }
  342. }
  345. void horistep(int direction) {// POSITIVE direction = LEFT
  346. horistate += direction;
  348. if(horistate > 7)
  349. horistate = 0;
  350. if(horistate < 0)
  351. horistate = 7;
  353. switch(horistate){
  354. case 0:
  355. GPIOC->ODR = 1<<12;
  356. break;
  357. case 1:
  358. GPIOC->ODR = 1<<12 | 1<<11;
  359. break;
  360. case 2:
  361. GPIOC->ODR = 1<<11;
  362. break;
  363. case 3:
  364. GPIOC->ODR = 1<<11 | 1<<10;
  365. break;
  366. case 4:
  367. GPIOC->ODR = 1<<10;
  368. break;
  369. case 5:
  370. GPIOC->ODR = 1<<10 | 1<<9;
  371. break;
  372. case 6:
  373. GPIOC->ODR = 1<<9;
  374. break;
  375. case 7:
  376. GPIOC->ODR = 1<<9 | 1<<12;
  377. break;
  378. default:
  379. GPIOC->ODR &= 0xF<<9;
  380. break;
  381. }
  382. }
comments powered by Disqus