1. /************************* (C) COPYRIGHT 2010 ROBOTIS **************************
2. * File Name          : main.c
3. * Author             : danceww
4. * Version            : V0.0.1
5. * Date               : 08/23/2010
6. * Description        : Main program body
7. *******************************************************************************/
8.  
9. /* Includes ------------------------------------------------------------------*/
10.  
11. //#include "includes.h"
12. #include "stm32f10x_lib.h"
13. #include "dynamixel.h"
14. #include "dxl_hal.h"
15.  
16. /* Private typedef -----------------------------------------------------------*/
17. /* Private define ------------------------------------------------------------*/
18. #define P_GOAL_POSITION_L               30
19. #define P_GOAL_POSITION_H               31
20. #define P_PRESENT_POSITION_L    36
21. #define P_PRESENT_POSITION_H    37
22. #define P_MOVING                                46
23. #define TEMP                                    43
24. #define CW                                              6
25. #define CCW                                             8
26. #define V                                               32
27.  
28. #define PORT_ENABLE_TXD                 GPIOB
29. #define PORT_ENABLE_RXD                 GPIOB
30. #define PORT_DXL_TXD                    GPIOB
31. #define PORT_DXL_RXD                    GPIOB
32.  
33.  
34. #define PIN_ENABLE_TXD                  GPIO_Pin_4
35. #define PIN_ENABLE_RXD                  GPIO_Pin_5
36. #define PIN_DXL_TXD                             GPIO_Pin_6
37. #define PIN_DXL_RXD                             GPIO_Pin_7
38. #define PIN_PC_TXD                              GPIO_Pin_10
39. #define PIN_PC_RXD              GPIO_Pin_11
40.  
41. #define USART_DXL                           0
42. #define USART_PC                            2
43.  
44. #define word                    u16
45. #define byte                    u8
46.  
47. /* Private macro -------------------------------------------------------------*/
48. /* Private variables ---------------------------------------------------------*/
49. volatile byte                   gbpRxInterruptBuffer[256]; // dxl buffer
50. volatile byte                   gbRxBufferWritePointer,gbRxBufferReadPointer;
51. volatile vu32                   gwTimingDelay,gw1msCounter;
52. u32                             Baudrate_DXL =  1000000;
53. u32                             Baudrate_PC = 57600;
54. vu16                            CCR1_Val = 100;                 // 1ms
55. vu32                            capture = 0;
56. word                            GoalPos[2] = {0, 102};
57. //word                            GoalPos[2] = {0, 1023};  //For EX and MX series
58. word                            Position;
59. word                            wPresentPos;
60. byte                            INDEX = 0;
61. byte                            Voltage;
62. byte                            id = 1;
63. byte                                                    id2 = 2;
64. byte                            bMoving, CommStatus;
65.  
66.  
67.  
68. /* Private function prototypes -----------------------------------------------*/
69. /* Private functions ---------------------------------------------------------*/
70. void RCC_Configuration(void);
71. void NVIC_Configuration(void);
72. void GPIO_Configuration(void);
73. void SysTick_Configuration(void);
74. void Timer_configuration(void);
75. void TimerInterrupt_1ms(void);
76. void RxD0Interrupt(void);
77. void __ISR_DELAY(void);
78. void USART1_Configuration(u32);
79. void USART_Configuration(u8, u32);
80. void DisableUSART1(void);
81. void ClearBuffer256(void);
82. byte CheckNewArrive(void);
83. void PrintCommStatus(int);
84. void PrintErrorCode(void);
85. void TxDByte_DXL(byte);
86. byte RxDByte_DXL(void);
87. void TxDString(byte*);
88. void TxDWord16(word);
89. void TxDByte16(byte);
90. void TxDByte_PC(byte);
91. void Timer_Configuration(void);
92. void mDelay(u32);
93. void StartDiscount(s32);
94. byte CheckTimeOut(void);
95.  
96. /*******************************************************************************
97. * Function Name  : main
98. * Description    : Main program
99. * Input          : None
100. * Output         : None
101. * Return         : None
102. *******************************************************************************/
103.  
104. int main(void)
105. {
106.     /* System Clocks Configuration */
107.         RCC_Configuration();
108.  
109.         /* NVIC configuration */
110.         NVIC_Configuration();
111.  
112.         /* GPIO configuration */
113.         GPIO_Configuration();
114.  
115.         SysTick_Configuration();
116.  
117.         Timer_Configuration();
118.  
119.  
120.         dxl_initialize( 0, 1 );
121.         USART_Configuration(USART_PC, Baudrate_PC);
122.  
123.  
124.         dxl_write_word( id, CW, 0 );
125.         dxl_write_word( id, CCW, 0 );
126.         dxl_write_word( id2, CW, 0 );
127.         dxl_write_word( id2, CCW, 0 );
128.  
129.         dxl_write_word( id, V, 500 );
130.         dxl_write_word( id2, V, 500 );
131.  
132. /*
133.         while(1)
134.         {
135.                 bMoving = dxl_read_byte( id, P_MOVING );
136.                 bMoving = dxl_read_byte( id2, P_MOVING );
137.                 CommStatus = dxl_get_result();
138.                 if( CommStatus == COMM_RXSUCCESS )
139.                 {
140.                         if( bMoving == 0 )
141.                         {
142.                                 // Change goal position
143.                                 if( INDEX == 0 )
144.                                         INDEX = 1;
145.                                 else
146.                                         INDEX = 0;
147.  
148.                                 // Write goal position
149.  
150.                                 dxl_write_word( id2, P_GOAL_POSITION_L, GoalPos[INDEX] );
151.                         }
152.  
153.                         PrintErrorCode();
154.  
155.                         // Read present position
156.                         wPresentPos = dxl_read_word( id, P_PRESENT_POSITION_L );
157.                         wPresentPos = dxl_read_word( id2, P_PRESENT_POSITION_L );
158.                         TxDWord16(GoalPos[INDEX]);
159.                         TxDString("   ");
160.                         TxDWord16(wPresentPos);
161.                         TxDByte_PC('\r');
162.                         TxDByte_PC('\n');
163.                         TxDString("    temp1 wynosi : ");
164.                         TxDWord16(TEMP);
165.                 }
166.                 else
167.                         PrintCommStatus(CommStatus);
168.  
169.         }
170.         */
171.         return 0;
172. }
173.  
174. /*******************************************************************************
175. * Function Name  : RCC_Configuration
176. * Description    : Configures the different system clocks.
177. * Input          : None
178. * Output         : None
179. * Return         : None
180. *******************************************************************************/
181. void RCC_Configuration(void)
182. {
183.         ErrorStatus HSEStartUpStatus;
184.         /* RCC system reset(for debug purpose) */
185.         RCC_DeInit();
186.  
187.         /* Enable HSE */
188.         RCC_HSEConfig(RCC_HSE_ON);
189.  
190.         /* Wait till HSE is ready */
191.         HSEStartUpStatus = RCC_WaitForHSEStartUp();
192.  
193.         if(HSEStartUpStatus == SUCCESS)
194.         {
195.                 /* Enable Prefetch Buffer */
196.                 FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);
197.  
198.                 /* Flash 2 wait state */
199.                 FLASH_SetLatency(FLASH_Latency_2);
200.  
201.                 /* HCLK = SYSCLK */
202.                 RCC_HCLKConfig(RCC_SYSCLK_Div1);
203.  
204.                 /* PCLK2 = HCLK */
205.                 RCC_PCLK2Config(RCC_HCLK_Div1);
206.  
207.                 /* PCLK1 = HCLK/2 */
208.                 RCC_PCLK1Config(RCC_HCLK_Div2);
209.  
210.                 /* PLLCLK = 8MHz * 9 = 72 MHz */
211.                 RCC_PLLConfig(RCC_PLLSource_HSE_Div1, RCC_PLLMul_9);
212.  
213.                 /* Enable PLL */
214.                 RCC_PLLCmd(ENABLE);
215.  
216.                 /* Wait till PLL is ready */
217.                 while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY) == RESET)
218.                 {
219.                 }
220.  
221.                 /* Select PLL as system clock source */
222.                 RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);
223.  
224.                 /* Wait till PLL is used as system clock source */
225.                 while(RCC_GetSYSCLKSource() != 0x08)
226.                 {
227.                 }
228.         }
229.  
230.         /* Enable peripheral clocks --------------------------------------------------*/
231.  
232.         /* Enable USART1 and GPIOB clocks */
233.         RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1 | RCC_APB2Periph_GPIOB, ENABLE);
234.  
235.         /* Enable USART3 clocks */
236.         RCC_APB1PeriphClockCmd ( RCC_APB1Periph_USART3 | RCC_APB1Periph_TIM2, ENABLE);
237.  
238.         PWR_BackupAccessCmd(ENABLE);
239. }
240.  
241. /*******************************************************************************
242. * Function Name  : NVIC_Configuration
243. * Description    : Configures Vector Table base location.
244. * Input          : None
245. * Output         : None
246. * Return         : None
247. *******************************************************************************/
248. void NVIC_Configuration(void)
249. {
250.         NVIC_InitTypeDef NVIC_InitStructure;
251.  
252.         #ifdef  VECT_TAB_RAM
253.                 // Set the Vector Table base location at 0x20000000
254.                 NVIC_SetVectorTable(NVIC_VectTab_RAM, 0x0);
255.         #else  // VECT_TAB_FLASH
256.                 // Set the Vector Table base location at 0x08003000
257.                 NVIC_SetVectorTable(NVIC_VectTab_FLASH, 0x3000);
258.         #endif
259.  
260.         // Configure the NVIC Preemption Priority Bits
261.         NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
262.  
263.         // Enable the USART1 Interrupt
264.         NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQChannel;
265.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
266.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
267.         NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
268.         NVIC_Init(&NVIC_InitStructure);
269.  
270.         // Enable the TIM2 Interrupt
271.         NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQChannel;
272.         NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
273.         NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
274.         NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
275.         NVIC_Init(&NVIC_InitStructure);
276. }
277.  
278. /*******************************************************************************
279. * Function Name  : GPIO_Configuration
280. * Description    : Configures the different GPIO ports.
281. * Input          : None
282. * Output         : None
283. * Return         : None
284. *******************************************************************************/
285. void GPIO_Configuration(void)
286. {
287.         GPIO_InitTypeDef GPIO_InitStructure;
288.         GPIO_StructInit(&GPIO_InitStructure);
289.  
290.         // PORTB CONFIG
291.         GPIO_InitStructure.GPIO_Pin =   PIN_ENABLE_TXD | PIN_ENABLE_RXD;
292.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
293.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
294.         GPIO_Init(GPIOB, &GPIO_InitStructure);
295.  
296.         GPIO_InitStructure.GPIO_Pin = PIN_DXL_RXD | PIN_PC_RXD;
297.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
298.         GPIO_Init(GPIOB, &GPIO_InitStructure);
299.  
300.         GPIO_InitStructure.GPIO_Pin = PIN_DXL_TXD | PIN_PC_TXD;
301.         GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
302.         GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;
303.         GPIO_Init(GPIOB, &GPIO_InitStructure);
304.  
305.         GPIO_PinRemapConfig( GPIO_Remap_USART1, ENABLE);
306.         GPIO_PinRemapConfig( GPIO_Remap_SWJ_Disable, ENABLE);
307.  
308.         GPIO_ResetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);        // TX Disable
309.         GPIO_SetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);  // RX Enable
310. }
311.  
312. void USART1_Configuration(u32 baudrate)
313. {
314.         USART_Configuration(USART_DXL, baudrate);
315. }
316.  
317. void USART_Configuration(u8 PORT, u32 baudrate)
318. {
319.  
320.         USART_InitTypeDef USART_InitStructure;
321.  
322.         USART_StructInit(&USART_InitStructure);
323.  
324.  
325.         USART_InitStructure.USART_BaudRate = baudrate;
326.         USART_InitStructure.USART_WordLength = USART_WordLength_8b;
327.         USART_InitStructure.USART_StopBits = USART_StopBits_1;
328.         USART_InitStructure.USART_Parity = USART_Parity_No ;
329.         USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
330.         USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
331.  
332.  
333.         if( PORT == USART_DXL )
334.         {
335.                 USART_DeInit(USART1);
336.                 mDelay(10);
337.                 /* Configure the USART1 */
338.                 USART_Init(USART1, &USART_InitStructure);
339.  
340.                 /* Enable USART1 Receive and Transmit interrupts */
341.                 USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
342.                 //USART_ITConfig(USART1, USART_IT_TC, ENABLE);
343.  
344.                 /* Enable the USART1 */
345.                 USART_Cmd(USART1, ENABLE);
346.         }
347.  
348.         else if( PORT == USART_PC )
349.         {
350.                 USART_DeInit(USART3);
351.                 mDelay(10);
352.                 /* Configure the USART3 */
353.                 USART_Init(USART3, &USART_InitStructure);
354.  
355.                 /* Enable USART3 Receive and Transmit interrupts */
356.                 //USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);
357.                 //USART_ITConfig(USART3, USART_IT_TC, ENABLE);
358.  
359.                 /* Enable the USART3 */
360.                 USART_Cmd(USART3, ENABLE);
361.         }
362. }
363.  
364. void DisableUSART1(void)
365. {
366.         USART_Cmd(USART1, DISABLE);
367. }
368.  
369. void ClearBuffer256(void)
370. {
371.         gbRxBufferReadPointer = gbRxBufferWritePointer = 0;
372. }
373.  
374. byte CheckNewArrive(void)
375. {
376.         if(gbRxBufferReadPointer != gbRxBufferWritePointer)
377.                 return 1;
378.         else
379.                 return 0;
380. }
381.  
382. void TxDByte_DXL(byte bTxdData)
383. {
384.         GPIO_ResetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);        // RX Disable
385.         GPIO_SetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);  // TX Enable
386.  
387.         USART_SendData(USART1,bTxdData);
388.         while( USART_GetFlagStatus(USART1, USART_FLAG_TC)==RESET );
389.  
390.         GPIO_ResetBits(PORT_ENABLE_TXD, PIN_ENABLE_TXD);        // TX Disable
391.         GPIO_SetBits(PORT_ENABLE_RXD, PIN_ENABLE_RXD);  // RX Enable
392. }
393.  
394. byte RxDByte_DXL(void)
395. {
396.         byte bTemp;
397.  
398.         while(1)
399.         {
400.                 if(gbRxBufferReadPointer != gbRxBufferWritePointer) break;
401.         }
402.  
403.         bTemp = gbpRxInterruptBuffer[gbRxBufferReadPointer];
404.         gbRxBufferReadPointer++;
405.  
406.         return bTemp;
407. }
408.  
409.  
410. // Print communication result
411. void PrintCommStatus(int CommStatus)
412. {
413.         switch(CommStatus)
414.         {
415.         case COMM_TXFAIL:
416.                 TxDString("COMM_TXFAIL: Failed transmit instruction packet!\n");
417.                 break;
418.  
419.         case COMM_TXERROR:
420.                 TxDString("COMM_TXERROR: Incorrect instruction packet!\n");
421.                 break;
422.  
423.         case COMM_RXFAIL:
424.                 TxDString("COMM_RXFAIL: Failed get status packet from device!\n");
425.                 break;
426.  
427.         case COMM_RXWAITING:
428.                 TxDString("COMM_RXWAITING: Now recieving status packet!\n");
429.                 break;
430.  
431.         case COMM_RXTIMEOUT:
432.                 TxDString("COMM_RXTIMEOUT: There is no status packet!\n");
433.                 break;
434.  
435.         case COMM_RXCORRUPT:
436.                 TxDString("COMM_RXCORRUPT: Incorrect status packet!\n");
437.                 break;
438.  
439.         default:
440.                 TxDString("This is unknown error code!\n");
441.                 break;
442.         }
443. }
444.  
445. // Print error bit of status packet
446. void PrintErrorCode()
447. {
448.         if(dxl_get_rxpacket_error(ERRBIT_VOLTAGE) == 1)
449.                 TxDString("Input voltage error!\n");
450.  
451.         if(dxl_get_rxpacket_error(ERRBIT_ANGLE) == 1)
452.                 TxDString("Angle limit error!\n");
453.  
454.         if(dxl_get_rxpacket_error(ERRBIT_OVERHEAT) == 1)
455.                 TxDString("Overheat error!\n");
456.  
457.         if(dxl_get_rxpacket_error(ERRBIT_RANGE) == 1)
458.                 TxDString("Out of range error!\n");
459.  
460.         if(dxl_get_rxpacket_error(ERRBIT_CHECKSUM) == 1)
461.                 TxDString("Checksum error!\n");
462.  
463.         if(dxl_get_rxpacket_error(ERRBIT_OVERLOAD) == 1)
464.                 TxDString("Overload error!\n");
465.  
466.         if(dxl_get_rxpacket_error(ERRBIT_INSTRUCTION) == 1)
467.                 TxDString("Instruction code error!\n");
468. }
469.  
470. void TxDString(byte *bData)
471. {
472.         while (*bData)
473.                 TxDByte_PC(*bData++);
474. }
475.  
476. void TxDWord16(word wSentData)
477. {
478.         TxDByte16((wSentData >> 8) & 0xff);
479.         TxDByte16(wSentData & 0xff);
480. }
481.  
482. void TxDByte16(byte bSentData)
483. {
484.         byte bTmp;
485.  
486.         bTmp = ((byte) (bSentData >> 4) & 0x0f) + (byte) '0';
487.         if (bTmp > '9')
488.                 bTmp += 7;
489.         TxDByte_PC(bTmp);
490.         bTmp = (byte) (bSentData & 0x0f) + (byte) '0';
491.         if (bTmp > '9')
492.                 bTmp += 7;
493.         TxDByte_PC(bTmp);
494. }
495.  
496. void TxDByte_PC(byte bTxdData)
497. {
498.         USART_SendData(USART3,bTxdData);
499.         while( USART_GetFlagStatus(USART3, USART_FLAG_TC)==RESET );
500. }
501.  
502. void Timer_Configuration(void)
503. {
504.         TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
505.         TIM_OCInitTypeDef  TIM_OCInitStructure;
506.  
507.         TIM_TimeBaseStructInit(&TIM_TimeBaseStructure);
508.         TIM_OCStructInit(&TIM_OCInitStructure);
509.  
510.         TIM_DeInit(TIM2);
511.  
512.         /* Time base configuration */
513.         TIM_TimeBaseStructure.TIM_Period = 65535;
514.         TIM_TimeBaseStructure.TIM_Prescaler = 0;
515.         TIM_TimeBaseStructure.TIM_ClockDivision = 0;
516.         TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
517.  
518.         TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
519.  
520.         /* Prescaler configuration */
521.         TIM_PrescalerConfig(TIM2, 722, TIM_PSCReloadMode_Immediate);
522.  
523.         /* Output Compare Timing Mode configuration: Channel1 */
524.         TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Timing;
525.         TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Disable;
526.         TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
527.         TIM_OCInitStructure.TIM_Pulse = CCR1_Val ;
528.  
529.         TIM_OC1Init(TIM2, &TIM_OCInitStructure);
530.         TIM_OC1PreloadConfig(TIM2, TIM_OCPreload_Disable);
531.  
532.         /* TIM IT enable */
533.         TIM_ITConfig(TIM2, TIM_IT_CC1, ENABLE);
534.  
535.         /* TIM2 enable counter */
536.         TIM_Cmd(TIM2, ENABLE);
537. }
538.  
539. void TimerInterrupt_1ms(void) //OLLO CONTROL
540. {
541.         if (TIM_GetITStatus(TIM2, TIM_IT_CC1) != RESET) // 1ms//
542.         {
543.                 TIM_ClearITPendingBit(TIM2, TIM_IT_CC1);
544.  
545.                 capture = TIM_GetCapture1(TIM2);
546.                 TIM_SetCompare1(TIM2, capture + CCR1_Val);
547.  
548.                 if(gw1msCounter > 0)
549.                         gw1msCounter--;
550.         }
551. }
552.  
553. /*__interrupt*/
554. void RxD0Interrupt(void)
555. {
556.         if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)
557.                 gbpRxInterruptBuffer[gbRxBufferWritePointer++] = USART_ReceiveData(USART1);
558. }
559.  
560. void SysTick_Configuration(void)
561. {
562.           /* SysTick end of count event each 1ms with input clock equal to 9MHz (HCLK/8, default) */
563.           SysTick_SetReload(9000);
564.  
565.           /* Enable SysTick interrupt */
566.           SysTick_ITConfig(ENABLE);
567. }
568.  
569. void __ISR_DELAY(void)
570. {
571.         if (gwTimingDelay != 0x00)
572.                 gwTimingDelay--;
573. }
574.  
575. void mDelay(u32 nTime)
576. {
577.         /* Enable the SysTick Counter */
578.         SysTick_CounterCmd(SysTick_Counter_Enable);
579.  
580.         gwTimingDelay = nTime;
581.  
582.         while(gwTimingDelay != 0);
583.  
584.         /* Disable SysTick Counter */
585.         SysTick_CounterCmd(SysTick_Counter_Disable);
586.         /* Clear SysTick Counter */
587.         SysTick_CounterCmd(SysTick_Counter_Clear);
588. }
589.  
590. void StartDiscount(s32 StartTime)
591. {
592.         gw1msCounter = StartTime;
593. }
594.  
595. u8 CheckTimeOut(void)
596. {
597.         // Check timeout
598.         // Return: 0 is false, 1 is true(timeout occurred)
599.  
600.         if(gw1msCounter == 0)
601.                 return 1;
602.         else
603.                 return 0;
604. }
605.  
606.