mirror of
https://github.com/yoboujon/dumber.git
synced 2025-06-09 14:20:50 +02:00
386 lines
12 KiB
C
386 lines
12 KiB
C
/**
|
||
******************************************************************************
|
||
* File Name : freertos.c
|
||
* Description : Code for freertos applications
|
||
******************************************************************************
|
||
* This notice applies to any and all portions of this file
|
||
* that are not between comment pairs USER CODE BEGIN and
|
||
* USER CODE END. Other portions of this file, whether
|
||
* inserted by the user or by software development tools
|
||
* are owned by their respective copyright owners.
|
||
*
|
||
* Copyright (c) 2018 STMicroelectronics International N.V.
|
||
* All rights reserved.
|
||
*
|
||
* Redistribution and use in source and binary forms, with or without
|
||
* modification, are permitted, provided that the following conditions are met:
|
||
*
|
||
* 1. Redistribution of source code must retain the above copyright notice,
|
||
* this list of conditions and the following disclaimer.
|
||
* 2. Redistributions in binary form must reproduce the above copyright notice,
|
||
* this list of conditions and the following disclaimer in the documentation
|
||
* and/or other materials provided with the distribution.
|
||
* 3. Neither the name of STMicroelectronics nor the names of other
|
||
* contributors to this software may be used to endorse or promote products
|
||
* derived from this software without specific written permission.
|
||
* 4. This software, including modifications and/or derivative works of this
|
||
* software, must execute solely and exclusively on microcontroller or
|
||
* microprocessor devices manufactured by or for STMicroelectronics.
|
||
* 5. Redistribution and use of this software other than as permitted under
|
||
* this license is void and will automatically terminate your rights under
|
||
* this license.
|
||
*
|
||
* THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS"
|
||
* AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT
|
||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A
|
||
* PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
|
||
* RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT
|
||
* SHALL STMICROELECTRONICS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
|
||
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
|
||
* OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
|
||
* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
|
||
* NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
|
||
* EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||
*
|
||
******************************************************************************
|
||
*/
|
||
|
||
/* Includes ------------------------------------------------------------------*/
|
||
#include "FreeRTOS.h"
|
||
#include "task.h"
|
||
#include "cmsis_os.h"
|
||
|
||
/* USER CODE BEGIN Includes */
|
||
#include "sharedData.h"
|
||
#include "common.h"
|
||
#include "Battery.h"
|
||
#include "serviceCom.h"
|
||
#include "Led.h"
|
||
#include "Xbee.h"
|
||
#include "mRequest.h"
|
||
#include "Motor.h"
|
||
#include <stdlib.h>
|
||
|
||
/* USER CODE END Includes */
|
||
|
||
/* Variables -----------------------------------------------------------------*/
|
||
osThreadId defaultTaskHandle;
|
||
osThreadId reqManagerHandle;
|
||
osThreadId batteryFilterHandle;
|
||
osThreadId ledManagerHandle;
|
||
osThreadId extinctionHandle;
|
||
osThreadId controlLawHandle;
|
||
osThreadId watchDogCtlHandle;
|
||
//osMutexId activityCptHandle;
|
||
//osMutexId cptWDHandle;
|
||
//osMutexId wdEnableHandle;
|
||
//osMutexId batteryLvlHandle;
|
||
//osMutexId dumbyStateHandle;
|
||
//osMutexId consigneHandle;
|
||
//osMutexId busyHandle;
|
||
//osSemaphoreId synchroMesHandle;
|
||
|
||
/* USER CODE BEGIN Variables */
|
||
|
||
/* USER CODE END Variables */
|
||
|
||
/* Function prototypes -------------------------------------------------------*/
|
||
void StartDefaultTask(void const * argument);
|
||
void StartReqManager(void const * argument);
|
||
void StartBatteryFilter(void const * argument);
|
||
void StartLedManager(void const * argument);
|
||
void StartExtinction(void const * argument);
|
||
void StartControlLow(void const * argument);
|
||
void StartWatchDogCtl(void const * argument);
|
||
|
||
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
|
||
|
||
/* USER CODE BEGIN FunctionPrototypes */
|
||
|
||
/* USER CODE END FunctionPrototypes */
|
||
|
||
/* Hook prototypes */
|
||
void configureTimerForRunTimeStats(void);
|
||
unsigned long getRunTimeCounterValue(void);
|
||
|
||
/* USER CODE BEGIN 1 */
|
||
/* Functions needed when configGENERATE_RUN_TIME_STATS is on */
|
||
__weak void configureTimerForRunTimeStats(void)
|
||
{
|
||
|
||
}
|
||
|
||
__weak unsigned long getRunTimeCounterValue(void)
|
||
{
|
||
return 0;
|
||
}
|
||
/* USER CODE END 1 */
|
||
|
||
/* Init FreeRTOS */
|
||
|
||
void MX_FREERTOS_Init(void) {
|
||
/* USER CODE BEGIN Init */
|
||
|
||
/* USER CODE END Init */
|
||
|
||
/* Create the mutex(es) */
|
||
/* definition and creation of activityCpt */
|
||
osMutexDef(activityCpt);
|
||
activityCptHandle = osMutexCreate(osMutex(activityCpt));
|
||
|
||
/* definition and creation of cptWD */
|
||
osMutexDef(cptWD);
|
||
cptWDHandle = osMutexCreate(osMutex(cptWD));
|
||
|
||
/* definition and creation of wdEnable */
|
||
osMutexDef(wdEnable);
|
||
wdEnableHandle = osMutexCreate(osMutex(wdEnable));
|
||
|
||
/* definition and creation of batteryLvl */
|
||
osMutexDef(batteryLvl);
|
||
batteryLvlHandle = osMutexCreate(osMutex(batteryLvl));
|
||
|
||
/* definition and creation of dumbyState */
|
||
osMutexDef(dumbyState);
|
||
dumbyStateHandle = osMutexCreate(osMutex(dumbyState));
|
||
|
||
/* definition and creation of consigne */
|
||
osMutexDef(consigne);
|
||
consigneHandle = osMutexCreate(osMutex(consigne));
|
||
|
||
/* definition and creation of busy */
|
||
osMutexDef(busy);
|
||
busyHandle = osMutexCreate(osMutex(busy));
|
||
|
||
/* USER CODE BEGIN RTOS_MUTEX */
|
||
/* add mutexes, ... */
|
||
/* USER CODE END RTOS_MUTEX */
|
||
|
||
/* Create the semaphores(s) */
|
||
/* definition and creation of synchroMes */
|
||
osSemaphoreDef(synchroMes);
|
||
synchroMesHandle = osSemaphoreCreate(osSemaphore(synchroMes), 1);
|
||
|
||
/* USER CODE BEGIN RTOS_SEMAPHORES */
|
||
/* add semaphores, ... */
|
||
/* USER CODE END RTOS_SEMAPHORES */
|
||
|
||
/* USER CODE BEGIN RTOS_TIMERS */
|
||
/* start timers, add new ones, ... */
|
||
/* USER CODE END RTOS_TIMERS */
|
||
|
||
/* Create the thread(s) */
|
||
/* definition and creation of defaultTask */
|
||
osThreadDef(defaultTask, StartDefaultTask, osPriorityNormal, 0, 128);
|
||
defaultTaskHandle = osThreadCreate(osThread(defaultTask), NULL);
|
||
|
||
/* definition and creation of reqManager */
|
||
osThreadDef(reqManager, StartReqManager, osPriorityIdle, 0, 128);
|
||
reqManagerHandle = osThreadCreate(osThread(reqManager), NULL);
|
||
|
||
/* definition and creation of batteryFilter */
|
||
osThreadDef(batteryFilter, StartBatteryFilter, osPriorityIdle, 0, 128);
|
||
batteryFilterHandle = osThreadCreate(osThread(batteryFilter), NULL);
|
||
|
||
/* definition and creation of ledManager */
|
||
osThreadDef(ledManager, StartLedManager, osPriorityIdle, 0, 128);
|
||
ledManagerHandle = osThreadCreate(osThread(ledManager), NULL);
|
||
|
||
/* definition and creation of extinction */
|
||
osThreadDef(extinction, StartExtinction, osPriorityIdle, 0, 128);
|
||
extinctionHandle = osThreadCreate(osThread(extinction), NULL);
|
||
|
||
/* definition and creation of controlLaw */
|
||
osThreadDef(controlLaw, StartControlLow, osPriorityHigh, 0, 128);
|
||
controlLawHandle = osThreadCreate(osThread(controlLaw), NULL);
|
||
|
||
/* definition and creation of watchDogCtl */
|
||
osThreadDef(watchDogCtl, StartWatchDogCtl, osPriorityAboveNormal, 0, 128);
|
||
watchDogCtlHandle = osThreadCreate(osThread(watchDogCtl), NULL);
|
||
|
||
/* USER CODE BEGIN RTOS_THREADS */
|
||
/* add threads, ... */
|
||
/* USER CODE END RTOS_THREADS */
|
||
|
||
/* USER CODE BEGIN RTOS_QUEUES */
|
||
/* add queues, ... */
|
||
/* USER CODE END RTOS_QUEUES */
|
||
}
|
||
|
||
/* StartDefaultTask function */
|
||
void StartDefaultTask(void const * argument)
|
||
{
|
||
|
||
/* USER CODE BEGIN StartDefaultTask */
|
||
/* Infinite loop */
|
||
for(;;)
|
||
{
|
||
osDelay(1);
|
||
}
|
||
/* USER CODE END StartDefaultTask */
|
||
}
|
||
|
||
/* StartReqManager function */
|
||
void StartReqManager(void const * argument)
|
||
{
|
||
/* USER CODE BEGIN StartReqManager */
|
||
comInit();
|
||
char message[2];
|
||
/* Infinite loop */
|
||
for(;;)
|
||
{
|
||
osSemaphoreWait(synchroMesHandle,osWaitForever);
|
||
setDumberActivity(BUSY);
|
||
message[0] = RxBuffer[8];
|
||
message[1] = manageMessage(RxBuffer);
|
||
xbeeSend(0x0001,message);
|
||
}
|
||
/* USER CODE END StartReqManager */
|
||
}
|
||
|
||
/* StartBatteryFilter function */
|
||
void StartBatteryFilter(void const * argument)
|
||
{
|
||
|
||
/* USER CODE BEGIN StartBatteryFilter */
|
||
BATTERY_INIT();
|
||
char bat;
|
||
/* Infinite loop */
|
||
for(;;)
|
||
{
|
||
bat = BATTERY_FILTER();
|
||
setDumberBattery(bat);
|
||
osDelay(200);
|
||
}
|
||
/* USER CODE END StartBatteryFilter */
|
||
}
|
||
|
||
/* StartLedManager function */
|
||
void StartLedManager(void const * argument)
|
||
{
|
||
/* USER CODE BEGIN StartLedManager */
|
||
LED_INIT();
|
||
/* Infinite loop */
|
||
for(;;)
|
||
{
|
||
char state = getDumberState();
|
||
char bat = getDumberBattery();
|
||
|
||
if(state == DISABLED)
|
||
LED_SET(LED_RED,75);
|
||
else if(state == IDLE){
|
||
if(bat > BATTERY_MEDIUM)
|
||
LED_SET(LED_GREEN,25);
|
||
else if(bat > BATTERY_LOW)
|
||
LED_SET(LED_ORANGE,25);
|
||
}
|
||
else if(state == RUN){
|
||
if(bat > BATTERY_MEDIUM)
|
||
LED_SET(LED_GREEN,122);
|
||
else if(bat > BATTERY_LOW)
|
||
LED_SET(LED_ORANGE, 122);
|
||
}
|
||
if(bat < BATTERY_LOW){
|
||
setDumberState(DISABLED);
|
||
}
|
||
osDelay(200);
|
||
}
|
||
/* USER CODE END StartLedManager */
|
||
}
|
||
|
||
/* StartExtinction function */
|
||
void StartExtinction(void const * argument)
|
||
{
|
||
/* USER CODE BEGIN StartExtinction */
|
||
int cptTimeOut = TIME_OUT;
|
||
/* Infinite loop */
|
||
// Savoir Lorsqu'un message arrive
|
||
// D<>compter le temps entre chaque message
|
||
// si on a un changement d'<27>tat sur busy ou si on re<72>oit un message valide.
|
||
// on remet le compteur <20> zero
|
||
// Sinon on incr<63>mente le compteur d'inactivit<69>
|
||
for(;;)
|
||
{
|
||
if(cptTimeOut == 0){
|
||
// Shutdown
|
||
HAL_GPIO_TogglePin(GPIOB,GPIO_PIN_5);
|
||
}
|
||
// On ne d<>cr<63>mente le compteur seulement si le robot est not<6F> comme NOT_BUSY
|
||
// Si celui-ci est BUSY on remet <20> jour le compteur <20> TIME_OUT et on reset le compteur
|
||
|
||
if(getDumberActivity() == NOT_BUSY)
|
||
cptTimeOut-=100;
|
||
else{
|
||
setDumberActivity(NOT_BUSY);
|
||
cptTimeOut = TIME_OUT;
|
||
}
|
||
osDelay(100);
|
||
}
|
||
/* USER CODE END StartExtinction */
|
||
}
|
||
|
||
/* StartControlLow function */
|
||
void StartControlLow(void const * argument)
|
||
{
|
||
/* USER CODE BEGIN StartControlLow */
|
||
MOTOR_INIT(MOTOR_LEFT);
|
||
|
||
MOTOR_INIT(MOTOR_RIGHT);
|
||
ENCODER_INIT();
|
||
Consigne consigne;
|
||
/* Infinite loop */
|
||
for(;;)
|
||
{
|
||
consigne = getDumberConsigne();
|
||
// BangBang Vitesse
|
||
if(speedL < consigne.motorLSpeed && distanceL < abs(consigne.motorLDistance *7)){
|
||
if(consigne.motorLDistance < 0){
|
||
MOTOR_LEFT_SET(MOTOR_BACKWARD,100);
|
||
}else{
|
||
MOTOR_LEFT_SET(MOTOR_FORWARD,100);
|
||
}
|
||
|
||
}else{
|
||
MOTOR_LEFT_SET(MOTOR_FORWARD,0);
|
||
}
|
||
if(speedR < consigne.motorRSpeed && distanceR < abs(consigne.motorRDistance *7) ){
|
||
if(consigne.motorRDistance < 0){
|
||
MOTOR_RIGHT_SET(MOTOR_BACKWARD,100);
|
||
}else{
|
||
MOTOR_RIGHT_SET(MOTOR_FORWARD,100);
|
||
}
|
||
}else{
|
||
MOTOR_RIGHT_SET(MOTOR_FORWARD,0);;
|
||
}
|
||
|
||
osDelay(1);
|
||
}
|
||
/* USER CODE END StartControlLow */
|
||
}
|
||
|
||
/* StartWatchDogCtl function */
|
||
void StartWatchDogCtl(void const * argument)
|
||
{
|
||
/* USER CODE BEGIN StartWatchDogCtl */
|
||
int cptWd;
|
||
/* Infinite loop */
|
||
for(;;)
|
||
{
|
||
if(getDumberWdActive() == WD_ACTIVE){
|
||
cptWd = getDumberCptWd();
|
||
setDumberCptWd(cptWd+5);
|
||
if(cptWd > (WD_BASE_TIME*WD_TRYING_NUMBER)){
|
||
setDumberState(DISABLED);
|
||
}
|
||
}
|
||
osDelay(5);
|
||
}
|
||
/* USER CODE END StartWatchDogCtl */
|
||
}
|
||
|
||
/* USER CODE BEGIN Application */
|
||
|
||
/* USER CODE END Application */
|
||
|
||
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
|