Browse Source

Version 1.3 du soft robot

Sébastien DI MERCURIO 5 years ago
parent
commit
365843d786

+ 1
- 1
software/robot/.cproject View File

41
 							<tool id="com.atollic.truestudio.exe.debug.toolchain.gcc.507098916" name="C Compiler" superClass="com.atollic.truestudio.exe.debug.toolchain.gcc">
41
 							<tool id="com.atollic.truestudio.exe.debug.toolchain.gcc.507098916" name="C Compiler" superClass="com.atollic.truestudio.exe.debug.toolchain.gcc">
42
 								<option id="com.atollic.truestudio.gcc.symbols.defined.110632980" name="Defined symbols" superClass="com.atollic.truestudio.gcc.symbols.defined" useByScannerDiscovery="false" valueType="definedSymbols">
42
 								<option id="com.atollic.truestudio.gcc.symbols.defined.110632980" name="Defined symbols" superClass="com.atollic.truestudio.gcc.symbols.defined" useByScannerDiscovery="false" valueType="definedSymbols">
43
 									<listOptionValue builtIn="false" value="STM32F10X_LD"/>
43
 									<listOptionValue builtIn="false" value="STM32F10X_LD"/>
44
-									<listOptionValue builtIn="false" value="USE_STDPERIPH_DRIVER"/>
45
 									<listOptionValue builtIn="false" value="__NO_INACTIVITY_SHUTDOWN__"/>
44
 									<listOptionValue builtIn="false" value="__NO_INACTIVITY_SHUTDOWN__"/>
45
+									<listOptionValue builtIn="false" value="USE_STDPERIPH_DRIVER"/>
46
 								</option>
46
 								</option>
47
 								<option id="com.atollic.truestudio.gcc.directories.select.902131530" name="Include path" superClass="com.atollic.truestudio.gcc.directories.select" useByScannerDiscovery="false" valueType="includePath">
47
 								<option id="com.atollic.truestudio.gcc.directories.select.902131530" name="Include path" superClass="com.atollic.truestudio.gcc.directories.select" useByScannerDiscovery="false" valueType="includePath">
48
 									<listOptionValue builtIn="false" value="../src"/>
48
 									<listOptionValue builtIn="false" value="../src"/>

+ 42
- 0
software/robot/X-CTU_command_List.xml View File

1
+<?xml version="1.0" encoding="UTF-8"?>
2
+
3
+<data>
4
+  <loop>false</loop>
5
+  <repeat_times>1</repeat_times>
6
+  <repeat_period>500</repeat_period>
7
+  <packets_list>
8
+    <packet name="Move">
9
+      <payload>4D3D313030410D</payload>
10
+    </packet>
11
+    <packet name="Turn">
12
+      <payload>543D313030580D</payload>
13
+    </packet>
14
+    <packet name="Ping">
15
+      <payload>70700D</payload>
16
+    </packet>
17
+    <packet name="Start without watchdog">
18
+      <payload>75750D</payload>
19
+    </packet>
20
+    <packet name="GetVersion">
21
+      <payload>56560D</payload>
22
+    </packet>
23
+    <packet name="Reset">
24
+      <payload>72720D</payload>
25
+    </packet>
26
+    <packet name="Start With Wtachdog">
27
+      <payload>57570D</payload>
28
+    </packet>
29
+    <packet name="Reset Watchdog">
30
+      <payload>77770D</payload>
31
+    </packet>
32
+    <packet name="GetState">
33
+      <payload>62620D</payload>
34
+    </packet>
35
+    <packet name="LongMove">
36
+      <payload>4D3D31303030710D</payload>
37
+    </packet>
38
+    <packet name="GetBattery">
39
+      <payload>76760D</payload>
40
+    </packet>
41
+  </packets_list>
42
+</data>

+ 128
- 95
software/robot/src/battery.c View File

5
  * @version V1.0
5
  * @version V1.0
6
  * @date    16-mai-2016
6
  * @date    16-mai-2016
7
  * @brief   Supervision de la tension batterie et detection de charge.
7
  * @brief   Supervision de la tension batterie et detection de charge.
8
- *			Calcule le voltage de la batterie � interval r�gulier.
8
+ *			Calcule le voltage de la batterie à interval régulier.
9
  *			Converti le voltage batterie en signaux de commande - 2 -1 - 0.
9
  *			Converti le voltage batterie en signaux de commande - 2 -1 - 0.
10
- *			Configure une interruption externe pour dtecter le branchement
10
+ *			Configure une interruption externe pour détecter le branchement
11
  *			du chargeur.
11
  *			du chargeur.
12
  ******************************************************************************
12
  ******************************************************************************
13
  ******************************************************************************
13
  ******************************************************************************
14
  */
14
  */
15
-
16
-#include <battery.h>
17
-#include "system_dumby.h"
18
-#include "motor.h"
19
 #include <stm32f10x.h>
15
 #include <stm32f10x.h>
16
+#include "battery.h"
17
+#include "system_dumby.h"
20
 
18
 
21
-uint16_t PrescalerValue = 0;
22
-uint16_t PWM_BATTERY_ON = 0xC0;
23
-uint16_t PWM_BATTERY_OFF = 0;
24
-TIM_TimeBaseInitTypeDef TIM_BaseTempsTimer;
25
-TIM_OCInitTypeDef TIM_PWMConfigure;
26
-
27
-ADC_InitTypeDef ADC_InitStructure;
28
 DMA_InitTypeDef DMA_BAT_InitStructure;
19
 DMA_InitTypeDef DMA_BAT_InitStructure;
29
-__IO uint16_t ADCConvertedValue[VOLTAGE_BUFFER_SIZE];
20
+uint16_t ADCConvertedValue[VOLTAGE_BUFFER_SIZE];
21
+
22
+char cptMesureHigh=0;
23
+char cptMesureLow=0;
24
+char cptMesureDisable=0;
25
+
26
+uint16_t vbatLowerVal;
27
+uint16_t vbatHighVal;
28
+uint16_t vbatDiff;
29
+
30
+uint16_t testPostion=0;
31
+uint32_t mesureVoltage;
32
+uint32_t meanVoltage;
33
+
34
+uint32_t cptMesureEmergencyHalt=0;
30
 
35
 
31
 /** @addtogroup Projects
36
 /** @addtogroup Projects
32
  * @{
37
  * @{
42
  */
47
  */
43
 
48
 
44
 /**
49
 /**
45
- * @brief 		Definis les GPIO necessaires pour la batterie.
50
+ * @brief 		Défini les GPIO nécessaires pour la batterie.
46
  *
51
  *
47
  * 				La fonction MAP_MotorPin va venir configurer le E/S du GPIO pour correspondre avec
52
  * 				La fonction MAP_MotorPin va venir configurer le E/S du GPIO pour correspondre avec
48
- * 				le schma electrique en ressource. La fonction initialise aussi l'interruption EXTI
49
- * 				de la dtection du chargeur.
53
+ * 				le schéma electrique en ressource. La fonction initialise aussi l'interruption EXTI
54
+ * 				de la détection du chargeur.
50
  *
55
  *
51
  * @note		A3 en output alternate function.
56
  * @note		A3 en output alternate function.
52
  *				A0 et A4 en floating input.
57
  *				A0 et A4 en floating input.
57
  *
62
  *
58
  */
63
  */
59
 
64
 
60
-void MAP_batteryPin(void)
65
+void batteryConfigure(void)
61
 {
66
 {
62
     GPIO_InitTypeDef Init_Structure;
67
     GPIO_InitTypeDef Init_Structure;
63
     NVIC_InitTypeDef NVIC_InitStructure;
68
     NVIC_InitTypeDef NVIC_InitStructure;
64
     EXTI_InitTypeDef EXTI_InitStructure;
69
     EXTI_InitTypeDef EXTI_InitStructure;
70
+    ADC_InitTypeDef ADC_InitStructure;
65
 
71
 
66
     Init_Structure.GPIO_Pin = GPIO_Pin_3;
72
     Init_Structure.GPIO_Pin = GPIO_Pin_3;
67
     Init_Structure.GPIO_Speed = GPIO_Speed_10MHz;
73
     Init_Structure.GPIO_Speed = GPIO_Speed_10MHz;
91
     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
97
     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
92
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
98
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
93
     NVIC_Init(&NVIC_InitStructure);
99
     NVIC_Init(&NVIC_InitStructure);
94
-}
95
 
100
 
96
-/**
97
- * @brief 		Initialise la dma pour stocker les valeur dans ADCConvertedValue.
98
- *				On stockera 16 valeurs de fa�on � faire un moyennage.
99
- *
100
- * @param  		None
101
- * @retval 		None
102
- *
103
- */
101
+    // Initialise la dma pour stocker les valeur dans ADCConvertedValue.
104
 
102
 
105
-void DMA_BAT(void)
106
-{
107
-    /* DMA1 channel1 configuration ----------------------------------------------*/
108
     DMA_DeInit(DMA1_Channel1);
103
     DMA_DeInit(DMA1_Channel1);
109
     DMA_BAT_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(ADC1->DR); //   ADC1_DR_Address;
104
     DMA_BAT_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(ADC1->DR); //   ADC1_DR_Address;
110
     DMA_BAT_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&ADCConvertedValue;
105
     DMA_BAT_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&ADCConvertedValue;
121
 
116
 
122
     DMA_Cmd(DMA1_Channel1, ENABLE);
117
     DMA_Cmd(DMA1_Channel1, ENABLE);
123
     DMA_ITConfig(DMA1_Channel1, DMA_IT_TC, ENABLE);
118
     DMA_ITConfig(DMA1_Channel1, DMA_IT_TC, ENABLE);
119
+
120
+    // Configuration et Calibration de l'ADC1 sur 1 channel.
121
+    ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
122
+    ADC_InitStructure.ADC_ScanConvMode = ENABLE;
123
+    ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
124
+    ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
125
+    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
126
+    ADC_InitStructure.ADC_NbrOfChannel = 1;
127
+    ADC_Init(ADC1, &ADC_InitStructure);
128
+
129
+    /* ADC1 regular channel1 configuration */
130
+    ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_71Cycles5);
131
+
132
+    /* Start ADC1 Software Conversion */
133
+    ADC_Cmd(ADC1, ENABLE);
134
+
135
+    ADC_StartCalibration(ADC1);
136
+    /* Check the end of ADC1 calibration */
137
+    while(ADC_GetCalibrationStatus(ADC1));
138
+
139
+    ADC_SoftwareStartConvCmd(ADC1, ENABLE);
140
+
141
+    /* Enable the ADC1 DMA Interrupt */
142
+    NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel1_IRQn;
143
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
144
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
145
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
146
+    NVIC_Init(&NVIC_InitStructure);
124
 }
147
 }
125
 
148
 
126
 /**
149
 /**
138
  *
161
  *
139
  */
162
  */
140
 
163
 
141
-void startACQDMA(void)
164
+void batteryStartAcquisition(void)
142
 {
165
 {
143
     ADC_DMACmd(ADC1, ENABLE);
166
     ADC_DMACmd(ADC1, ENABLE);
144
     DMA_DeInit(DMA1_Channel1);
167
     DMA_DeInit(DMA1_Channel1);
149
 
172
 
150
 /**
173
 /**
151
  * @brief 		Fonction de plus haut niveau qui initialisera la DMA  et qui lancera une nouvelle acquisition.
174
  * @brief 		Fonction de plus haut niveau qui initialisera la DMA  et qui lancera une nouvelle acquisition.
152
- * 				Cette fonction est appell� � interval r�gulier dans le systick. Cette fonction utilise startACQDMA.
175
+ * 				Cette fonction est appelée à intervalle régulier dans le systick. Cette fonction utilise startACQDMA.
153
  *
176
  *
154
  * @param  		None
177
  * @param  		None
155
  * @retval 		None
178
  * @retval 		None
156
  *
179
  *
157
  */
180
  */
158
 
181
 
159
-void voltagePrepare(void)
182
+void batteryRefreshData(void)
160
 {
183
 {
161
     DMA_BAT_InitStructure.DMA_BufferSize = VOLTAGE_BUFFER_SIZE;
184
     DMA_BAT_InitStructure.DMA_BufferSize = VOLTAGE_BUFFER_SIZE;
162
 
185
 
163
     ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 1, ADC_SampleTime_55Cycles5);
186
     ADC_RegularChannelConfig(ADC1, ADC_Channel_4, 1, ADC_SampleTime_55Cycles5);
164
     ADC_Cmd(ADC1, ENABLE);
187
     ADC_Cmd(ADC1, ENABLE);
165
-    startACQDMA();
188
+    batteryStartAcquisition();
166
 }
189
 }
167
 
190
 
168
 /**
191
 /**
169
- * @}
170
- */
171
-
172
-/** @addtogroup Init_GPIO_DMA_IT_Battery
173
- * @{
174
- */
175
-
176
-/**
177
- * @brief 		Configuration et Calibration de l'ADC1 sur 1 channel.
178
- * 				L'adc lira en mode continue.
179
- *
180
- * @param  		None
181
- * @retval 		None
182
- *
183
- */
184
-
185
-void ADC1_CONFIG(void)
186
-{
187
-    /* ADC1 configuration ------------------------------------------------------*/
188
-    ADC_InitStructure.ADC_Mode = ADC_Mode_Independent;
189
-    ADC_InitStructure.ADC_ScanConvMode = ENABLE;
190
-    ADC_InitStructure.ADC_ContinuousConvMode = ENABLE;
191
-    ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None;
192
-    ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right;
193
-    ADC_InitStructure.ADC_NbrOfChannel = 1;
194
-    ADC_Init(ADC1, &ADC_InitStructure);
195
-
196
-    /* ADC1 regular channel1 configuration */
197
-    ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_71Cycles5);
198
-
199
-    /* Start ADC1 Software Conversion */
200
-    ADC_Cmd(ADC1, ENABLE);
201
-
202
-    ADC_StartCalibration(ADC1);
203
-    /* Check the end of ADC1 calibration */
204
-    while(ADC_GetCalibrationStatus(ADC1));
205
-
206
-    ADC_SoftwareStartConvCmd(ADC1, ENABLE);
207
-}
208
-
209
-/**
210
- * @brief 		Initialise l'interruption � la fin des acquisitions sur la DMA.
211
- *
212
- * @param  		None
213
- * @retval 		None
192
+ * @brief       Appelé de manière régulière pour mettre à jour le niveau batterie
214
  *
193
  *
194
+ * @param       None
195
+ * @retval      None
215
  */
196
  */
216
-
217
-void INIT_IT_DMA(void)
218
-{
219
-    NVIC_InitTypeDef NVIC_InitStructure;
220
-
221
-    /* Enable the USARTz Interrupt */
222
-    NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel1_IRQn;
223
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
224
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
225
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
226
-    NVIC_Init(&NVIC_InitStructure);
197
+void batteryManagement(void) {
198
+    int k;
199
+
200
+    if(Dumber.acquisition==VOLTAGE && Dumber.BatterieChecking==TRUE) {
201
+        vbatLowerVal = 0xFFF;
202
+        vbatHighVal = 0;
203
+
204
+        for(k=0; k<VOLTAGE_BUFFER_SIZE; k++)
205
+        {
206
+            meanVoltage+=ADCConvertedValue[k];
207
+
208
+            if (vbatLowerVal> ADCConvertedValue[k]) vbatLowerVal = ADCConvertedValue[k];
209
+            if (vbatHighVal< ADCConvertedValue[k]) vbatHighVal = ADCConvertedValue[k];
210
+        }
211
+
212
+        vbatDiff = vbatHighVal - vbatLowerVal;
213
+
214
+        meanVoltage= meanVoltage/VOLTAGE_BUFFER_SIZE;
215
+        mesureVoltage = meanVoltage;
216
+
217
+        Dumber.BatteryPercentage = mesureVoltage;
218
+        Dumber.acquisition=FALSE;
219
+
220
+        if(Dumber.BatteryPercentage >= VBAT_SEUIL_LOW)
221
+        {
222
+            cptMesureHigh++;
223
+            if(cptMesureHigh >= COMPTEUR_SEUIL_HIGH)
224
+            {
225
+                if(Dumber.StateSystem == STATE_LOW)
226
+                    systemChangeState(STATE_RUN);
227
+
228
+                Dumber.stateBattery = 2;
229
+                cptMesureHigh=0;
230
+                cptMesureLow=0;
231
+                cptMesureDisable=0;
232
+                cptMesureEmergencyHalt=0;
233
+            }
234
+        }
235
+        else if (Dumber.BatteryPercentage < VBAT_SEUIL_LOW && Dumber.BatteryPercentage >= VBAT_SEUIL_DISABLE)
236
+        {
237
+            cptMesureLow++;
238
+            if(cptMesureLow >= COMPTEUR_SEUIL_LOW)
239
+            {
240
+                if(Dumber.StateSystem == STATE_RUN)
241
+                    systemChangeState(STATE_LOW);
242
+
243
+                Dumber.stateBattery =1;
244
+                cptMesureHigh=0;
245
+                cptMesureLow=0;
246
+                cptMesureDisable=0;
247
+            }
248
+        }
249
+        else // Dumber.BatteryPercentage < VBAT_SEUIL_DISABLE
250
+        {
251
+            cptMesureDisable++;
252
+
253
+            if(cptMesureDisable >= COMPTEUR_SEUIL_DISABLE)
254
+            {
255
+                systemChangeState(STATE_DISABLE);
256
+            }
257
+        }
258
+    }
227
 }
259
 }
228
 
260
 
229
 /**
261
 /**
231
  */
263
  */
232
 
264
 
233
 
265
 
266
+
234
 /** @addtogroup Handler
267
 /** @addtogroup Handler
235
  * @{
268
  * @{
236
  */
269
  */
237
 
270
 
238
 
271
 
239
 /**
272
 /**
240
- * @brief 		Interruption Handler. Qui va faire la moyenne des dernires
241
- * 				acquisitions lorsque la DMA  rempli son buffer.
273
+ * @brief 		Interruption Handler. Qui va faire la moyenne des dernières
274
+ * 				acquisitions lorsque la DMA à rempli son buffer.
242
  *
275
  *
243
  * @param  		None
276
  * @param  		None
244
  * @retval 		None
277
  * @retval 		None
256
 }
289
 }
257
 
290
 
258
 /**
291
 /**
259
- * @brief 		Interruption qui donne l'ordre d'teindre le robot.
260
- * 				L'IT se d�clenche lorsque le chargeur est branch�.
292
+ * @brief 		Interruption qui donne l'ordre d'éteindre le robot.
293
+ * 				L'IT se déclenche lorsque le chargeur est branché.
261
  *
294
  *
262
  * @param  		None
295
  * @param  		None
263
  * @retval 		None
296
  * @retval 		None
265
  */
298
  */
266
 void EXTI15_10_IRQHandler(void)
299
 void EXTI15_10_IRQHandler(void)
267
 {
300
 {
268
-    shutDown();
301
+    systemShutDown();
269
     while (1);
302
     while (1);
270
 }
303
 }
271
 
304
 

+ 29
- 13
software/robot/src/battery.h View File

14
  ******************************************************************************
14
  ******************************************************************************
15
  */
15
  */
16
 
16
 
17
-#ifndef Battery_H
18
-#define Battery_H
17
+#ifndef _BATTERY_H_
18
+#define _BATTERY_H_
19
 
19
 
20
 #include "stm32f10x.h"
20
 #include "stm32f10x.h"
21
 
21
 
22
-#define VOLTAGE_BUFFER_SIZE 64
23
-extern __IO uint16_t ADCConvertedValue[VOLTAGE_BUFFER_SIZE];
22
+#define VOLTAGE_BUFFER_SIZE         64
24
 
23
 
25
-#define VBAT_SEUIL_LOW			0x878
26
-#define VBAT_SEUIL_DISABLE		0x7CA
24
+#define VBAT_SEUIL_LOW			    0x878
25
+#define VBAT_SEUIL_DISABLE		    0x7CA
27
 #define VBAT_SEUIL_EMERGENCY_HALT	0x6E0
26
 #define VBAT_SEUIL_EMERGENCY_HALT	0x6E0
28
 
27
 
29
-void MAP_batteryPin(void);
30
-void DMA_BAT(void);
31
-void ADC1_CONFIG(void);
32
-void INIT_IT_DMA(void);
33
-void startACQDMA(void);
34
-void voltagePrepare(void);
28
+#define COMPTEUR_SEUIL_HIGH         8
29
+#define COMPTEUR_SEUIL_LOW          8
30
+#define COMPTEUR_SEUIL_DISABLE      8
31
+#define COMPTEUR_SEUIL_EMERGENCY_HALT   3000
35
 
32
 
36
-#endif /* Battery_H */
33
+extern char cptMesureHigh;
34
+extern char cptMesureLow;
35
+extern char cptMesureDisable;
36
+
37
+extern uint16_t vbatLowerVal;
38
+extern uint16_t vbatHighVal;
39
+extern uint16_t vbatDiff;
40
+
41
+extern uint16_t testPostion;
42
+extern uint32_t mesureVoltage;
43
+extern uint32_t meanVoltage;
44
+
45
+extern uint32_t cptMesureEmergencyHalt;
46
+
47
+void batteryConfigure(void);
48
+void batteryManagement(void);
49
+void batteryStartAcquisition(void);
50
+void batteryRefreshData(void);
51
+
52
+#endif /* _BATTERY_H_ */
37
 
53
 

+ 141
- 117
software/robot/src/cmdManager.c View File

16
  */
16
  */
17
 
17
 
18
 #include <stm32f10x.h>
18
 #include <stm32f10x.h>
19
-
20
-#include "cmdManager.h"
21
-
22
 #include <stdio.h>
19
 #include <stdio.h>
23
 #include <stdlib.h>
20
 #include <stdlib.h>
24
 #include <string.h>
21
 #include <string.h>
25
 
22
 
26
-#include "cmde_usart.h"
23
+#include "cmdManager.h"
27
 #include "battery.h"
24
 #include "battery.h"
28
 #include "motor.h"
25
 #include "motor.h"
29
 #include "system_dumby.h"
26
 #include "system_dumby.h"
27
+#include "usart.h"
30
 
28
 
31
 /** @addtogroup Projects
29
 /** @addtogroup Projects
32
  * @{
30
  * @{
36
  * @{
34
  * @{
37
  */
35
  */
38
 
36
 
39
-volatile unsigned char checksum;
40
-volatile int length;
41
-uint16_t j;
37
+/* Definition des commandes */
38
+
39
+#define PingCMD                 'p'
40
+#define ResetCMD                'r'
41
+#define SetMotorCMD             'm'
42
+#define StartWWatchDogCMD       'W'
43
+#define ResetWatchdogCMD        'w'
44
+#define GetBatteryVoltageCMD    'v'
45
+#define GetVersionCMD           'V'
46
+#define StartWithoutWatchCMD    'u'
47
+#define MoveCMD                 'M'
48
+#define TurnCMD                 'T'
49
+#define BusyStateCMD            'b'
50
+#define TestCMD                 't'
51
+#define DebugCMD                'a'
52
+
53
+#define OK_ANS              "O\r"
54
+#define ERR_ANS             "E\r"
55
+#define UNKNOW_ANS          "C\r"
56
+#define BAT_OK              "2\r"
57
+#define BAT_LOW             "1\r"
58
+#define BAT_EMPTY           "0\r"
42
 
59
 
43
 /** @addtogroup Checksum
60
 /** @addtogroup Checksum
44
  * @{
61
  * @{
53
  * @retval 		0 ou 1
70
  * @retval 		0 ou 1
54
  *
71
  *
55
  */
72
  */
56
-void inclusionCheckSum(void) {
57
-    checksum = 0;
73
+void cmdAddChecksum(void) {
74
+    uint16_t j;
75
+    unsigned char checksum=0;
76
+
58
     for (j = 0; sendString[j] != '\r'; j++)
77
     for (j = 0; sendString[j] != '\r'; j++)
59
         checksum ^= sendString[j];
78
         checksum ^= sendString[j];
60
     if (checksum == '\r')
79
     if (checksum == '\r')
73
  * @retval 		0 ou 1
92
  * @retval 		0 ou 1
74
  *
93
  *
75
  */
94
  */
76
-char verifyCheckSum(void) {
77
-    uint16_t j, lenght;
78
-    checksum = 0;
79
-    lenght = strlen(receiptString);
80
-    for (j = 0; j < lenght - 2; j++) {
95
+char cmdVerifyChecksum(void) {
96
+    uint16_t j;
97
+    uint16_t length;
98
+    unsigned char checksum=0;
99
+
100
+    length = strlen(receiptString);
101
+    for (j = 0; j < length - 2; j++) {
81
         checksum ^= receiptString[j];
102
         checksum ^= receiptString[j];
82
     }
103
     }
83
     if (checksum == '\r')
104
     if (checksum == '\r')
84
         checksum++;
105
         checksum++;
85
 
106
 
86
     if (receiptString[j] == checksum) {
107
     if (receiptString[j] == checksum) {
87
-        receiptString[lenght - 2] = 13;
88
-        receiptString[lenght - 1] = 0;
89
-        receiptString[lenght] = 0;
108
+        receiptString[length - 2] = 13;
109
+        receiptString[length - 1] = 0;
110
+        receiptString[length] = 0;
111
+
90
         return 0;
112
         return 0;
91
     } else
113
     } else
92
         return 1;
114
         return 1;
112
  * @retval 		None
134
  * @retval 		None
113
  */
135
  */
114
 
136
 
115
-void manageCmd(void) {
116
-    switch (receiptString[0]) {
117
-        case PingCMD:
118
-            actionPing();
119
-            break;
120
-
121
-        case ResetCMD:
122
-            actionReset();
123
-            break;
124
-
125
-        case StartWWatchDogCMD:
126
-            actionStartWithWD();
127
-            break;
128
-
129
-        case ResetWatchdogCMD:
130
-            actionResetWD();
131
-            break;
132
-
133
-        case GetBatteryVoltageCMD:
134
-            actionBatteryVoltage();
135
-            break;
136
-
137
-        case GetVersionCMD:
138
-            actionVersion();
139
-            break;
140
-
141
-        case StartWithoutWatchCMD:
142
-            actionStartWWD();
143
-            break;
144
-
145
-        case MoveCMD:
146
-            actionMove();
147
-            break;
148
-
149
-        case TurnCMD:
150
-            actionTurn();
151
-            break;
152
-
153
-        case BusyStateCMD:
154
-            actionBusyState();
155
-            break;
156
-
157
-        case 'a':
158
-            actionDebug();
159
-            break;
160
-
161
-        default:
162
-            strcpy(sendString, UNKNOW_ANS);
137
+void cmdManage(void) {
138
+    if (cmdVerifyChecksum() != 0) {
139
+        strcpy(sendString, UNKNOW_ANS);
140
+    } else { // Checksum valide
141
+        if (Dumber.StateSystem==STATE_DISABLE) { // SI la batterie est trop faible, impossible d'accepter une commande: on reste dans ce mode
142
+            strcpy(sendString, ERR_ANS);
143
+        } else {
144
+            switch (receiptString[0]) {
145
+                case PingCMD:
146
+                    cmdPingAction();
147
+                    break;
148
+
149
+                case ResetCMD:
150
+                    cmdResetAction();
151
+                    break;
152
+
153
+                case StartWWatchDogCMD:
154
+                    cmdStartWithWatchdogAction();
155
+                    break;
156
+
157
+                case ResetWatchdogCMD:
158
+                    cmdResetWatchdogAction();
159
+                    break;
160
+
161
+                case GetBatteryVoltageCMD:
162
+                    cmdBatteryVoltageAction();
163
+                    break;
164
+
165
+                case GetVersionCMD:
166
+                    cmdVersionAction();
167
+                    break;
168
+
169
+                case StartWithoutWatchCMD:
170
+                    cmdStartWithoutWatchdogAction();
171
+                    break;
172
+
173
+                case MoveCMD:
174
+                    cmdMoveAction();
175
+                    break;
176
+
177
+                case TurnCMD:
178
+                    cmdTurnAction();
179
+                    break;
180
+
181
+                case BusyStateCMD:
182
+                    cmdBusyStateAction();
183
+                    break;
184
+
185
+                case 'a':
186
+                    cmdDebugAction();
187
+                    break;
188
+
189
+                default:
190
+                    strcpy(sendString, UNKNOW_ANS);
191
+            }
192
+        }
163
     }
193
     }
194
+
195
+    Dumber.cpt_inactivity=0; // remise a zéro du compteur d'inativité
164
 }
196
 }
165
 
197
 
166
 /**
198
 /**
179
  * @retval 		None
211
  * @retval 		None
180
  */
212
  */
181
 
213
 
182
-void actionPing(void) {
183
-    if (receiptString[1] == 13)
214
+void cmdPingAction(void) {
215
+    if (receiptString[1] == '\r')
184
         strcpy(sendString, OK_ANS);
216
         strcpy(sendString, OK_ANS);
185
     else
217
     else
186
         strcpy(sendString, ERR_ANS);
218
         strcpy(sendString, ERR_ANS);
194
  * @param  		None
226
  * @param  		None
195
  * @retval 		None
227
  * @retval 		None
196
  */
228
  */
197
-void actionReset(void) {
198
-    Dumber.StateSystem = IDLE;
199
-    Dumber.WatchDogStartEnable = TRUE;
200
-    Dumber.cpt_watchdog = 0;
201
-    Dumber.cpt_systick = 0;
202
-    cmdLeftMotor(BRAKE, 0);
203
-    cmdRightMotor(BRAKE, 0);
229
+void cmdResetAction(void) {
230
+    systemChangeState(STATE_IDLE);
204
     strcpy(sendString, OK_ANS);
231
     strcpy(sendString, OK_ANS);
205
 }
232
 }
206
 
233
 
210
  * @param  		None
237
  * @param  		None
211
  * @retval 		None
238
  * @retval 		None
212
  */
239
  */
213
-void actionVersion(void) {
214
-    if (receiptString[1] == 13)
240
+void cmdVersionAction(void) {
241
+    if (receiptString[1] == '\r')
215
         strcpy(sendString, VERSION);
242
         strcpy(sendString, VERSION);
216
     else
243
     else
217
         strcpy(sendString, ERR_ANS);
244
         strcpy(sendString, ERR_ANS);
225
  * @param  		None
252
  * @param  		None
226
  * @retval 		None
253
  * @retval 		None
227
  */
254
  */
228
-void actionBusyState(void) {
229
-    if (Dumber.StateSystem == RUN || Dumber.StateSystem == LOW) {
255
+void cmdBusyStateAction(void) {
256
+    if ((Dumber.StateSystem == STATE_RUN) || (Dumber.StateSystem == STATE_LOW)) {
230
         if (Dumber.busyState == TRUE)
257
         if (Dumber.busyState == TRUE)
231
-            strcpy(sendString, "1");
258
+            strcpy(sendString, "1\r");
232
         else
259
         else
233
-            strcpy(sendString, "0");
260
+            strcpy(sendString, "0\r");
234
     } else {
261
     } else {
235
         strcpy(sendString, ERR_ANS);
262
         strcpy(sendString, ERR_ANS);
236
     }
263
     }
237
 }
264
 }
238
 
265
 
239
 /**
266
 /**
240
- * @brief 		Effectue une remise à zero du watchdog.
267
+ * @brief 		Effectue une remise à zéro du watchdog.
241
  *
268
  *
242
  * @param  		None
269
  * @param  		None
243
  * @retval 		None
270
  * @retval 		None
244
  */
271
  */
245
-void actionResetWD(void) {
246
-    if (Dumber.StateSystem == RUN && watchDogState==TRUE){
247
-        Dumber.cpt_watchdog = 0;
272
+void cmdResetWatchdogAction(void) {
273
+    if (systemResetWatchdog()!=0) { // Réussite
248
         strcpy(sendString, OK_ANS);
274
         strcpy(sendString, OK_ANS);
249
-    }
250
-    else
251
-        strcpy(sendString, ERR_ANS);
275
+    } else strcpy(sendString, ERR_ANS);
252
 }
276
 }
253
 
277
 
254
 /**
278
 /**
260
  * @param  		None
284
  * @param  		None
261
  * @retval 		None
285
  * @retval 		None
262
  */
286
  */
263
-void actionStartWithWD(void) {
264
-    if (Dumber.StateSystem == IDLE && receiptString[1] == 13) {
265
-        strcpy(sendString, OK_ANS);
287
+void cmdStartWithWatchdogAction(void) {
288
+    if (Dumber.StateSystem == STATE_IDLE && receiptString[1] == '\r') {
266
         Dumber.WatchDogStartEnable = TRUE;
289
         Dumber.WatchDogStartEnable = TRUE;
267
-        Dumber.StateSystem = RUN;
290
+        systemChangeState(STATE_RUN);
291
+        strcpy(sendString, OK_ANS);
268
     } else
292
     } else
269
         strcpy(sendString, ERR_ANS);
293
         strcpy(sendString, ERR_ANS);
270
 }
294
 }
276
  * @param  		None
300
  * @param  		None
277
  * @retval 		None
301
  * @retval 		None
278
  */
302
  */
279
-void actionStartWWD(void) {
280
-    if (Dumber.StateSystem == IDLE && receiptString[1] == 13) {
281
-        strcpy(sendString, OK_ANS);
303
+void cmdStartWithoutWatchdogAction(void) {
304
+    if (Dumber.StateSystem == STATE_IDLE && receiptString[1] == '\r') {
282
         Dumber.WatchDogStartEnable = FALSE;
305
         Dumber.WatchDogStartEnable = FALSE;
283
-        Dumber.StateSystem = RUN;
306
+        systemChangeState(STATE_RUN);
307
+        strcpy(sendString, OK_ANS);
284
     } else
308
     } else
285
         strcpy(sendString, ERR_ANS);
309
         strcpy(sendString, ERR_ANS);
286
 }
310
 }
295
  * @param  		None
319
  * @param  		None
296
  * @retval 		None
320
  * @retval 		None
297
  */
321
  */
298
-void actionMove(void) {
299
-    if (Dumber.StateSystem == RUN || Dumber.StateSystem == LOW) {
322
+void cmdMoveAction(void) {
323
+    if (Dumber.StateSystem == STATE_RUN || Dumber.StateSystem == STATE_LOW) {
300
         int laps;
324
         int laps;
301
         uint16_t testReception = sscanf(receiptString, "M=%i\r", &laps);
325
         uint16_t testReception = sscanf(receiptString, "M=%i\r", &laps);
302
         unsigned char mod = 0;
326
         unsigned char mod = 0;
313
                 mod = FORWARD;
337
                 mod = FORWARD;
314
 
338
 
315
             laps = laps * 2;
339
             laps = laps * 2;
316
-            regulationMoteur(mod, mod, (unsigned) laps, (unsigned) laps,
340
+
341
+            motorRegulation(mod, mod, (unsigned) laps, (unsigned) laps,
317
                     COMMONSPEED, COMMONSPEED);
342
                     COMMONSPEED, COMMONSPEED);
343
+
318
             strcpy(sendString, OK_ANS);
344
             strcpy(sendString, OK_ANS);
319
         } else
345
         } else
320
             strcpy(sendString, ERR_ANS);
346
             strcpy(sendString, ERR_ANS);
322
 }
348
 }
323
 
349
 
324
 /**
350
 /**
325
- * @brief 		Execute une action tourne avec les paramétres dans receitpString.
351
+ * @brief 		Execute une action tourne avec les paramètres dans receitpString.
326
  * 				Type de commande à envoyer : "T=val\r". Ou val peut être positif
352
  * 				Type de commande à envoyer : "T=val\r". Ou val peut être positif
327
  * 				ou negatif.
353
  * 				ou negatif.
328
  *
354
  *
329
  * @param  		None
355
  * @param  		None
330
  * @retval 		None
356
  * @retval 		None
331
  */
357
  */
332
-void actionTurn(void) {
333
-    if (Dumber.StateSystem == RUN || Dumber.StateSystem == LOW) {
358
+void cmdTurnAction(void) {
359
+    if (Dumber.StateSystem == STATE_RUN || Dumber.StateSystem == STATE_LOW) {
334
         int degree;
360
         int degree;
335
         uint16_t testReception = sscanf(receiptString, "T=%i\r", &degree);
361
         uint16_t testReception = sscanf(receiptString, "T=%i\r", &degree);
336
         tourPositionG = 0;
362
         tourPositionG = 0;
337
         tourPositionD = 0;
363
         tourPositionD = 0;
364
+
338
         if (testReception == 1) {
365
         if (testReception == 1) {
339
             degree = degree * 1.40;
366
             degree = degree * 1.40;
340
             Dumber.cpt_inactivity = 0;
367
             Dumber.cpt_inactivity = 0;
341
             Dumber.busyState = TRUE;
368
             Dumber.busyState = TRUE;
369
+
342
             if (degree < 0) {
370
             if (degree < 0) {
343
                 degree = degree * -1;
371
                 degree = degree * -1;
344
-                if (degree < 30)
345
-                    regulationMoteur(FORWARD, REVERSE, (unsigned) degree,
346
-                            (unsigned) degree, LOWSPEED, LOWSPEED);
347
-                else
348
-                    regulationMoteur(FORWARD, REVERSE, (unsigned) degree,
349
-                            (unsigned) degree, COMMONSPEED, COMMONSPEED);
372
+                motorRegulation(FORWARD, REVERSE, (unsigned) degree,
373
+                        (unsigned) degree, LOWSPEED, LOWSPEED);
350
             } else {
374
             } else {
351
-                if (degree < 30)
352
-                    regulationMoteur(REVERSE, FORWARD, (unsigned) degree,
353
-                            (unsigned) degree, LOWSPEED, LOWSPEED);
354
-                else
355
-                    regulationMoteur(REVERSE, FORWARD, (unsigned) degree,
356
-                            (unsigned) degree, COMMONSPEED, COMMONSPEED);
375
+                motorRegulation(REVERSE, FORWARD, (unsigned) degree,
376
+                        (unsigned) degree, LOWSPEED, LOWSPEED);
357
             }
377
             }
378
+            strcpy(sendString, OK_ANS);
358
         } else
379
         } else
359
             strcpy(sendString, ERR_ANS);
380
             strcpy(sendString, ERR_ANS);
360
     }
381
     }
370
  * @param  		None
391
  * @param  		None
371
  * @retval 		None
392
  * @retval 		None
372
  */
393
  */
373
-void actionBatteryVoltage(void) {
394
+void cmdBatteryVoltageAction(void) {
374
     char battery[2];
395
     char battery[2];
375
     battery[0] = Dumber.stateBattery + '0';
396
     battery[0] = Dumber.stateBattery + '0';
376
     battery[1] = '\r';
397
     battery[1] = '\r';
386
  * @param  		None
407
  * @param  		None
387
  * @retval 		None
408
  * @retval 		None
388
  */
409
  */
389
-void actionDebug(void) {
410
+void cmdDebugAction(void) {
411
+    uint8_t j;
412
+
390
     sprintf(sendString, "Th-D=%u G=%u\r", tourPositionD, tourPositionG);
413
     sprintf(sendString, "Th-D=%u G=%u\r", tourPositionD, tourPositionG);
391
-    sendDataUSART();
414
+    usartSendData();
415
+
392
     for (j = 0; j < 200; j++);
416
     for (j = 0; j < 200; j++);
393
     sprintf(sendString, "Re-D=%u G=%u\r", G_lapsRight, G_lapsLeft);
417
     sprintf(sendString, "Re-D=%u G=%u\r", G_lapsRight, G_lapsLeft);
394
-    sendDataUSART();
418
+    usartSendData();
395
 }
419
 }
396
 
420
 
397
 /**
421
 /**

+ 20
- 20
software/robot/src/cmdManager.h View File

6
  * @date    19-June-2017
6
  * @date    19-June-2017
7
  * @brief   Gestion de commande reçu via l'uart
7
  * @brief   Gestion de commande reçu via l'uart
8
  *
8
  *
9
- *			Traite les chaines de caractére reçu par l'uart.
10
- *			Permet de verifier les erreurs de checksum,
9
+ *			Traite les chaînes de caractère reçu par l'uart.
10
+ *			Permet de vérifier les erreurs de checksum,
11
  *			de traiter les valeurs retours.
11
  *			de traiter les valeurs retours.
12
  *
12
  *
13
- *@attention Utilise les variables globals - receiptString - sendString
13
+ * @attention Utilise les variables globales - receiptString - sendString
14
  *
14
  *
15
  ******************************************************************************
15
  ******************************************************************************
16
  ******************************************************************************
16
  ******************************************************************************
17
  */
17
  */
18
 
18
 
19
-#ifndef CMD_MANAGER_H_
20
-#define CMD_MANAGER_H_
19
+#ifndef _CMD_MANAGER_H_
20
+#define _CMD_MANAGER_H_
21
 
21
 
22
 #include <stm32f10x.h>
22
 #include <stm32f10x.h>
23
 
23
 
24
-void manageCmd(void);
25
-char verifyCheckSum(void);
26
-void inclusionCheckSum(void);
27
-void actionReset(void);
28
-void actionBusyState(void);
29
-void actionPing(void);
30
-void actionVersion(void);
31
-void actionStartWWD(void);
32
-void actionMove(void);
33
-void actionTurn(void);
34
-void actionBatteryVoltage(void);
35
-void actionStartWithWD(void);
36
-void actionResetWD(void);
37
-void actionDebug(void);
24
+void cmdManage(void);
25
+char cmdVerifyChecksum(void);
26
+void cmdAddChecksum(void);
27
+void cmdResetAction(void);
28
+void cmdBusyStateAction(void);
29
+void cmdPingAction(void);
30
+void cmdVersionAction(void);
31
+void cmdStartWithoutWatchdogAction(void);
32
+void cmdMoveAction(void);
33
+void cmdTurnAction(void);
34
+void cmdBatteryVoltageAction(void);
35
+void cmdStartWithWatchdogAction(void);
36
+void cmdResetWatchdogAction(void);
37
+void cmdDebugAction(void);
38
 
38
 
39
-#endif /* CMD_MANAGER_H_ */
39
+#endif /* _CMD_MANAGER_H_ */
40
 
40
 

+ 3
- 2
software/robot/src/debug.c View File

11
  ******************************************************************************
11
  ******************************************************************************
12
  */
12
  */
13
 
13
 
14
-#include "stm32f10x.h"                  // Device header
15
-#include "debug.h"
14
+#include <stm32f10x.h>                  // Device header
16
 #include <stdio.h>
15
 #include <stdio.h>
17
 #include <stdlib.h>
16
 #include <stdlib.h>
18
 
17
 
18
+#include "debug.h"
19
+
19
 /** @addtogroup Projects
20
 /** @addtogroup Projects
20
  * @{
21
  * @{
21
  */
22
  */

+ 113
- 9
software/robot/src/led.c View File

13
  */
13
  */
14
 
14
 
15
 #include <stm32f10x.h>
15
 #include <stm32f10x.h>
16
-#include "led.h"
17
-
18
 #include "system_dumby.h"
16
 #include "system_dumby.h"
17
+#include "led.h"
19
 
18
 
20
 /** @addtogroup Projects
19
 /** @addtogroup Projects
21
  * @{
20
  * @{
33
 
32
 
34
 TIM_TimeBaseInitTypeDef TIM_TimeBaseLED;
33
 TIM_TimeBaseInitTypeDef TIM_TimeBaseLED;
35
 TIM_OCInitTypeDef TIM_OCConfigure;
34
 TIM_OCInitTypeDef TIM_OCConfigure;
35
+char etatLED;
36
+char LEDON;
37
+
38
+#define LED_GREEN   0
39
+#define LED_RED     1
40
+#define LED_ORANGE  2
41
+
42
+/*void ledGreenOn(void);
43
+void ledRedOn(void);
44
+void ledOff(void);
45
+void ledOrangeOn(void);*/
36
 
46
 
37
 /**
47
 /**
38
- * @brief Configure le GPIO PB0 et PB1 afin de contr�ler la led.
48
+ * @brief Configure le GPIO PB0 et PB1 afin de contrôler la led.
39
  * @param Aucun
49
  * @param Aucun
40
  */
50
  */
41
-void MAP_LEDpin(void)
51
+void ledConfigure(void)
42
 {
52
 {
43
     GPIO_InitTypeDef Init_Structure;
53
     GPIO_InitTypeDef Init_Structure;
44
 
54
 
47
     Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
57
     Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
48
     Init_Structure.GPIO_Mode = GPIO_Mode_Out_PP;
58
     Init_Structure.GPIO_Mode = GPIO_Mode_Out_PP;
49
     GPIO_Init(GPIOB, &Init_Structure);
59
     GPIO_Init(GPIOB, &Init_Structure);
60
+
61
+    etatLED = 1;
62
+    LEDON =0;
50
 }
63
 }
51
 
64
 
52
 /**
65
 /**
60
  * @brief Allume une LED de couleur orange.
73
  * @brief Allume une LED de couleur orange.
61
  * @param Aucun
74
  * @param Aucun
62
  */
75
  */
63
-void LEDorange(void)
76
+void ledOrangeOn(void)
64
 {
77
 {
65
     GPIO_SetBits(GPIOB, GPIO_Pin_1);
78
     GPIO_SetBits(GPIOB, GPIO_Pin_1);
66
     GPIO_SetBits(GPIOB,GPIO_Pin_0);
79
     GPIO_SetBits(GPIOB,GPIO_Pin_0);
70
  * @brief Allume une LED de couleur rouge.
83
  * @brief Allume une LED de couleur rouge.
71
  * @param Aucun
84
  * @param Aucun
72
  */
85
  */
73
-void LEDred(void)
86
+void ledRedOn(void)
74
 {
87
 {
75
     GPIO_ResetBits(GPIOB, GPIO_Pin_1);
88
     GPIO_ResetBits(GPIOB, GPIO_Pin_1);
76
     GPIO_SetBits(GPIOB,GPIO_Pin_0);
89
     GPIO_SetBits(GPIOB,GPIO_Pin_0);
80
  * @brief Allume une LED de couleur verte.
93
  * @brief Allume une LED de couleur verte.
81
  * @param Aucun
94
  * @param Aucun
82
  */
95
  */
83
-void LEDgreen(void)
96
+void ledGreenOn(void)
84
 {
97
 {
85
 
98
 
86
     GPIO_SetBits(GPIOB, GPIO_Pin_1);
99
     GPIO_SetBits(GPIOB, GPIO_Pin_1);
88
 }
101
 }
89
 
102
 
90
 /**
103
 /**
91
- * @brief Eteint la LED.
104
+ * @brief Eteint les LED.
92
  * @param Aucun
105
  * @param Aucun
93
  */
106
  */
94
-void LEDoff(void)
107
+void ledOff(void)
95
 {
108
 {
96
     GPIO_ResetBits(GPIOB,GPIO_Pin_0);
109
     GPIO_ResetBits(GPIOB,GPIO_Pin_0);
97
     GPIO_ResetBits(GPIOB,GPIO_Pin_1);
110
     GPIO_ResetBits(GPIOB,GPIO_Pin_1);
98
 }
111
 }
99
 
112
 
100
 /**
113
 /**
114
+ * @brief Allume une couleur de LED.
115
+ * @param Aucun
116
+ */
117
+void ledOn(char color)
118
+{
119
+    switch (color)
120
+    {
121
+        case LED_GREEN:
122
+            GPIO_SetBits(GPIOB, GPIO_Pin_1);
123
+            GPIO_ResetBits(GPIOB,GPIO_Pin_0);
124
+            break;
125
+
126
+        case LED_RED:
127
+            GPIO_ResetBits(GPIOB, GPIO_Pin_1);
128
+            GPIO_SetBits(GPIOB,GPIO_Pin_0);
129
+            break;
130
+
131
+        case LED_ORANGE:
132
+            GPIO_SetBits(GPIOB, GPIO_Pin_1);
133
+            GPIO_SetBits(GPIOB,GPIO_Pin_0);
134
+            break;
135
+
136
+        default:
137
+            ledOff();
138
+    }
139
+}
140
+
141
+/**
142
+ * @brief Gere l'etat du clignotement de la led en fonction de l'etat du système
143
+ *        Appelée toutes les 10 ms
144
+ * @param state: état actuel du système
145
+ * @param batteryState: état de la batterie
146
+ */
147
+void ledManagement(States state, char batteryState) {
148
+    static char ledCounter=0;
149
+    char color;
150
+
151
+    if (batteryState>1) color=LED_GREEN;
152
+    else color = LED_RED;
153
+
154
+    switch (state)
155
+    {
156
+        case STATE_IDLE:
157
+            if ((ledCounter<15) || ((ledCounter>=100) && (ledCounter<115))) ledOn(color);
158
+            else ledOff();
159
+            break;
160
+
161
+        case STATE_RUN:
162
+        case STATE_LOW:
163
+            ledOn(color);
164
+            break;
165
+
166
+        case STATE_WATCHDOG_DISABLE:
167
+            if (ledCounter<100) ledOn(LED_RED);
168
+            else ledOff();
169
+            break;
170
+
171
+        case STATE_DISABLE:
172
+        default:
173
+            if (ledCounter%10 ==0) ledOn(LED_RED);
174
+            else ledOff();
175
+    }
176
+
177
+    ledCounter++;
178
+    if (ledCounter>200) ledCounter=0;
179
+}
180
+
181
+//void ledManagement(States state) {
182
+//    if (state == STATE_IDLE) {
183
+//        if (etatLED == 1) {
184
+//            LEDON = 1;
185
+//        } else if (etatLED == 2)
186
+//            LEDON = 0;
187
+//    }
188
+//
189
+//    if (state == STATE_RUN || state == STATE_LOW)
190
+//        LEDON = 1;
191
+//
192
+//    if (state == STATE_DISABLE) {
193
+//        if (etatLED % 2 == 0) ledRedOn();
194
+//        else ledOff();
195
+//    }
196
+//    else if (LEDON) {
197
+//        if (Dumber.stateBattery == 1 && Dumber.StateSystem != STATE_DISABLE)
198
+//            ledOrangeOn();
199
+//        else if (Dumber.stateBattery == 2 && Dumber.StateSystem != STATE_DISABLE)
200
+//            ledGreenOn();
201
+//    } else
202
+//        ledOff();
203
+//}
204
+/**
101
  * @}
205
  * @}
102
  */
206
  */
103
 
207
 

+ 9
- 10
software/robot/src/led.h View File

12
  ******************************************************************************
12
  ******************************************************************************
13
  ******************************************************************************
13
  ******************************************************************************
14
  */
14
  */
15
-#ifndef LED_H
16
-#define LED_H
15
+#ifndef _LED_H_
16
+#define _LED_H_
17
 
17
 
18
 #include "stm32f10x.h"
18
 #include "stm32f10x.h"
19
 
19
 
20
-#define RED	255
20
+#define RED	    255
21
 #define GREEN	250
21
 #define GREEN	250
22
 
22
 
23
-void MAP_LEDpin(void);
24
-void INIT_TIM3Led(void);
25
-void LEDgreen(void);
26
-void LEDred(void);
27
-void LEDoff(void);
28
-void LEDorange(void);
23
+extern char etatLED; // Tout les 200 ms cette variable s'incrémente de 1 jusqu'à 5
24
+extern char LEDON;
29
 
25
 
30
-#endif /* LED_H */
26
+void ledConfigure(void);
27
+void ledManagement(States state, char batteryState);
28
+
29
+#endif /* _LED_H_ */

+ 40
- 223
software/robot/src/main.c View File

5
  * @version V1.0
5
  * @version V1.0
6
  * @date    16-mai-2016
6
  * @date    16-mai-2016
7
  * @brief   Programme principale du robot Dumby.
7
  * @brief   Programme principale du robot Dumby.
8
- *			Dumby est un robot utilis par l'INSA toulouse lors des TPs de temps
9
- *			R�el en 4�me ann�e. Ce fichier est le fichier principal et la
10
- *			derni�re version de son software. Le micro-controleur utilis� est
8
+ *			Dumby est un robot utilisé par l'INSA toulouse lors des TPs de temps
9
+ *			Réel en 4éme année. Ce fichier est le fichier principal et la
10
+ *			dernière version de son software. Le micro-controleur utilisé est
11
  *			un STM32-103-RB.
11
  *			un STM32-103-RB.
12
- *			Il comporte comme fonctionnalite entre autre :
12
+ *			Il comporte comme fonctionnalitée entre autre :
13
  *			- Asservisement des moteurs
13
  *			- Asservisement des moteurs
14
  *			- Detection de tension de batterie
14
  *			- Detection de tension de batterie
15
  *			- Gestion de commande via l'uart
15
  *			- Gestion de commande via l'uart
16
  *			La tache du fichier est de :
16
  *			La tache du fichier est de :
17
- *			- Alimenter les horloges des periphriques necessaires.
17
+ *			- Alimenter les horloges des periphériques necessaires.
18
  *			- Appeller les sous programme necessaire au bon fonctionnement
18
  *			- Appeller les sous programme necessaire au bon fonctionnement
19
  *			  du robot.
19
  *			  du robot.
20
  ******************************************************************************
20
  ******************************************************************************
21
  */
21
  */
22
-#include <battery.h>
22
+
23
 #include <stm32f10x.h>
23
 #include <stm32f10x.h>
24
 #include <string.h>
24
 #include <string.h>
25
 #include <stdio.h>
25
 #include <stdio.h>
26
 
26
 
27
 #include "system_dumby.h"
27
 #include "system_dumby.h"
28
-#include "cmde_usart.h"
29
 #include "motor.h"
28
 #include "motor.h"
30
 #include "led.h"
29
 #include "led.h"
31
-
30
+#include "usart.h"
31
+#include "battery.h"
32
 #include "debug.h"
32
 #include "debug.h"
33
 
33
 
34
-float integration1 = 0;
35
-float integration2 = 0;
36
-/**
37
- * @brief  Coefficient PI
38
- */
39
-const float kp = 15;
40
-/**
41
- * @brief  Coefficient PI
42
- */
43
-const float ki = 1.5;
44
-/**
45
- * @brief  Valeurs de vitesse
46
- */
47
-float motD = 0, motG = 0;
48
-/**
49
- * @brief  erreurs entre vitesse reel et moteurs (droite)
50
- */
51
-int erreurD;
52
-/**
53
- * @brief  erreurs entre vitesse reel et moteurs (gauche)
54
- */
55
-int erreurG;
56
-
57
-char cptMesureHigh=0;
58
-char cptMesureLow=0;
59
-char cptMesureDisable=0;
60
-uint32_t cptMesureEmergencyHalt=0;
61
-
62
-uint16_t vbatLowerVal;
63
-uint16_t vbatHighVal;
64
-uint16_t vbatDiff;
65
-
66
-uint16_t testPostion=0;
67
-uint32_t mesureVoltage;
68
-uint32_t meanVoltage;
69
-
70
-#define COMPTEUR_SEUIL_HIGH		8
71
-#define COMPTEUR_SEUIL_LOW		8
72
-#define COMPTEUR_SEUIL_DISABLE		8
73
-#define COMPTEUR_SEUIL_EMERGENCY_HALT	3000
74
-
75
-void Configure_Clock_Periph(void);
34
+void mainConfigureClock(void);
35
+void mainPeripheralsInit(void);
76
 
36
 
77
 /**
37
 /**
78
- * @brief  Initialise les horloges du micro et de ses p�riph�riques.
38
+ * @brief  Initialise les horloges du micro et de ses périphériques.
79
  * @param  None
39
  * @param  None
80
  * @retval None
40
  * @retval None
81
  */
41
  */
82
 int main(void)
42
 int main(void)
83
 {	
43
 {	
84
-    uint16_t k;
85
     /**
44
     /**
86
      * Initialisation
45
      * Initialisation
87
      */
46
      */
88
-
89
-    Configure_Clock_Periph();
90
-    default_settings();
91
-
92
-    MAP_PinShutDown();
93
-
94
-    MAP_MotorPin();
95
-    MAP_LEDpin();
96
-    MAP_UsartPin();
97
-    MAP_batteryPin();
98
-    INIT_TIM2();
99
-    INIT_OCMotorPwm();
100
-    Configure_SysTick();
101
-
102
-    INIT_USART();
103
-    INIT_IT_UsartReceive();
104
-
105
-    DMA_BAT();
106
-    ADC1_CONFIG();
107
-    INIT_IT_DMA();
108
-    IC_TIM1_CHANEL3();
109
-    IC_TIM1_CHANEL1();
110
-    IT_TIM1();
111
-    GPIO_ResetBits(GPIOA,GPIO_Pin_12); //enable encodeurs
47
+    mainPeripheralsInit();
112
 
48
 
113
     while (1){
49
     while (1){
114
         __WFE(); // Bascule la puce en sleep mode
50
         __WFE(); // Bascule la puce en sleep mode
117
         {
53
         {
118
             Dumber.flagSystick = 0;
54
             Dumber.flagSystick = 0;
119
 
55
 
120
-            if(Dumber.acquisition==VOLTAGE && Dumber.BatterieChecking==TRUE)
121
-            {
122
-                vbatLowerVal = 0xFFF;
123
-                vbatHighVal = 0;
124
-
125
-                for(k=0; k<VOLTAGE_BUFFER_SIZE; k++)
126
-                {
127
-                    meanVoltage+=ADCConvertedValue[k];
128
-
129
-                    if (vbatLowerVal> ADCConvertedValue[k]) vbatLowerVal = ADCConvertedValue[k];
130
-                    if (vbatHighVal< ADCConvertedValue[k]) vbatHighVal = ADCConvertedValue[k];
131
-                }
132
-
133
-                vbatDiff = vbatHighVal - vbatLowerVal;
134
-
135
-                meanVoltage= meanVoltage/VOLTAGE_BUFFER_SIZE;
136
-
137
-                mesureVoltage = meanVoltage;
138
-
139
-                Dumber.BatteryPercentage = mesureVoltage;
140
-                Dumber.acquisition=FALSE;
141
-
142
-                if(Dumber.BatteryPercentage >= VBAT_SEUIL_LOW)
143
-                {
144
-                    cptMesureHigh++;
145
-                    if(cptMesureHigh >= COMPTEUR_SEUIL_HIGH)
146
-                    {
147
-                        if(Dumber.StateSystem == LOW)	Dumber.StateSystem = RUN;
148
-
149
-                        Dumber.stateBattery = 2;
150
-                        cptMesureHigh=0;
151
-                        cptMesureLow=0;
152
-                        cptMesureDisable=0;
153
-                        cptMesureEmergencyHalt=0;
154
-                    }
155
-                }
156
-                else if (Dumber.BatteryPercentage < VBAT_SEUIL_LOW && Dumber.BatteryPercentage >= VBAT_SEUIL_DISABLE)
157
-                {
158
-                    cptMesureLow++;
159
-                    if(cptMesureLow >= COMPTEUR_SEUIL_LOW)
160
-                    {
161
-                        if(Dumber.StateSystem == RUN)	Dumber.StateSystem=LOW;
162
-
163
-                        Dumber.stateBattery =1;
164
-                        cptMesureHigh=0;
165
-                        cptMesureLow=0;
166
-                        cptMesureDisable=0;
167
-                    }
168
-                }
169
-                else // Dumber.BatteryPercentage < VBAT_SEUIL_DISABLE
170
-                {
171
-                    cptMesureDisable++;
172
-
173
-                    if(cptMesureDisable >= COMPTEUR_SEUIL_DISABLE)
174
-                    {
175
-                        Dumber.StateSystem = DISABLE;
176
-                        cptMesureHigh=0;
177
-                        cptMesureLow=0;
178
-                        cptMesureDisable=0;
179
-                        Dumber.stateBattery= 0;
180
-
181
-                        cmdRightMotor(BRAKE,0);
182
-                        cmdLeftMotor(BRAKE,0);
183
-                    }
184
-                }
185
-            }
56
+            /* Gestion des niveaux de batterie */
57
+            batteryManagement();
186
 
58
 
187
-            if (regulation_vitesseD) {
188
-                erreurD = (signed int) G_speedRight - (signed int) tourD;
189
-                motD = kp * erreurD + integration1;
190
-                integration1 += ki * erreurD;
59
+            /* Gestion des moteurs (asservissement, .. */
60
+            motorManagement();
191
 
61
 
192
-                if (motD > 255)	motD = 255;
193
-
194
-                if (motD < 0) motD = 0;
195
-
196
-                motD = (uint16_t) motD;
197
-                majVitesseMotorD(motD);
198
-                tourD = 0;
199
-                regulation_vitesseD = 0;
200
-
201
-                if (G_lapsRight - tourPositionD < 0) {
202
-                    cmdRightMotor(BRAKE, 255);
203
-                }
204
-            }
205
-
206
-            if (regulation_vitesseG) {
207
-                erreurG = (signed int) G_speedLeft - (signed int) tourG;
208
-                motG = kp * erreurG + integration2;
209
-
210
-                integration2 += ki * erreurG;
211
-
212
-                if (motG > 255)	motG = 255;
213
-
214
-                if (motG < 0) motG = 0;
215
-
216
-                motG = (uint16_t) motG;
217
-
218
-                majVitesseMotorG(motG);
219
-                tourG = 0;
220
-                regulation_vitesseG = 0;
221
-
222
-                if (G_lapsLeft - tourPositionG < 0) {
223
-                    cmdLeftMotor(BRAKE, 255);
224
-                }
225
-            }
226
-
227
-            if (G_lapsLeft - tourPositionG < 0 && G_lapsRight - tourPositionD < 0
228
-                    && asservissement == 1) {
229
-
230
-                cmdLeftMotor(BRAKE, 255);
231
-                cmdRightMotor(BRAKE, 255);
232
-                asservissement = 0;
233
-                erreurD = 0;
234
-                erreurG = 0;
235
-                integration1 = 0;
236
-                integration2 = 0;
237
-                Dumber.busyState = FALSE;
238
-                Dumber.cpt_inactivity = 0;
239
-            }
240
-
241
-            if (Dumber.StateSystem == IDLE) {
242
-                if (etatLED == 1) {
243
-                    LEDON = 1;
244
-                } else if (etatLED == 2)
245
-                    LEDON = 0;
246
-            }
247
-
248
-            if (Dumber.StateSystem == DISABLE) {
249
-                if (etatLED % 2 == 0) LEDred();
250
-                else LEDoff();
62
+            /* Gestion du clignotement de la led, f=100Hz*/
63
+            ledManagement(Dumber.StateSystem, Dumber.stateBattery);
251
 
64
 
65
+            if (Dumber.StateSystem == STATE_DISABLE) {
252
                 cptMesureEmergencyHalt++;
66
                 cptMesureEmergencyHalt++;
253
 
67
 
254
-                if (cptMesureEmergencyHalt >= COMPTEUR_SEUIL_EMERGENCY_HALT)
255
-                {
256
-                    shutDown();
68
+                if (cptMesureEmergencyHalt >= COMPTEUR_SEUIL_EMERGENCY_HALT) {
69
+                    systemShutDown();
257
                     while (1);
70
                     while (1);
258
                 }
71
                 }
259
             }
72
             }
260
-
261
-            if (Dumber.StateSystem == RUN || Dumber.StateSystem == LOW)
262
-                LEDON = 1;
263
-
264
-            if (LEDON) {
265
-                if (Dumber.stateBattery == 1 && Dumber.StateSystem != DISABLE)
266
-                    LEDorange();
267
-                if (Dumber.stateBattery == 2 && Dumber.StateSystem != DISABLE)
268
-                    LEDgreen();
269
-            } else
270
-                LEDoff();
271
         }
73
         }
272
     }
74
     }
273
 
75
 
278
 }
80
 }
279
 
81
 
280
 /**
82
 /**
281
- * @brief  Initialise les horloges du micro et de ses p�riph�riques.
83
+ * @brief  Initialise les périphériques pour leur utilisation.
84
+ * @param  None
85
+ * @retval None
86
+ */
87
+void mainPeripheralsInit(void) {
88
+    mainConfigureClock();
89
+
90
+    systemConfigure();
91
+    motorConfigure();
92
+    ledConfigure();
93
+    usartConfigure();
94
+    batteryConfigure();
95
+}
96
+
97
+/**
98
+ * @brief  Initialise les horloges du micro et de ses périphériques.
282
  * @param  None
99
  * @param  None
283
  * @retval None
100
  * @retval None
284
  */
101
  */
285
-void Configure_Clock_Periph(void)
102
+void mainConfigureClock(void)
286
 {
103
 {
287
-    //Configuration de la fr�qyence d'horloge de l'adc */
104
+    //Configuration de la fréquence d'horloge de l'adc */
288
     RCC_ADCCLKConfig(RCC_PCLK2_Div2);
105
     RCC_ADCCLKConfig(RCC_PCLK2_Div2);
289
     RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
106
     RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
290
 
107
 
291
-    //Activation de l'horloge du GPIO, de A B et C, de ADC1, de AFIO
108
+    //Activation de l'horloge du GPIO, de A, B et C, de ADC1, de AFIO
292
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3|RCC_APB1Periph_TIM2|RCC_APB2Periph_USART1, ENABLE);
109
     RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3|RCC_APB1Periph_TIM2|RCC_APB2Periph_USART1, ENABLE);
293
     RCC_APB2PeriphClockCmd( RCC_APB2Periph_ADC1| RCC_APB2Periph_TIM1 |RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
110
     RCC_APB2PeriphClockCmd( RCC_APB2Periph_ADC1| RCC_APB2Periph_TIM1 |RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB |
294
             RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO|RCC_APB2Periph_USART1|RCC_APB2Periph_SPI1,
111
             RCC_APB2Periph_GPIOC | RCC_APB2Periph_AFIO|RCC_APB2Periph_USART1|RCC_APB2Periph_SPI1,

+ 144
- 105
software/robot/src/motor.c View File

14
  * 			- La commandes des moteurs dans leurs 3 modes de fonctionnement (AVANT, ARRIERE, FREIN)
14
  * 			- La commandes des moteurs dans leurs 3 modes de fonctionnement (AVANT, ARRIERE, FREIN)
15
  * 			  et de leurs vitesse (valeur de PWM entre 0 et 255).
15
  * 			  et de leurs vitesse (valeur de PWM entre 0 et 255).
16
  *
16
  *
17
- *@note			TABLE DE VERITEE DU DRIVER MOTEUR
17
+ * @note			TABLE DE VERITEE DU DRIVER MOTEUR
18
  *
18
  *
19
  *	ENABLE 			|			INPUTS			| 		 Moteurs
19
  *	ENABLE 			|			INPUTS			| 		 Moteurs
20
  *					|							|
20
  *					|							|
28
  */
28
  */
29
 
29
 
30
 #include <stm32f10x.h>
30
 #include <stm32f10x.h>
31
-#include "system_dumby.h"
32
 
31
 
32
+#include "system_dumby.h"
33
 #include "motor.h"
33
 #include "motor.h"
34
 
34
 
35
-__IO uint16_t IC1ReadValue1 = 0, IC1ReadValue2 = 0, IC3ReadValue1 = 0, IC3ReadValue2 = 0;
36
-__IO uint16_t CaptureMotor1Nbr = 0, CaptureMotor2Nbr = 0;
37
-__IO uint32_t motor1Capture, motor2Capture = 0;
38
-
39
-TIM_TimeBaseInitTypeDef TIM2_TempsPWMsettings;
40
-TIM_OCInitTypeDef TIM2_Configure;
41
-
42
 /**
35
 /**
43
- * @brief 	Variables Globals nombre d'incr�mentation de capteur de position de la roue droite.
44
- * @note 	tourPositionG correspond exactement � la m�me valeur.
36
+ * @brief 	Variables globales nombre d'incrémentation de capteur de position de la roue droite.
37
+ * @note 	tourPositionG correspond exactement à la même valeur.
45
  */
38
  */
46
 uint16_t tourD = 0;
39
 uint16_t tourD = 0;
47
 
40
 
48
 /**
41
 /**
49
- * @brief 	Variables Globals nombre d'incr�mentation de capteur de position de la roue gauche.
50
- * @note 	tourPositionG correspond exactement � la m�me valeur.
42
+ * @brief 	Variables globales nombre d'incrémentation de capteur de position de la roue gauche.
43
+ * @note 	tourPositionG correspond exactement à la même valeur.
51
  */
44
  */
52
 uint16_t tourG = 0;
45
 uint16_t tourG = 0;
53
 uint16_t tourPositionD;
46
 uint16_t tourPositionD;
54
 uint16_t tourPositionG;
47
 uint16_t tourPositionG;
55
 
48
 
56
 /**
49
 /**
57
- * @brief 	Variables Globals des consignes de vitesses du moteur droit.
58
- * @note 	Variables utilis dans le programme principal main.c
50
+ * @brief 	Variables globales des consignes de vitesses du moteur droit.
51
+ * @note 	Variables utilisées dans le programme principal main.c
59
  */
52
  */
60
 uint16_t G_speedRight=20;
53
 uint16_t G_speedRight=20;
61
 
54
 
62
 /**
55
 /**
63
- * @brief 	Variables Globals des consignes de vitesses du moteur gauche.
64
- * @note 	Variables utilis dans le programme principal main.c
56
+ * @brief 	Variables globales des consignes de vitesses du moteur gauche.
57
+ * @note 	Variables utilisées dans le programme principal main.c
65
  */
58
  */
66
 uint16_t G_speedLeft=20;
59
 uint16_t G_speedLeft=20;
67
 
60
 
68
 /**
61
 /**
69
- * @brief 	Variables Globals des consignes de position du moteur gauche.
70
- * @note 	Variables utilis dans le programme principal main.c
62
+ * @brief 	Variables globales des consignes de position du moteur gauche.
63
+ * @note 	Variables utilisées dans le programme principal main.c
71
  */
64
  */
72
 uint16_t G_lapsLeft;
65
 uint16_t G_lapsLeft;
73
 
66
 
74
 /**
67
 /**
75
- * @brief 	Variables Globals des consignes de position du moteur droit.
76
- * @note 	Variables utilis dans le programme principal main.c
68
+ * @brief 	Variables globales des consignes de position du moteur droit.
69
+ * @note 	Variables utilisées dans le programme principal main.c
77
  */
70
  */
78
 uint16_t G_lapsRight;
71
 uint16_t G_lapsRight;
79
 
72
 
73
+float integration1 = 0;
74
+float integration2 = 0;
75
+/**
76
+ * @brief  Coefficient PI
77
+ */
78
+const float kp = 15;
79
+/**
80
+ * @brief  Coefficient PI
81
+ */
82
+const float ki = 1.5;
83
+/**
84
+ * @brief  Valeurs de vitesse
85
+ */
86
+float motD = 0, motG = 0;
87
+/**
88
+ * @brief  erreurs entre vitesse réelle et moteur (droite)
89
+ */
90
+int erreurD;
91
+
92
+/**
93
+ * @brief  erreurs entre vitesse réelle et moteur (gauche)
94
+ */
95
+int erreurG;
96
+
97
+uint16_t asservissement;
98
+uint16_t regulation_vitesseD, regulation_vitesseG;
80
 /** @addtogroup Projects
99
 /** @addtogroup Projects
81
  * @{
100
  * @{
82
  */
101
  */
90
  */
109
  */
91
 
110
 
92
 /**
111
 /**
93
- * @brief 		Assigne et d�finis le GPIO necessaire pour le moteur.
112
+ * @brief 		Assigne et définis le GPIO nécessaire pour le moteur.
94
  *
113
  *
95
  * 				La fonction MAP_MotorPin va venir configurer le E/S du GPIO pour correspondre avec
114
  * 				La fonction MAP_MotorPin va venir configurer le E/S du GPIO pour correspondre avec
96
- * 				le sch�ma electrique en ressource.
115
+ * 				le schéma électrique en ressource.
97
  * @note			2 pwm en alternate fonction : PA1,PA2. 4 entr�e timer.
116
  * @note			2 pwm en alternate fonction : PA1,PA2. 4 entr�e timer.
98
  *				PA8,PA9,PA10,PA11. 4 sortie ppull PB12,PB13,PB14,PB15
117
  *				PA8,PA9,PA10,PA11. 4 sortie ppull PB12,PB13,PB14,PB15
99
  *
118
  *
101
  * @retval 		None
120
  * @retval 		None
102
  *
121
  *
103
  */
122
  */
104
-void MAP_MotorPin(void)
123
+void motorConfigure(void)
105
 {
124
 {
106
-    // Variable local necessaire � l'initialisation des structures
107
     GPIO_InitTypeDef Init_Structure;
125
     GPIO_InitTypeDef Init_Structure;
126
+    TIM_TimeBaseInitTypeDef TIM2_TempsPWMsettings;
127
+    TIM_OCInitTypeDef TIM2_Configure;
128
+    TIM_ICInitTypeDef  TIM_ICInitStructure;
129
+    NVIC_InitTypeDef NVIC_InitStructure;
108
 
130
 
109
     // Configure les PIN A1 et A2 en output / alternate fonction
131
     // Configure les PIN A1 et A2 en output / alternate fonction
110
     Init_Structure.GPIO_Pin = GPIO_Pin_1|GPIO_Pin_2;
132
     Init_Structure.GPIO_Pin = GPIO_Pin_1|GPIO_Pin_2;
130
     Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
152
     Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
131
     Init_Structure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
153
     Init_Structure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
132
     GPIO_Init(GPIOA, &Init_Structure);
154
     GPIO_Init(GPIOA, &Init_Structure);
133
-}
134
 
155
 
135
-/**
136
- * @brief 	La fonction INIT_TIM2 initialise le Timer2 � une fr�quence de 25kHz (fc nominal) et initialise
137
- *			les pwm des moteurs � un dutycycle � 0% de 255
138
- *
139
- * @param  None
140
- * @retval None
141
- */
142
-void INIT_TIM2(void)
143
-{
144
-    // On souhaite une r�solution du PWM de 256 valeurs MOTOR1 TIM2
156
+    // Configuration du timer 2 (pwm moteurs*
157
+    // On souhaite une résolution du PWM de 256 valeurs MOTOR1 TIM2
145
     TIM2_TempsPWMsettings.TIM_Period = 255;
158
     TIM2_TempsPWMsettings.TIM_Period = 255;
146
     TIM2_TempsPWMsettings.TIM_Prescaler = 0;
159
     TIM2_TempsPWMsettings.TIM_Prescaler = 0;
147
     TIM2_TempsPWMsettings.TIM_ClockDivision=0;
160
     TIM2_TempsPWMsettings.TIM_ClockDivision=0;
148
     TIM2_TempsPWMsettings.TIM_CounterMode=TIM_CounterMode_Up;
161
     TIM2_TempsPWMsettings.TIM_CounterMode=TIM_CounterMode_Up;
149
 
162
 
150
     TIM_TimeBaseInit(TIM2, &TIM2_TempsPWMsettings);
163
     TIM_TimeBaseInit(TIM2, &TIM2_TempsPWMsettings);
151
-}
152
 
164
 
153
-/**
154
- * @brief 	Initialise les channels
155
- *
156
- * 			Initialise le chanel 2 et 3 du TM2 pour g�rer les moteurs
157
- *
158
- * @param  None
159
- * @retval None
160
- */
161
-void INIT_OCMotorPwm(void)
162
-{
163
     // Configuration du PWM sur le timer 2
165
     // Configuration du PWM sur le timer 2
164
     TIM2_Configure.TIM_OCMode=TIM_OCMode_PWM2;
166
     TIM2_Configure.TIM_OCMode=TIM_OCMode_PWM2;
165
     TIM2_Configure.TIM_OutputState = TIM_OutputState_Enable;
167
     TIM2_Configure.TIM_OutputState = TIM_OutputState_Enable;
166
     TIM2_Configure.TIM_OutputNState = TIM_OutputNState_Enable;
168
     TIM2_Configure.TIM_OutputNState = TIM_OutputNState_Enable;
167
-    TIM2_Configure.TIM_Pulse = 256; // Constante initialis� � 256, Pour un rapport cyclique nul
169
+    TIM2_Configure.TIM_Pulse = 256; // Constante initialisée à 256, pour un rapport cyclique nul
168
     TIM2_Configure.TIM_OCPolarity = TIM_OCPolarity_High;
170
     TIM2_Configure.TIM_OCPolarity = TIM_OCPolarity_High;
169
 
171
 
170
     TIM_OC3Init(TIM2, &TIM2_Configure);
172
     TIM_OC3Init(TIM2, &TIM2_Configure);
178
     TIM_Cmd(TIM2, ENABLE);
180
     TIM_Cmd(TIM2, ENABLE);
179
 
181
 
180
     TIM_CtrlPWMOutputs(TIM2,ENABLE);
182
     TIM_CtrlPWMOutputs(TIM2,ENABLE);
181
-}
182
 
183
 
183
-/**
184
- * @brief 	Initialise le channel 1 pour le retour des capteurs de position
185
- * 			de la roue droite.
186
- *
187
- * @param  None
188
- * @retval None
189
- */
190
-void IC_TIM1_CHANEL1(void)
191
-{
192
-    TIM_ICInitTypeDef  TIM_ICInitStructure;
184
+    // Configuration de la capture de l'encodeur 1
193
     TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
185
     TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
194
     TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
186
     TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
195
     TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
187
     TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
199
     TIM_ICInit(TIM1, &TIM_ICInitStructure);
191
     TIM_ICInit(TIM1, &TIM_ICInitStructure);
200
     TIM_Cmd(TIM1, ENABLE);
192
     TIM_Cmd(TIM1, ENABLE);
201
     TIM_ITConfig(TIM1, TIM_IT_CC1, ENABLE);
193
     TIM_ITConfig(TIM1, TIM_IT_CC1, ENABLE);
202
-}
203
 
194
 
204
-/**
205
- * @brief 	Initialise le channel 3 pour le retour des capteurs de position
206
- * 			de la roue gauche.
207
- *
208
- * @param  None
209
- * @retval None
210
- */
211
-void IC_TIM1_CHANEL3(void)
212
-{
213
-    TIM_ICInitTypeDef  TIM_ICInitStructure;
195
+    // Configuration de la capture de l'encodeur 1
214
     TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
196
     TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
215
     TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
197
     TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
216
     TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
198
     TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
220
     TIM_ICInit(TIM1, &TIM_ICInitStructure);
202
     TIM_ICInit(TIM1, &TIM_ICInitStructure);
221
     TIM_Cmd(TIM1, ENABLE);
203
     TIM_Cmd(TIM1, ENABLE);
222
     TIM_ITConfig(TIM1, TIM_IT_CC3, ENABLE);
204
     TIM_ITConfig(TIM1, TIM_IT_CC3, ENABLE);
223
-}
224
 
205
 
225
-/**
226
- * @brief 	D�finis une interuptions sur le Timer1.
227
- *
228
- * @param  None
229
- * @retval None
230
- */
231
-void IT_TIM1(void)
232
-{
233
-    NVIC_InitTypeDef NVIC_InitStructure;
234
-
235
-    /* Enable the TIM1 global Interrupt */
206
+    // Enable the TIM1 Capture interrupt
236
     NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
207
     NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
237
     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
208
     NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
238
     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
209
     NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
239
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
210
     NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
240
     NVIC_Init(&NVIC_InitStructure);
211
     NVIC_Init(&NVIC_InitStructure);
212
+
213
+    // RAZ des variables
214
+    regulation_vitesseD = 0;
215
+    regulation_vitesseG = 0;
216
+    asservissement =0;
241
 }
217
 }
242
 
218
 
243
 /**
219
 /**
248
  * @{
224
  * @{
249
  */
225
  */
250
 /**
226
 /**
251
- * @brief  Commande de plus haut niveau pour contrler le moteur droit.
227
+ * @brief  Commande de plus haut niveau pour contrôler le moteur droit.
252
  *
228
  *
253
- * @param  Mode de fonctionnement du moteur, peut �tre �gal aux constantes BRAKE, FORWARD, REVERSE d�finis dans moteur.h
254
- * @param  Consigne de vitesse du moteur, d�finis par un pwm entre 0 et 255.
229
+ * @param  Mode de fonctionnement du moteur, peut être égal aux constantes BRAKE, FORWARD, REVERSE définis dans moteur.h
230
+ * @param  Consigne de vitesse du moteur, défini par un pwm entre 0 et 255.
255
  * @retval None
231
  * @retval None
256
  */					
232
  */					
257
 
233
 
258
-void cmdRightMotor(char mod, uint16_t pwm)
234
+void motorCmdRight(char mod, uint16_t pwm)
259
 {
235
 {
260
     pwm = 256 - pwm;
236
     pwm = 256 - pwm;
237
+
261
     switch (mod) {
238
     switch (mod) {
262
         case BRAKE:
239
         case BRAKE:
263
             GPIO_SetBits(GPIOB, GPIO_Pin_12);
240
             GPIO_SetBits(GPIOB, GPIO_Pin_12);
281
 }
258
 }
282
 
259
 
283
 /**
260
 /**
284
- * @brief  Commande de plus haut niveau pour contrler le moteur Gauche.
261
+ * @brief  Commande de plus haut niveau pour contrôler le moteur Gauche.
285
  *
262
  *
286
- * @param  Mode de fonctionnement du moteur, peut �tre �gal aux constantes BRAKE, FORWARD, REVERSE d�finis dans moteur.h
287
- * @param  Consigne de vitesse du moteur, d�finis par un pwm entre 0 et 255.
263
+ * @param  Mode de fonctionnement du moteur, peut être égal aux constantes BRAKE, FORWARD, REVERSE définis dans moteur.h
264
+ * @param  Consigne de vitesse du moteur, défini par un pwm entre 0 et 255.
288
  * @retval None
265
  * @retval None
289
  */
266
  */
290
 
267
 
291
-void cmdLeftMotor(char mod, uint16_t pwm) {
268
+void motorCmdLeft(char mod, uint16_t pwm) {
292
     pwm = 256 - pwm;
269
     pwm = 256 - pwm;
293
     switch (mod) {
270
     switch (mod) {
294
         case BRAKE:
271
         case BRAKE:
313
 }
290
 }
314
 
291
 
315
 /**
292
 /**
316
- * @brief  Commande de plus haut niveau pour contrler la vitesse moteur Gauche.
293
+ * @brief  Commande de plus haut niveau pour contrôler la vitesse moteur Gauche.
317
  *
294
  *
318
- * @param  Consigne de vitesse du moteur, d�finis par un pwm entre 0 et 255.
295
+ * @param  Consigne de vitesse du moteur, défini par un pwm entre 0 et 255.
319
  * @retval None
296
  * @retval None
320
  */
297
  */
321
-void majVitesseMotorG(uint16_t pwm) {
298
+void motorSpeedUpdateLeft(uint16_t pwm) {
322
     pwm = 256 - pwm;
299
     pwm = 256 - pwm;
323
     TIM_SetCompare2(TIM2, pwm);
300
     TIM_SetCompare2(TIM2, pwm);
324
 }
301
 }
325
 
302
 
326
 /**
303
 /**
327
- * @brief  Commande de plus haut niveau pour contrler la vitesse moteur Droit.
304
+ * @brief  Commande de plus haut niveau pour contrôler la vitesse moteur Droit.
328
  *
305
  *
329
- * @param  Consigne de vitesse du moteur, d�finis par un pwm entre 0 et 255.
306
+ * @param  Consigne de vitesse du moteur, défini par un pwm entre 0 et 255.
330
  * @retval None
307
  * @retval None
331
  */
308
  */
332
-void majVitesseMotorD(uint16_t pwm) {
309
+void motorSpeedUpdateRight(uint16_t pwm) {
333
     pwm = 256 - pwm;
310
     pwm = 256 - pwm;
334
     TIM_SetCompare3(TIM2, pwm);
311
     TIM_SetCompare3(TIM2, pwm);
335
 }
312
 }
336
 
313
 
337
 /**
314
 /**
338
- * @brief  Regulation des deux moteurs.
315
+ * @brief  Régulation des deux moteurs.
339
  *
316
  *
340
- * 		   Modifile le mode de fonctionnement des roues, puis
341
- * 		   va mettre  jour les variables globale G_laps* et G_speed*
342
- * 		   utilis dans main.c pour l'asservissement.
317
+ * 		   Modifie le le mode de fonctionnement des roues, puis
318
+ * 		   va mettre à jour les variables globale G_laps* et G_speed*
319
+ * 		   utilisé dans main.c pour l'asservissement.
343
  *
320
  *
344
  * @param  Mode de fonctionnement la roue droite (BRAKE, REVERSE, FORWARD)
321
  * @param  Mode de fonctionnement la roue droite (BRAKE, REVERSE, FORWARD)
345
  * @param  Mode de fonctionnement la roue droite (BRAKE, REVERSE, FORWARD)
322
  * @param  Mode de fonctionnement la roue droite (BRAKE, REVERSE, FORWARD)
346
- * @param  Nombre de tour de roue droite  effectuer.
347
- * @param  Nombre de tour de roue gauche  effectuer.
323
+ * @param  Nombre de tour de roue droite à effectuer.
324
+ * @param  Nombre de tour de roue gauche à effectuer.
348
  * @param  Vitesse de la roue de droite.
325
  * @param  Vitesse de la roue de droite.
349
  * @param  Vitesse de la roue de gauche.
326
  * @param  Vitesse de la roue de gauche.
350
  *
327
  *
351
  * @retval None
328
  * @retval None
352
  */
329
  */
353
-void regulationMoteur(char modRight, char modLeft, uint16_t lapsRight,
330
+void motorRegulation(char modRight, char modLeft, uint16_t lapsRight,
354
         uint16_t lapsLeft, uint16_t speedRight, uint16_t speedLeft) {
331
         uint16_t lapsLeft, uint16_t speedRight, uint16_t speedLeft) {
355
     /*Moteur Droit*/
332
     /*Moteur Droit*/
356
     switch (modRight) {
333
     switch (modRight) {
411
 /**
388
 /**
412
  * @brief  Gestion de l'interuption des capteurs de positions.
389
  * @brief  Gestion de l'interuption des capteurs de positions.
413
  *
390
  *
414
- *		   Fonctin de callback permettant d'incr�menter le nombre
391
+ *		   Fonction de callback permettant d'incrémenter le nombre
415
  *		   de tour des deux roues.
392
  *		   de tour des deux roues.
416
  *
393
  *
417
  * @param  None
394
  * @param  None
422
         TIM_ClearITPendingBit(TIM1, TIM_IT_CC1);
399
         TIM_ClearITPendingBit(TIM1, TIM_IT_CC1);
423
         tourD++;
400
         tourD++;
424
         tourPositionD++;
401
         tourPositionD++;
425
-
426
     }
402
     }
427
 
403
 
428
     if (TIM_GetITStatus(TIM1, TIM_IT_CC3) == SET) {
404
     if (TIM_GetITStatus(TIM1, TIM_IT_CC3) == SET) {
431
         tourPositionG++;
407
         tourPositionG++;
432
     }
408
     }
433
 }
409
 }
410
+
411
+/**
412
+ * @brief  Gestion de l'asservissement des moteurs
413
+ *
414
+ * @param  None
415
+ * @retval None
416
+ */
417
+void motorManagement(void) {
418
+    if (regulation_vitesseD) {
419
+        erreurD = (signed int) G_speedRight - (signed int) tourD;
420
+        motD = kp * erreurD + integration1;
421
+        integration1 += ki * erreurD;
422
+
423
+        if (motD > 255) motD = 255;
424
+
425
+        if (motD < 0) motD = 0;
426
+
427
+        motD = (uint16_t) motD;
428
+        motorSpeedUpdateRight(motD);
429
+        tourD = 0;
430
+        regulation_vitesseD = 0;
431
+
432
+        if (G_lapsRight - tourPositionD < 0) {
433
+            motorCmdRight(BRAKE, 255);
434
+        }
435
+    }
436
+
437
+    if (regulation_vitesseG) {
438
+        erreurG = (signed int) G_speedLeft - (signed int) tourG;
439
+        motG = kp * erreurG + integration2;
440
+
441
+        integration2 += ki * erreurG;
442
+
443
+        if (motG > 255) motG = 255;
444
+
445
+        if (motG < 0) motG = 0;
446
+
447
+        motG = (uint16_t) motG;
448
+
449
+        motorSpeedUpdateLeft(motG);
450
+        tourG = 0;
451
+        regulation_vitesseG = 0;
452
+
453
+        if (G_lapsLeft - tourPositionG < 0) {
454
+            motorCmdLeft(BRAKE, 255);
455
+        }
456
+    }
457
+
458
+    if (G_lapsLeft - tourPositionG < 0 && G_lapsRight - tourPositionD < 0
459
+            && asservissement == 1) {
460
+
461
+        motorCmdLeft(BRAKE, 255);
462
+        motorCmdRight(BRAKE, 255);
463
+        asservissement = 0;
464
+        erreurD = 0;
465
+        erreurG = 0;
466
+        integration1 = 0;
467
+        integration2 = 0;
468
+        Dumber.busyState = FALSE;
469
+        Dumber.cpt_inactivity = 0;
470
+    }
471
+}
472
+
434
 /**
473
 /**
435
  * @}
474
  * @}
436
  */
475
  */

+ 15
- 23
software/robot/src/motor.h View File

22
  *		0			|				X	| Roue Libre
22
  *		0			|				X	| Roue Libre
23
  *--------------------------------------------------------------------
23
  *--------------------------------------------------------------------
24
  *					|  CMDA & !CMDB 			| Avant
24
  *					|  CMDA & !CMDB 			| Avant
25
- *		1			|  !CMDA & CMDB				| Arriére
25
+ *		1			|  !CMDA & CMDB				| Arrière
26
  *					|  CMDA = CMDB				| Frein
26
  *					|  CMDA = CMDB				| Frein
27
  ******************************************************************************
27
  ******************************************************************************
28
  ******************************************************************************
28
  ******************************************************************************
29
  */
29
  */
30
-#ifndef Motor_H
31
-#define Motor_H
30
+#ifndef _MOTOR_H_
31
+#define _MOTOR_H_
32
 
32
 
33
 #include "stm32f10x.h"
33
 #include "stm32f10x.h"
34
-//#include "system_dumby.h"
35
 
34
 
36
 extern uint16_t G_speedRight, G_speedLeft, G_lapsLeft, G_lapsRight;
35
 extern uint16_t G_speedRight, G_speedLeft, G_lapsLeft, G_lapsRight;
37
 extern uint16_t tourG, tourD, tourPositionD,tourPositionG;
36
 extern uint16_t tourG, tourD, tourPositionD,tourPositionG;
38
 
37
 
38
+extern uint16_t asservissement;
39
+extern uint16_t regulation_vitesseD, regulation_vitesseG;
39
 /**
40
 /**
40
- * @brief Etats que peuvent prendre chaques moteurs
41
- * 		  Avant, Arriére, Frein
41
+ * @brief États que peuvent prendre chaque moteurs
42
+ * 		  Avant, Arrière, Frein
42
  */
43
  */
43
 #define FORWARD 11  
44
 #define FORWARD 11  
44
 #define REVERSE 12 
45
 #define REVERSE 12 
45
 #define BRAKE 	13 
46
 #define BRAKE 	13 
46
 
47
 
47
-void MAP_MotorPin(void);
48
-void INIT_TIM2(void);
49
-void INIT_OCMotorPwm(void);
50
-void commandeMoteur(void);
48
+void motorConfigure(void);
49
+void motorManagement(void);
51
 
50
 
52
-void IC_TIM1_CHANEL3(void);
53
-void IC_TIM1_CHANEL1(void);
54
-void IT_TIM1(void);
51
+void motorCmdRight(char mod, uint16_t pwm);
52
+void motorCmdLeft(char mod, uint16_t pwm);
53
+void motorSpeedUpdateLeft(uint16_t pwm);
54
+void motorSpeedUpdateRight(uint16_t pwm);
55
+void motorRegulation(char modRight, char modLeft, uint16_t lapsRight, uint16_t lapsLeft, uint16_t speedRight, uint16_t speedLeft);
55
 
56
 
56
-void cmdRightMotor(char mod, uint16_t pwm);
57
-void cmdLeftMotor(char mod, uint16_t pwm);
58
-
59
-void majVitesseMotorG(uint16_t pwm);
60
-void majVitesseMotorD(uint16_t pwm);
61
-
62
-void regulationMoteur(char modRight, char modLeft, uint16_t lapsRight, uint16_t lapsLeft, uint16_t speedRight, uint16_t speedLeft);
63
-//void regulationCBK(void);
64
-
65
-#endif /* Motor_H */
57
+#endif /* _MOTOR_H_ */
66
 
58
 

+ 132
- 76
software/robot/src/system_dumby.c View File

27
  ******************************************************************************
27
  ******************************************************************************
28
  ******************************************************************************
28
  ******************************************************************************
29
  */
29
  */
30
-#include <battery.h>
31
-#include "system_dumby.h"
32
 #include <stm32f10x.h>
30
 #include <stm32f10x.h>
33
-#include "led.h"
34
 #include <string.h>
31
 #include <string.h>
35
 #include <stdio.h>
32
 #include <stdio.h>
33
+
34
+#include "system_dumby.h"
35
+#include "battery.h"
36
+#include "led.h"
36
 #include "motor.h"
37
 #include "motor.h"
37
-#include "cmde_usart.h"
38
+#include "usart.h"
38
 
39
 
39
 Settings Dumber;
40
 Settings Dumber;
40
-static __IO uint32_t TimingBattery=10000;
41
-uint16_t greenLight = 0;
42
-uint16_t redLight = 0;
43
-uint16_t regulation_vitesseD = 0, regulation_vitesseG = 0, asservissement =0;
44
-char etatLED = 1;
45
-uint16_t watchDogState;;
46
-char vClignotement1s=0;
47
-uint16_t tourPositionGprec =1, tourPositionDprec=1;
48
-char LEDON =0;
49
-
50
-int led=0;
51
 
41
 
52
 /** @addtogroup Projects
42
 /** @addtogroup Projects
53
  * @{
43
  * @{
57
  * @{
47
  * @{
58
  */
48
  */
59
 
49
 
60
-
61
-
62
-/** @addtogroup init shutdown_gpio init
63
- * @{
64
- */
65
-
66
 /**
50
 /**
67
- * @brief 		Assigne et défini le GPIO necessaire pour la gestion du shutdown.
68
- *
69
- * @param  		None
70
- * @retval 		None
51
+ *@brief	Configure les propriété de dumber au démarage :
52
+ *			Etat de depart		: IDLE
53
+ *			Liaison série		: USART
54
+ *@param 	None
55
+ *@retval 	None
71
  */
56
  */
72
-void MAP_PinShutDown(void)
57
+void systemConfigure(void)
73
 {
58
 {
74
     GPIO_InitTypeDef Init_Structure;
59
     GPIO_InitTypeDef Init_Structure;
60
+
61
+    // Configure le systick pour générer des interruptions toutes les 10ms.
62
+    SysTick_Config(SystemCoreClock / 100); //configuration du systick à 10ms
63
+
64
+    // Assigne et défini le GPIO necessaire pour la gestion du shutdown.
75
     Init_Structure.GPIO_Pin = GPIO_Pin_5;
65
     Init_Structure.GPIO_Pin = GPIO_Pin_5;
76
     Init_Structure.GPIO_Mode = GPIO_Mode_Out_PP;
66
     Init_Structure.GPIO_Mode = GPIO_Mode_Out_PP;
77
     Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
67
     Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
78
     GPIO_SetBits(GPIOB, GPIO_Pin_5);
68
     GPIO_SetBits(GPIOB, GPIO_Pin_5);
79
     GPIO_Init(GPIOB, &Init_Structure);
69
     GPIO_Init(GPIOB, &Init_Structure);
80
-}
81
-
82
-/**
83
- * @}
84
- */
85
 
70
 
86
-/**
87
- *@brief	Configure les propriété de dumber au démarage :
88
- *			Etat de depart		: IDLE
89
- *			Liaison série		: USART
90
- *@param 	None
91
- *@retval 	None
92
- */
93
-void default_settings(void)
94
-{
71
+    // Initialise la structure system Dumber
95
     Dumber.BatteryPercentage = UNDEFINED;
72
     Dumber.BatteryPercentage = UNDEFINED;
96
     Dumber.BatteryCurrent = UNDEFINED;
73
     Dumber.BatteryCurrent = UNDEFINED;
97
     // Retourne le pourcentage de charge de la batterie
74
     // Retourne le pourcentage de charge de la batterie
98
-    Dumber.StateSystem = IDLE;								// Etat de la MAE
75
+    Dumber.StateSystem = STATE_IDLE;								// État de la MAE
99
     Dumber.AddOn = FALSE;							// Un AddOn a été détecté
76
     Dumber.AddOn = FALSE;							// Un AddOn a été détecté
100
-    Dumber.BatterieChecking = FALSE; // On doit verifier la valeur de la batterie
77
+    Dumber.BatterieChecking = FALSE; // On doit vérifier la valeur de la batterie
101
     Dumber.WatchDogStartEnable = FALSE;	// Le Robot a été lancé en mode WithWatchDog ou WithoutWatchDog
78
     Dumber.WatchDogStartEnable = FALSE;	// Le Robot a été lancé en mode WithWatchDog ou WithoutWatchDog
79
+    Dumber.InvalidWatchdogResetCpt=0;
102
     Dumber.cpt_watchdog = 0;
80
     Dumber.cpt_watchdog = 0;
103
     Dumber.cpt_systick = 0;
81
     Dumber.cpt_systick = 0;
104
     Dumber.cpt_inactivity = 0;
82
     Dumber.cpt_inactivity = 0;
108
 }
86
 }
109
 
87
 
110
 /**
88
 /**
111
- *@brief	Configure le systick pour générer des interruptions toutes les 50ms.
112
- *
113
- *@param 	None
114
- *@retval 	None
115
- */
116
-void Configure_SysTick(void) {
117
-    SysTick_Config(SystemCoreClock / 100); //configuration du systick à 50ms
118
-}
119
-
120
-/**
121
  *	@brief Désactive les interruptions et entre dans une boucle while (1) en attendant l'extinction du CPU.
89
  *	@brief Désactive les interruptions et entre dans une boucle while (1) en attendant l'extinction du CPU.
122
  *	@param None
90
  *	@param None
123
  */
91
  */
124
 
92
 
125
-void shutDown(void) {
93
+void systemShutDown(void) {
126
     __disable_irq();
94
     __disable_irq();
127
     GPIO_ResetBits(GPIOB, GPIO_Pin_5);
95
     GPIO_ResetBits(GPIOB, GPIO_Pin_5);
128
     while (1);
96
     while (1);
129
 }
97
 }
130
 
98
 
99
+/**
100
+ * @brief Bascule le système dans un état.
101
+ *
102
+ * @param    state: Nouvel état
103
+ * @retval   None
104
+ */
105
+void systemChangeState(States state) {
106
+    switch (state)
107
+    {
108
+        case STATE_IDLE:
109
+            Dumber.StateSystem = STATE_IDLE;
110
+            Dumber.WatchDogStartEnable = FALSE;
111
+            Dumber.cpt_watchdog = 0;
112
+            Dumber.InvalidWatchdogResetCpt=0;
113
+            Dumber.cpt_systick = 0;
114
+            GPIO_SetBits(GPIOA,GPIO_Pin_12); // Désactive les encodeurs
115
+            motorCmdLeft(BRAKE, 0);
116
+            motorCmdRight(BRAKE, 0);
117
+            break;
118
+
119
+        case STATE_RUN:
120
+            Dumber.StateSystem=STATE_RUN;
121
+            GPIO_ResetBits(GPIOA,GPIO_Pin_12); // Active les encodeurs
122
+            Dumber.cpt_watchdog=0;
123
+            break;
124
+
125
+        case STATE_LOW:
126
+            Dumber.StateSystem=STATE_LOW;
127
+            GPIO_ResetBits(GPIOA,GPIO_Pin_12); // Active les encodeurs
128
+            Dumber.cpt_watchdog=0;
129
+            break;
130
+
131
+        case STATE_DISABLE:
132
+            Dumber.StateSystem=STATE_DISABLE;
133
+            GPIO_SetBits(GPIOA,GPIO_Pin_12); // Désactive les encodeurs
134
+            motorCmdRight(BRAKE,0);
135
+            motorCmdLeft(BRAKE,0);
136
+            Dumber.WatchDogStartEnable = FALSE;
137
+
138
+            cptMesureHigh=0;
139
+            cptMesureLow=0;
140
+            cptMesureDisable=0;
141
+            Dumber.stateBattery= 0;
142
+            break;
143
+
144
+        case STATE_WATCHDOG_DISABLE:
145
+            Dumber.StateSystem=STATE_WATCHDOG_DISABLE;
146
+            GPIO_SetBits(GPIOA,GPIO_Pin_12); // Désactive les encodeurs
147
+            motorCmdRight(BRAKE,0);
148
+            motorCmdLeft(BRAKE,0);
149
+            Dumber.WatchDogStartEnable = FALSE;
150
+            break;
151
+
152
+        default:
153
+            /* Unknown state -> go into DISABLE */
154
+            systemChangeState(STATE_DISABLE);
155
+    }
156
+}
157
+
158
+/**
159
+ * @brief   Remise à zéro du watchdog, en fonction de l'état en cours.
160
+ *
161
+ * @param   None
162
+ * @retval  None
163
+ */
164
+char systemResetWatchdog(void) {
165
+    char resultat =0;
166
+
167
+    if ((Dumber.StateSystem == STATE_RUN) || (Dumber.StateSystem == STATE_LOW)) { // si on est actif
168
+        if (Dumber.WatchDogStartEnable == TRUE) { // si le watchdog est lancé
169
+            if ((Dumber.cpt_watchdog >= WATCHDOG_MIN) && (Dumber.cpt_watchdog <= WATCHDOG_MAX)) { // si le watchdog est dans sa plage réarmable
170
+
171
+                Dumber.InvalidWatchdogResetCpt=0; // on remet le compteur d'erreur à zéro
172
+            } else {
173
+                Dumber.InvalidWatchdogResetCpt++; // on incrémente le compteur d'erreur
174
+                if (Dumber.InvalidWatchdogResetCpt>WATCHDOG_INVALID_COUNTER_MAX) // on a atteint le max d'erreur possible
175
+                    systemChangeState(STATE_WATCHDOG_DISABLE); // le système est désactivé
176
+            }
177
+
178
+            Dumber.cpt_watchdog = 0; // on remet le watchdog à zéro
179
+
180
+            resultat=1;
181
+        }
182
+    }
183
+
184
+    return resultat;
185
+}
131
 
186
 
132
 /** @addtogroup Gestion Gestions compteurs systick
187
 /** @addtogroup Gestion Gestions compteurs systick
133
  * @{
188
  * @{
136
 
191
 
137
 /**
192
 /**
138
  * @brief Interruption systick. Gestion des timers internes.
193
  * @brief Interruption systick. Gestion des timers internes.
139
- *		  Definis la vitesse de la loi d'asservissement.
140
- *		  Definis les valeurs de shutdown.
141
- *		  Definis les valeurs d'inactivit�s.
142
- *		  Definis la valeur de clignotement de la LED.
194
+ *		  Défini la vitesse de la loi d'asservissement.
195
+ *		  Défini les valeurs de shutdown.
196
+ *		  Défini les valeurs d'inactivités.
197
+ *		  Défini la valeur de clignotement de la LED.
143
  *
198
  *
144
- * @note  Tout ces temps sont calcul sous base du systick.
199
+ * @note  Tout ces temps sont calculés sous base du systick.
145
  */
200
  */
146
 void SysTick_Handler(void){
201
 void SysTick_Handler(void){
147
     Dumber.cpt_systick+=10;
202
     Dumber.cpt_systick+=10;
148
     Dumber.cpt_inactivity+=10;
203
     Dumber.cpt_inactivity+=10;
149
 
204
 
150
-    if((Dumber.WatchDogStartEnable == TRUE) && (Dumber.StateSystem != IDLE)) Dumber.cpt_watchdog+=10;
151
-
152
-
153
     if(asservissement == 1){
205
     if(asservissement == 1){
154
         regulation_vitesseD =1;
206
         regulation_vitesseD =1;
155
         regulation_vitesseG =1;
207
         regulation_vitesseG =1;
156
     }
208
     }
157
     if(Dumber.cpt_systick % TIMER_1s==0) Dumber.cpt_systick=0;
209
     if(Dumber.cpt_systick % TIMER_1s==0) Dumber.cpt_systick=0;
158
 
210
 
159
-    if((Dumber.cpt_watchdog % 1000 ) >=975 || (Dumber.cpt_watchdog % 1000) <=25)
160
-        watchDogState=TRUE;
161
-    else watchDogState=FALSE;
211
+    /* Gestion du watchdog */
212
+    if((Dumber.WatchDogStartEnable == TRUE) && (Dumber.StateSystem != STATE_IDLE))
213
+        Dumber.cpt_watchdog+=10;
214
+    else
215
+        Dumber.cpt_watchdog=0;
216
+
217
+    if (Dumber.cpt_watchdog > WATCHDOG_MAX) {
218
+        Dumber.cpt_watchdog=30; // pour avoir toujours un watchdog cadancé à 1000 ms, et pas 1030ms
219
+        Dumber.InvalidWatchdogResetCpt++;
220
+    }
221
+
222
+    if (Dumber.InvalidWatchdogResetCpt > WATCHDOG_INVALID_COUNTER_MAX) {
223
+        systemChangeState(STATE_WATCHDOG_DISABLE);
224
+    }
162
 
225
 
163
     if(Dumber.cpt_systick % 500 == 0){
226
     if(Dumber.cpt_systick % 500 == 0){
164
         Dumber.acquisition=VOLTAGE;
227
         Dumber.acquisition=VOLTAGE;
165
-        voltagePrepare();
228
+        batteryRefreshData();
166
     }
229
     }
167
 
230
 
168
-    if(Dumber.cpt_systick % 100==0){
231
+    /*if(Dumber.cpt_systick % 100==0){
169
         etatLED++;
232
         etatLED++;
170
 
233
 
171
-        if(etatLED ==12) etatLED = 0;
172
-    }
234
+        if (etatLED ==12) etatLED = 0;
235
+    }*/
173
 
236
 
174
 #if !defined (__NO_INACTIVITY_SHUTDOWN__)
237
 #if !defined (__NO_INACTIVITY_SHUTDOWN__)
175
     if(Dumber.cpt_inactivity>=120000){
238
     if(Dumber.cpt_inactivity>=120000){
176
-        shutDown();
239
+        systemShutDown();
177
     }
240
     }
178
 #else
241
 #else
179
 #warning "Shutdown after inactivity period disabled! Not for production !!!"
242
 #warning "Shutdown after inactivity period disabled! Not for production !!!"
180
 #endif /* __NO_INACTIVITY_SHUTDOWN__ */
243
 #endif /* __NO_INACTIVITY_SHUTDOWN__ */
181
 
244
 
182
-    if(Dumber.cpt_watchdog>=TIMER_Watchdog)
183
-    {
184
-        Dumber.StateSystem=DISABLE;
185
-        cmdRightMotor(BRAKE,0);
186
-        cmdLeftMotor(BRAKE,0);
187
-    }
188
-
189
     Dumber.flagSystick=1;
245
     Dumber.flagSystick=1;
190
 }
246
 }
191
 
247
 

+ 36
- 64
software/robot/src/system_dumby.h View File

27
  ******************************************************************************
27
  ******************************************************************************
28
  ******************************************************************************
28
  ******************************************************************************
29
  */
29
  */
30
-#ifndef system_dumby_H
31
-#define system_dumby_H
30
+#ifndef _SYSTEM_DUMBY_H_
31
+#define _SYSTEM_DUMBY_H_
32
 
32
 
33
 #include "stm32f10x.h"
33
 #include "stm32f10x.h"
34
 
34
 
35
-/* Declaration des Constantes */
36
-#define VERSION				"version 1.2\r"
35
+/* Déclaration des Constantes */
36
+#define VERSION				"version 1.3\r"
37
 
37
 
38
 #define SPI 				10 
38
 #define SPI 				10 
39
 #define USART 				20 
39
 #define USART 				20 
40
-#define I2C				30 
40
+#define I2C				    30
41
 
41
 
42
 #define TRUE  				40 
42
 #define TRUE  				40 
43
 #define FALSE 				50 
43
 #define FALSE 				50 
44
 
44
 
45
-#define RUN				51 
46
-#define	IDLE				52
47
-#define LOW				53 
48
-#define DISABLE 			54
49
-
50
 #define	VOLTAGE 			98
45
 #define	VOLTAGE 			98
51
 #define CURRENT 			99
46
 #define CURRENT 			99
52
 
47
 
53
 #define	UNDEFINED			101 
48
 #define	UNDEFINED			101 
54
 
49
 
55
-//CMDE
56
-
57
-#define	PingCMD				'p'		
58
-#define ResetCMD			'r'			
59
-#define SetMotorCMD			'm'			
60
-#define StartWWatchDogCMD		'W'			
61
-#define	ResetWatchdogCMD		'w'				
62
-#define GetBatteryVoltageCMD		'v'			
63
-#define GetVersionCMD			'V'			
64
-#define StartWithoutWatchCMD 		'u'
65
-#define MoveCMD				'M'			
66
-#define TurnCMD				'T'
67
-#define BusyStateCMD 			'b'	
68
-#define TestCMD                 	't'
69
-#define DebugCMD			'a'
70
-
71
-#define SystemCoreClock 		8000000
72
-
73
-//extern volatile uint16_t voltageADC;
74
-
75
 #define TIMER_1s 			1000 // 1 sec
50
 #define TIMER_1s 			1000 // 1 sec
76
-#define TIMER_Watchdog			3050	 //
77
-#define TTMER_Inactivity 		120000 // 2 min
78
-
79
-#define RIGHT				'>'
80
-#define LEFT				'<'
51
+#define TIMER_Watchdog		3050	 //
52
+#define TTMER_Inactivity 	120000 // 2 min
81
 
53
 
82
-#define OK_ANS				"O\r"
83
-#define ERR_ANS				"E\r"
84
-#define UNKNOW_ANS			"C\r"
85
-#define BAT_OK				"2\r"
86
-#define BAT_LOW				"1\r"
87
-#define BAT_EMPTY			"0\r"
54
+#define WATCHDOG_MIN        970
55
+#define WATCHDOG_MAX        1030
56
+#define WATCHDOG_INVALID_COUNTER_MAX 3
88
 
57
 
89
 #define COMMONSPEED			5
58
 #define COMMONSPEED			5
90
 #define LOWSPEED			2
59
 #define LOWSPEED			2
91
-#define HYPERVITESSE			7
60
+#define HYPERVITESSE		7
92
 
61
 
93
-/* Declaration de structure */
62
+/* Déclaration de structure */
63
+
64
+enum States {
65
+        STATE_IDLE = 0,
66
+        STATE_RUN,
67
+        STATE_LOW,
68
+        STATE_DISABLE,
69
+        STATE_WATCHDOG_DISABLE
70
+};
71
+typedef enum States States;
94
 
72
 
95
-typedef struct Settings Settings;
96
 struct Settings
73
 struct Settings
97
 {	
74
 {	
98
         // Information
75
         // Information
99
         uint16_t BatteryPercentage;			// Retourne le pourcentage de charge de la batterie
76
         uint16_t BatteryPercentage;			// Retourne le pourcentage de charge de la batterie
100
-        uint16_t BatteryCurrent;				//
101
-        char StateSystem;								// Etat de la MAE
102
-        char AddOn;											// Un AddOn a �t� d�tect�							// Les instructions seront sur le protocol SPI
103
-        char BatterieChecking; 					// On doit verifier la valeur de la batterie
77
+        uint16_t BatteryCurrent;			//
78
+        States StateSystem;					// État de la MAE
79
+        char AddOn;							// Un AddOn a été détecté
80
+        // Les instructions seront sur le protocole SPI
81
+        char BatterieChecking; 				// On doit vérifier la valeur de la batterie
104
         uint16_t cpt_systick;
82
         uint16_t cpt_systick;
105
-        char WatchDogStartEnable;				// Le Robot a �t� lanc� en mode WithWatchDog ou WithoutWatchDog
83
+        char WatchDogStartEnable;			// Le Robot a été lancé en mode WithWatchDog ou WithoutWatchDog
106
         uint16_t cpt_watchdog;
84
         uint16_t cpt_watchdog;
85
+        uint8_t InvalidWatchdogResetCpt;     // Compteur de remise à zéro du watchdog en dehors du temps imparti
107
         char busyState;
86
         char busyState;
108
         int  cpt_inactivity;
87
         int  cpt_inactivity;
109
         char acquisition;
88
         char acquisition;
111
         char flagSystick;
90
         char flagSystick;
112
 };
91
 };
113
 
92
 
114
-/* Declaration des variables systemes */
115
-extern uint16_t greenLight;
116
-extern uint16_t redLight;
93
+typedef struct Settings Settings;
94
+
95
+/* Déclaration des variables systèmes */
117
 extern Settings Dumber;
96
 extern Settings Dumber;
118
-extern uint16_t asservissement;
119
-extern uint16_t regulation_vitesseD, regulation_vitesseG;
120
-extern uint16_t watchDogState;
121
-extern char etatLED; // Tout les 200 ms cette variable s'incr�mente de 1 jusqu'� 5
122
-extern char vClignotement1s;
123
-extern char LEDON;
124
 
97
 
125
 /* Prototype Fonctions */
98
 /* Prototype Fonctions */
126
-void default_settings(void);
127
-void MAP_PinShutDown(void);
128
-void SysTick_Handler(void);
129
-void Configure_SysTick(void);
130
-void shutDown(void);
99
+void systemConfigure(void);
100
+void systemChangeState(States state);
101
+char systemResetWatchdog(void);
102
+void systemShutDown(void);
131
 
103
 
132
-#endif /* SYSTEM_DUMBY_H_ */
104
+#endif /* _SYSTEM_DUMBY_H_ */
133
 
105
 
134
 
106
 

software/robot/src/cmde_usart.c → software/robot/src/usart.c View File

16
 #include "system_dumby.h"
16
 #include "system_dumby.h"
17
 #include <stm32f10x.h>
17
 #include <stm32f10x.h>
18
 #include <string.h>
18
 #include <string.h>
19
+#include <usart.h>
19
 
20
 
20
 #include "cmdManager.h"
21
 #include "cmdManager.h"
21
 
22
 
22
-#include "cmde_usart.h"
23
 
23
 
24
 //#include "Battery.h"
24
 //#include "Battery.h"
25
 //#include "motor.h"
25
 //#include "motor.h"
64
  */
64
  */
65
 
65
 
66
 /**
66
 /**
67
- * @brief		Configuration du GPIO pour l'uart.
68
- *
69
- * @note		La fonction mapUsartPin va venir configurer le E/S du GPIO pour correspondre avec le sch�ma electrique en ressource.
70
- *				PB7 Analog Input / PB6 Alternate function output.
67
+ * @brief Initialise l'UART avec les paramètres suivants : 9600 bauds / 1bits de stop / pas de partité ou de controle
71
  *
68
  *
72
- * @param  		Aucun
73
- * @retval 		Aucun
69
+ * @param  Aucun
70
+ * @retval Aucun
74
  */
71
  */
75
-void MAP_UsartPin()
72
+
73
+void usartConfigure(void)
76
 {
74
 {
75
+    USART_InitTypeDef USART_InitStructure;
77
     GPIO_InitTypeDef Init_Structure;
76
     GPIO_InitTypeDef Init_Structure;
77
+    NVIC_InitTypeDef NVIC_InitStructure;
78
 
78
 
79
-    /// Configure Output ALTERNATE FONCTION PPULL PORT B6 Tx
79
+    // Configure les lignes d'E/S
80
+    // Configure Output ALTERNATE FONCTION PPULL PORT B6 Tx
80
     Init_Structure.GPIO_Pin = GPIO_Pin_6;
81
     Init_Structure.GPIO_Pin = GPIO_Pin_6;
81
     Init_Structure.GPIO_Mode = GPIO_Mode_AF_PP;
82
     Init_Structure.GPIO_Mode = GPIO_Mode_AF_PP;
82
     Init_Structure.GPIO_Speed=GPIO_Speed_50MHz;
83
     Init_Structure.GPIO_Speed=GPIO_Speed_50MHz;
83
     GPIO_Init(GPIOB, &Init_Structure);
84
     GPIO_Init(GPIOB, &Init_Structure);
84
 
85
 
85
-    /// Configure B7 Rx
86
+    // Configure B7 Rx
86
     Init_Structure.GPIO_Pin = GPIO_Pin_7;
87
     Init_Structure.GPIO_Pin = GPIO_Pin_7;
87
     Init_Structure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
88
     Init_Structure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
88
     GPIO_Init(GPIOB, &Init_Structure);
89
     GPIO_Init(GPIOB, &Init_Structure);
90
+
91
+    // Configure l'USART
92
+    USART_InitStructure.USART_BaudRate = 9600;
93
+    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
94
+    USART_InitStructure.USART_StopBits = USART_StopBits_1;
95
+    USART_InitStructure.USART_Parity = USART_Parity_No;
96
+    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
97
+    USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
98
+
99
+    USART_Init(USART1, &USART_InitStructure);
100
+
101
+    USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
102
+    USART_Cmd(USART1, ENABLE);
103
+    GPIO_PinRemapConfig(GPIO_Remap_USART1,ENABLE);
104
+    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
105
+
106
+    // configure les interruptions de l'USART
107
+    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
108
+    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
109
+    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
110
+    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
111
+    NVIC_Init(&NVIC_InitStructure);
89
 }
112
 }
90
 
113
 
91
 /**
114
 /**
94
  * @param  Aucun
117
  * @param  Aucun
95
  * @retval Aucun
118
  * @retval Aucun
96
  */
119
  */
97
-void INIT_DMASend(uint16_t bufferSize)
120
+void usartInitDMA(uint16_t bufferSize)
98
 {
121
 {
99
     DMA_InitTypeDef DMA_InitStructure;
122
     DMA_InitTypeDef DMA_InitStructure;
100
 
123
 
110
     DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
133
     DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
111
     DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
134
     DMA_InitStructure.DMA_Priority = DMA_Priority_VeryHigh;
112
     DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
135
     DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;
113
-    DMA_Init(DMA1_Channel4, &DMA_InitStructure);
114
-}
115
-
116
-/**
117
- * @brief  Configure l'IT sur la reception d'un caractère sur l'uart.
118
- *
119
- * @param  Aucun
120
- * @retval Aucun
121
- */
122
-void INIT_IT_UsartReceive(void)
123
-{
124
-    NVIC_InitTypeDef NVIC_InitStructure;
125
 
136
 
126
-    /* Enable the USARTz Interrupt */
127
-    NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
128
-    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;
129
-    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
130
-    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
131
-    NVIC_Init(&NVIC_InitStructure);
137
+    DMA_Init(DMA1_Channel4, &DMA_InitStructure);
132
 }
138
 }
133
 
139
 
134
 /**
140
 /**
135
  * @}
141
  * @}
136
  */
142
  */
137
 
143
 
138
-/**
139
- * @brief Initialise l'UART avec les paramètres suivants : 9600 bauds / 1bits de stop / pas de partité ou de controle
140
- *
141
- * @param  Aucun
142
- * @retval Aucun
143
- */
144
-
145
-void INIT_USART(void)
146
-{
147
-    USART_InitTypeDef USART_InitStructure;
148
-
149
-    USART_InitStructure.USART_BaudRate = 9600;
150
-    USART_InitStructure.USART_WordLength = USART_WordLength_8b;
151
-    USART_InitStructure.USART_StopBits = USART_StopBits_1;
152
-    USART_InitStructure.USART_Parity = USART_Parity_No;
153
-    USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
154
-    USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
155
-
156
-    USART_Init(USART1, &USART_InitStructure);
157
-
158
-    USART_DMACmd(USART1, USART_DMAReq_Tx, ENABLE);
159
-    USART_Cmd(USART1, ENABLE);
160
-    GPIO_PinRemapConfig(GPIO_Remap_USART1,ENABLE);
161
-    USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);
162
-}
163
-
164
 /** @addtogroup Function Send - Receive
144
 /** @addtogroup Function Send - Receive
165
  * @{
145
  * @{
166
  */
146
  */
181
 
161
 
182
 void USART1_IRQHandler(void)
162
 void USART1_IRQHandler(void)
183
 {
163
 {
184
-   if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) {
164
+    if (USART_GetITStatus(USART1, USART_IT_RXNE) != RESET) {
185
         receiptString[cpt_Rx] = USART_ReceiveData(USART1);
165
         receiptString[cpt_Rx] = USART_ReceiveData(USART1);
186
         if (cpt_Rx < 16)
166
         if (cpt_Rx < 16)
187
             cpt_Rx++;
167
             cpt_Rx++;
188
 
168
 
189
-        if (receiptString[cpt_Rx - 1] == 13) {
190
-            if (verifyCheckSum() == 0) {
191
-                manageCmd();
192
-            } else
193
-                strcpy(sendString, UNKNOW_ANS);
169
+        if (receiptString[cpt_Rx - 1] == '\r') {
170
+            cmdManage();
194
 
171
 
195
             if (Dumber.AddOn == FALSE) {
172
             if (Dumber.AddOn == FALSE) {
196
-                inclusionCheckSum();
197
-                sendDataUSART();
173
+                cmdAddChecksum();
174
+                usartSendData(); // Fonction bloquante
198
             }
175
             }
199
 
176
 
200
             for (i = 0; i < cpt_Rx + 1; i++)
177
             for (i = 0; i < cpt_Rx + 1; i++)
221
  * @param  Aucun
198
  * @param  Aucun
222
  * @retval Aucun
199
  * @retval Aucun
223
  */
200
  */
224
-void sendDataUSART(void){
201
+void usartSendData(void){
225
     int taille;
202
     int taille;
226
     for(taille = 0; sendString[taille]!= '\r'; taille++);
203
     for(taille = 0; sendString[taille]!= '\r'; taille++);
227
-    INIT_DMASend(taille+1);
204
+    usartInitDMA(taille+1);
228
     DMA_Cmd(DMA1_Channel4, ENABLE);
205
     DMA_Cmd(DMA1_Channel4, ENABLE);
229
     while (DMA_GetFlagStatus(DMA1_FLAG_TC4) == RESET);
206
     while (DMA_GetFlagStatus(DMA1_FLAG_TC4) == RESET);
230
     for(i=0; i<TBuffer;i++)
207
     for(i=0; i<TBuffer;i++)

software/robot/src/cmde_usart.h → software/robot/src/usart.h View File

13
  ******************************************************************************
13
  ******************************************************************************
14
  ******************************************************************************
14
  ******************************************************************************
15
  */
15
  */
16
-#ifndef cmde_usart_H
17
-#define cmde_usart_H
16
+#ifndef _USART_H_
17
+#define _USART_H_
18
 
18
 
19
 #include "stm32f10x.h"
19
 #include "stm32f10x.h"
20
 
20
 
21
 #define TBuffer   30
21
 #define TBuffer   30
22
 
22
 
23
-/* Prototype des fonctions */
24
-
25
-void INIT_IT_UsartReceive(void);
26
-void INIT_DMASend(uint16_t bufferSize);
27
-void sendDataUSART(void);
28
-void INIT_USART(void);
29
-void MAP_UsartPin(void);
30
-
31
-/* Variable Externes necessair e*/
32
-
23
+/* Variables externes nécessaire*/
33
 extern char sendString[TBuffer];
24
 extern char sendString[TBuffer];
34
 extern char receiptString[TBuffer];
25
 extern char receiptString[TBuffer];
35
 extern uint16_t cpt_Rx;
26
 extern uint16_t cpt_Rx;
36
 
27
 
37
-#endif /* CMD_UART_H_ */
28
+/* Prototype des fonctions */
29
+void INIT_IT_UsartReceive(void);
30
+void usartInitDMA(uint16_t bufferSize);
31
+void usartSendData(void);
32
+void usartConfigure(void);
33
+void MAP_UsartPin(void);
34
+
35
+#endif /* _USART_H_ */
38
 
36
 

+ 1
- 1
software/robot/system/system_stm32f10x.c View File

118
 /* #define SYSCLK_FREQ_36MHz  36000000 */
118
 /* #define SYSCLK_FREQ_36MHz  36000000 */
119
 /* #define SYSCLK_FREQ_48MHz  48000000 */
119
 /* #define SYSCLK_FREQ_48MHz  48000000 */
120
 /* #define SYSCLK_FREQ_56MHz  56000000 */
120
 /* #define SYSCLK_FREQ_56MHz  56000000 */
121
-#define SYSCLK_FREQ_72MHz  72000000
121
+/* #define SYSCLK_FREQ_72MHz  72000000 */
122
 #endif
122
 #endif
123
 
123
 
124
 /*!< Uncomment the following line if you need to use external SRAM mounted
124
 /*!< Uncomment the following line if you need to use external SRAM mounted

Loading…
Cancel
Save