From e3dbb6a4ed2e533571416f894901f8877161d0b8 Mon Sep 17 00:00:00 2001 From: Arnaud Vergnet Date: Tue, 10 Nov 2020 16:47:15 +0100 Subject: [PATCH 1/3] add accelerometer functions --- MyDrivers/ADC.c | 19 +++++++++-------- MyDrivers/ADC.h | 8 +++---- Services/Accelerometer.c | 45 ++++++++++++++++++++++++++++++++++++++++ Services/Accelerometer.h | 9 ++++++++ Services/ServoMotor.c | 2 +- Src/Roll.c | 24 +++++++++++++++++++++ Src/Roll.h | 6 ++++++ Src/main.c | 37 +++++++++++++++++++++++---------- 8 files changed, 125 insertions(+), 25 deletions(-) diff --git a/MyDrivers/ADC.c b/MyDrivers/ADC.c index 7fbd2d0..e1c6b13 100644 --- a/MyDrivers/ADC.c +++ b/MyDrivers/ADC.c @@ -1,7 +1,7 @@ #include "ADC.h" #include "stm32f1xx_ll_bus.h" // Pour horloge -void ADC_conf(ADC_TypeDef *adc, uint32_t voie) +void ADC_conf(ADC_TypeDef *adc) { if (adc == ADC1) { LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC1); @@ -13,10 +13,9 @@ void ADC_conf(ADC_TypeDef *adc, uint32_t voie) RCC->CFGR |= RCC_CFGR_ADCPRE_DIV6; // Fixe le nombre de conversion à 1 - adc->SQR1&= ADC_SQR1_L; + adc->SQR1 &= ADC_SQR1_L; - // Indique la voie a convertir - adc->SQR3|= voie; + // Calibration adc->CR2 |= ADC_CR2_CAL_Msk; @@ -30,23 +29,25 @@ void ADC_start(ADC_TypeDef *adc) } -uint16_t ADC_readRaw(ADC_TypeDef *adc) +uint16_t ADC_readRaw(ADC_TypeDef *adc, int channel) { + // Indique la voie a convertir + adc->SQR3 = channel; // Lancement de la conversion adc->CR2 |= ADC_CR2_ADON; while(!(ADC1->SR & ADC_SR_EOC)) {} - + return ADC1->DR & ADC_DR_DATA_Msk; } -double ADC_convertToVolt(uint16_t value) +float ADC_convertToVolt(uint16_t value) { return ((double) value) / 4095.0 * 3.3; } -double ADC_readVolt(ADC_TypeDef *adc) +float ADC_readVolt(ADC_TypeDef *adc, int channel) { - return ADC_convertToVolt(ADC_readRaw(adc)); + return ADC_convertToVolt(ADC_readRaw(adc, channel)); } diff --git a/MyDrivers/ADC.h b/MyDrivers/ADC.h index b73929d..a7ebee7 100644 --- a/MyDrivers/ADC.h +++ b/MyDrivers/ADC.h @@ -3,13 +3,13 @@ #include "stm32f1xx_ll_adc.h" -void ADC_conf(ADC_TypeDef *adc, uint32_t voie); +void ADC_conf(ADC_TypeDef *adc); void ADC_start(ADC_TypeDef *adc); -uint16_t ADC_readRaw(ADC_TypeDef *adc); -double ADC_readVolt(ADC_TypeDef *adc); +uint16_t ADC_readRaw(ADC_TypeDef *adc, int channel); +float ADC_readVolt(ADC_TypeDef *adc, int channel); -double ADC_convertToVolt(uint16_t value); +float ADC_convertToVolt(uint16_t value); #endif diff --git a/Services/Accelerometer.c b/Services/Accelerometer.c index 022679c..99005e1 100644 --- a/Services/Accelerometer.c +++ b/Services/Accelerometer.c @@ -1 +1,46 @@ #include "Accelerometer.h" +#include "ADC.h" +#include "math.h" + +#define M_PI 3.14159265358979323846 + +// g-sel1 et g-sel1 à 0 => range de 2.5g +// Donc sensibilité de +- 480 mv/g + + +const float ZERO_G = 1.65; // 0 g +//const float MAX_G = 2.13; // 1 g +//const float MIN_G = 1.17; // -1 g +const float SENSITIVITY = 0.48; + +void Accelerometer_conf(ADC_TypeDef *adc, GPIO_TypeDef * gpio, int pinx, int piny) +{ + ADC_conf(adc); + GPIO_conf(gpio, pinx, LL_GPIO_MODE_ANALOG, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP); + GPIO_conf(gpio, piny, LL_GPIO_MODE_ANALOG, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP); +} + +void Accelerometer_start(ADC_TypeDef *adc) +{ + ADC_start(adc); +} + +float voltsToG(float volts) +{ + float readG = (volts - ZERO_G) / SENSITIVITY; + if (readG > 1.0) + readG = 1.0; + else if (readG < -1.0) + readG = -1.0; + return readG; +} + +int Accelerometer_getAngle(ADC_TypeDef *adc, int channel) +{ + const float readG = voltsToG(ADC_readVolt(adc, channel)); + + float angleRad = asin(readG); + int angleDeg = angleRad * (180/M_PI); + + return angleDeg; +} diff --git a/Services/Accelerometer.h b/Services/Accelerometer.h index 67bee39..58ce421 100644 --- a/Services/Accelerometer.h +++ b/Services/Accelerometer.h @@ -1,4 +1,13 @@ #ifndef ACCELEROMETER_H #define ACCELEROMETER_H +#include "GPIO.h" +#include "stm32f1xx_ll_adc.h" + +void Accelerometer_conf(ADC_TypeDef *adc, GPIO_TypeDef * gpio, int pinx, int piny); + +void Accelerometer_start(ADC_TypeDef *adc); + +int Accelerometer_getAngle(ADC_TypeDef *adc, int channel); + #endif diff --git a/Services/ServoMotor.c b/Services/ServoMotor.c index 70e636c..3fd03b9 100644 --- a/Services/ServoMotor.c +++ b/Services/ServoMotor.c @@ -4,7 +4,7 @@ void ServoMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin) { - Timer_pwmo_conf(timer, channel, SERVO_MOTO_FREQ, 0.5); + Timer_pwmo_conf(timer, channel, SERVO_MOTO_FREQ, 0); GPIO_conf(gpio, pin, LL_GPIO_MODE_ALTERNATE, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP); } diff --git a/Src/Roll.c b/Src/Roll.c index 3dd9b42..8e2bc0d 100644 --- a/Src/Roll.c +++ b/Src/Roll.c @@ -1 +1,25 @@ #include "Roll.h" +#include "Accelerometer.h" + +ADC_TypeDef * ROLL_ADC = ADC1; +const int ROLL_X_CHANNEL = LL_ADC_CHANNEL_11; +const int ROLL_Y_CHANNEL = LL_ADC_CHANNEL_10; +GPIO_TypeDef * ROLL_GPIO = GPIOC; +const int ROLL_X_PIN = LL_GPIO_PIN_0; +const int ROLL_Y_PIN = LL_GPIO_PIN_1; + +void Roll_conf(void) +{ + Accelerometer_conf(ROLL_ADC, ROLL_GPIO, ROLL_X_PIN, ROLL_Y_PIN); +} + +void Roll_start(void) +{ + Accelerometer_start(ROLL_ADC); +} + +void Roll_background(void) +{ + const int xAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_X_CHANNEL); + const int yAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_Y_CHANNEL); +} diff --git a/Src/Roll.h b/Src/Roll.h index e6969d7..2b48a8c 100644 --- a/Src/Roll.h +++ b/Src/Roll.h @@ -1,4 +1,10 @@ #ifndef ROLL_H #define ROLL_H +void Roll_conf(void); + +void Roll_start(void); + +void Roll_background(void); + #endif diff --git a/Src/main.c b/Src/main.c index 77bdaaf..d55b4a8 100644 --- a/Src/main.c +++ b/Src/main.c @@ -21,36 +21,55 @@ #include "stm32f1xx_ll_system.h" // utile dans la fonction SystemClock_Config #include "Sail.h" +#include "Roll.h" + #include "Scheduler.h" #include "ADC.h" #include "GPIO.h" +#include "Accelerometer.h" void SystemClock_Config(void); /* Private functions ---------------------------------------------------------*/ int counter = 1; -int adcRaw = 0; -double adcVolt = 0.0; +int adcRaw1 = 0; +int adcRaw2 = 0; +double adcVolt1 = 0.0; +double adcVolt2 = 0.0; + +int angle = 0; void backgroundTask() { counter++; Sail_background(); - adcRaw = ADC_readRaw(ADC1); - adcVolt = ADC_convertToVolt(adcRaw); + Roll_background(); + + angle = Accelerometer_getAngle(ADC1, LL_ADC_CHANNEL_11); + + // DEBUG + adcRaw1 = ADC_readRaw(ADC1, LL_ADC_CHANNEL_11); + adcRaw2 = ADC_readRaw(ADC1, LL_ADC_CHANNEL_10); + adcVolt1 = ADC_convertToVolt(adcRaw1); + adcVolt2 = ADC_convertToVolt(adcRaw2); } - void configurePeripherals() { Sail_conf(); - ADC_conf(ADC1, 12); + Roll_conf(); + + // DEBUG + ADC_conf(ADC1); } void startPeripherals() { Sail_start(); + Roll_start(); + + // DEBUG ADC_start(ADC1); } @@ -65,11 +84,6 @@ int main(void) SystemClock_Config(); /* Add your application code here */ - // Configuration chronomètre - // Chrono_Conf(TIM3); - // Lancement chronomètre - // Chrono_Start(); - // time = Chrono_Read(); configurePeripherals(); startPeripherals(); @@ -78,6 +92,7 @@ int main(void) while (1) { + // Send to display here } } From 76da8b43420a242bbb9986146216f24dde5f986d Mon Sep 17 00:00:00 2001 From: Arnaud Vergnet Date: Tue, 10 Nov 2020 17:37:10 +0100 Subject: [PATCH 2/3] add sail and roll logic --- Src/Roll.c | 19 ++++++++++++++++++- Src/Roll.h | 2 ++ Src/Sail.c | 33 ++++++++++++++++++++++++++++----- Src/Sail.h | 2 +- 4 files changed, 49 insertions(+), 7 deletions(-) diff --git a/Src/Roll.c b/Src/Roll.c index 8e2bc0d..05a39b9 100644 --- a/Src/Roll.c +++ b/Src/Roll.c @@ -1,5 +1,7 @@ #include "Roll.h" +#include "Sail.h" #include "Accelerometer.h" +#include "stdlib.h" ADC_TypeDef * ROLL_ADC = ADC1; const int ROLL_X_CHANNEL = LL_ADC_CHANNEL_11; @@ -8,6 +10,10 @@ GPIO_TypeDef * ROLL_GPIO = GPIOC; const int ROLL_X_PIN = LL_GPIO_PIN_0; const int ROLL_Y_PIN = LL_GPIO_PIN_1; +const int TRIGGER_ANGLE = 40; + +int Roll_isEmergencyState = 0; + void Roll_conf(void) { Accelerometer_conf(ROLL_ADC, ROLL_GPIO, ROLL_X_PIN, ROLL_Y_PIN); @@ -18,8 +24,19 @@ void Roll_start(void) Accelerometer_start(ROLL_ADC); } +int Roll_getEmergencyState(void) +{ + return Roll_isEmergencyState; +} + void Roll_background(void) { const int xAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_X_CHANNEL); - const int yAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_Y_CHANNEL); + //const int yAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_Y_CHANNEL); + + const int currentState = abs(xAngle) >= 40; + if (Roll_isEmergencyState && !currentState) + Sail_setEmergency(1); + else if (!Roll_isEmergencyState && currentState) + Sail_setEmergency(0); } diff --git a/Src/Roll.h b/Src/Roll.h index 2b48a8c..97093ee 100644 --- a/Src/Roll.h +++ b/Src/Roll.h @@ -5,6 +5,8 @@ void Roll_conf(void); void Roll_start(void); +int Roll_getEmergencyState(void); + void Roll_background(void); #endif diff --git a/Src/Sail.c b/Src/Sail.c index 5b0ae95..f6b6bc2 100644 --- a/Src/Sail.c +++ b/Src/Sail.c @@ -11,25 +11,48 @@ TIM_TypeDef * ENCODER_TIMER = TIM3; GPIO_TypeDef * ENCODER_GPIO = GPIOA; const int ENCODER_PIN = LL_GPIO_PIN_5; +const int RESET_ANGLE = 90; + +int Sail_isEmergencyState = 0; + void Sail_conf() { ServoMotor_conf(MOTOR_TIMER, MOTOR_CHANNEL, MOTOR_GPIO, MOTOR_PIN); IncrementalEncoder_conf(ENCODER_TIMER, ENCODER_GPIO, ENCODER_PIN); } -void Sail_background() +int getSailAngle(int windAngle) { - + if (windAngle > 180) + return 90 * (windAngle - 45) / 135; + else + return 360 - 90 * ((360 - windAngle) - 45) / 135; } -void Sail_reset() +void Sail_background() { + if (Sail_isEmergencyState) + return; + const int windAngle = IncrementalEncoder_getAngle(ENCODER_TIMER); + + if (windAngle < 45 || windAngle > 315) + ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, 0); + else + ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, getSailAngle(windAngle)); +} + +void Sail_setEmergency(int state) +{ + Sail_isEmergencyState = state; + if (Sail_isEmergencyState) + ServoMotor_setAngle(MOTOR_TIMER, MOTOR_CHANNEL, RESET_ANGLE); } void Sail_start() { - Timer_start(MOTOR_TIMER); - Timer_start(ENCODER_TIMER); + Sail_isEmergencyState = 0; + ServoMotor_start(MOTOR_TIMER); + IncrementalEncoder_start(ENCODER_TIMER); } diff --git a/Src/Sail.h b/Src/Sail.h index cc49ef0..1973501 100644 --- a/Src/Sail.h +++ b/Src/Sail.h @@ -23,7 +23,7 @@ void Sail_background(void); * @param None * @retval None */ -void Sail_reset(void); +void Sail_setEmergency(int state); /** * @brief Réinitialise la voile à sa position initiale From 2d5c4e4f940044bfa264353fc08c396910946ef1 Mon Sep 17 00:00:00 2001 From: Arnaud Vergnet Date: Tue, 10 Nov 2020 17:37:34 +0100 Subject: [PATCH 3/3] update project files --- MDK-ARM/Project.uvoptx | 71 +++++++++++++++++++++++++++-------------- MDK-ARM/Project.uvprojx | 20 ++++++++++++ 2 files changed, 67 insertions(+), 24 deletions(-) diff --git a/MDK-ARM/Project.uvoptx b/MDK-ARM/Project.uvoptx index 2a464ad..fad46b2 100644 --- a/MDK-ARM/Project.uvoptx +++ b/MDK-ARM/Project.uvoptx @@ -345,7 +345,7 @@ 0 DLGDARM - (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=2156,213,2522,450,0)(1009=-1,-1,-1,-1,0)(100=-1,-1,-1,-1,0)(110=-1,-1,-1,-1,0)(111=-1,-1,-1,-1,0)(1011=-1,-1,-1,-1,0)(180=-1,-1,-1,-1,0)(120=-1,-1,-1,-1,0)(121=2124,217,2535,660,0)(122=-1,-1,-1,-1,0)(123=-1,-1,-1,-1,0)(140=-1,-1,-1,-1,0)(240=-1,-1,-1,-1,0)(190=-1,-1,-1,-1,0)(200=-1,-1,-1,-1,0)(170=-1,-1,-1,-1,0)(130=-1,-1,-1,-1,0)(131=-1,-1,-1,-1,0)(132=2076,105,2660,897,0)(133=2592,151,3176,943,0)(160=-1,-1,-1,-1,0)(161=-1,-1,-1,-1,0)(162=-1,-1,-1,-1,0)(210=-1,-1,-1,-1,0)(211=-1,-1,-1,-1,0)(220=-1,-1,-1,-1,0)(221=-1,-1,-1,-1,0)(230=-1,-1,-1,-1,0)(234=-1,-1,-1,-1,0)(231=-1,-1,-1,-1,0)(232=-1,-1,-1,-1,0)(233=-1,-1,-1,-1,0)(150=-1,-1,-1,-1,0)(151=-1,-1,-1,-1,0) + (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=1554,213,1920,450,0)(1009=-1,-1,-1,-1,0)(100=-1,-1,-1,-1,0)(110=-1,-1,-1,-1,0)(111=-1,-1,-1,-1,0)(1011=-1,-1,-1,-1,0)(180=-1,-1,-1,-1,0)(120=-1,-1,-1,-1,0)(121=1509,217,1920,660,0)(122=-1,-1,-1,-1,0)(123=-1,-1,-1,-1,0)(140=-1,-1,-1,-1,0)(240=-1,-1,-1,-1,0)(190=-1,-1,-1,-1,0)(200=-1,-1,-1,-1,0)(170=-1,-1,-1,-1,0)(130=-1,-1,-1,-1,0)(131=-1,-1,-1,-1,0)(132=1336,105,1920,897,0)(133=1148,116,1732,908,0)(160=-1,-1,-1,-1,0)(161=-1,-1,-1,-1,0)(162=-1,-1,-1,-1,0)(210=-1,-1,-1,-1,0)(211=-1,-1,-1,-1,0)(220=-1,-1,-1,-1,0)(221=-1,-1,-1,-1,0)(230=-1,-1,-1,-1,0)(234=-1,-1,-1,-1,0)(231=-1,-1,-1,-1,0)(232=-1,-1,-1,-1,0)(233=-1,-1,-1,-1,0)(150=915,87,1508,879,0)(151=-1,-1,-1,-1,0) 0 @@ -382,9 +382,9 @@ 0 0 - 65 + 84 1 -
134223216
+
134226236
0 0 0 @@ -393,23 +393,7 @@ 1 ../Src/main.c - \\NUCLEO_F103RB\../Src/main.c\65 -
- - 1 - 0 - 69 - 1 -
134223230
- 0 - 0 - 0 - 0 - 0 - 1 - ../Src/main.c - - \\NUCLEO_F103RB\../Src/main.c\69 + \\NUCLEO_F103RB\../Src/main.c\84
@@ -418,6 +402,21 @@ 1 counter,0x10 + + 1 + 1 + adcVolt1 + + + 2 + 1 + adcVolt2 + + + 3 + 1 + angle,0x0A + 0 @@ -465,7 +464,7 @@ 0 ((PORTB & 0x00000100) >> 8 & 0x100) >> 8 - FF000000000000000000000000000000E0FFEF400100000000000000000000000000000028504F5254422026203078303030303031303029203E3E2038000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000100000001000000000000000000F03F1400000000000000000000000000000000000000AE110008 + FF000000000000000000000000000000E0FFEF400100000000000000000000000000000028504F5254422026203078303030303031303029203E3E2038000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000100000001000000000000000000F03F1300000000000000000000000000000000000000AE110008 @@ -772,6 +771,30 @@ 0 0 + + 4 + 23 + 1 + 0 + 0 + 0 + ..\LLDrivers\src\stm32f1xx_ll_usart.c + stm32f1xx_ll_usart.c + 0 + 0 + + + 4 + 24 + 1 + 0 + 0 + 0 + ..\LLDrivers\src\stm32f1xx_ll_adc.c + stm32f1xx_ll_adc.c + 0 + 0 + @@ -782,7 +805,7 @@ 0 5 - 23 + 25 5 0 0 @@ -802,7 +825,7 @@ 0 6 - 24 + 26 1 0 0 @@ -822,7 +845,7 @@ 0 7 - 25 + 27 2 0 0 diff --git a/MDK-ARM/Project.uvprojx b/MDK-ARM/Project.uvprojx index 01a437b..04a36be 100644 --- a/MDK-ARM/Project.uvprojx +++ b/MDK-ARM/Project.uvprojx @@ -508,6 +508,16 @@ 1 ..\LLDrivers\src\stm32f1xx_ll_gpio.c + + stm32f1xx_ll_usart.c + 1 + ..\LLDrivers\src\stm32f1xx_ll_usart.c + + + stm32f1xx_ll_adc.c + 1 + ..\LLDrivers\src\stm32f1xx_ll_adc.c + @@ -1047,6 +1057,16 @@ 1 ..\LLDrivers\src\stm32f1xx_ll_gpio.c + + stm32f1xx_ll_usart.c + 1 + ..\LLDrivers\src\stm32f1xx_ll_usart.c + + + stm32f1xx_ll_adc.c + 1 + ..\LLDrivers\src\stm32f1xx_ll_adc.c +