123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539 |
- /**
- ******************************************************************************
- * @file motor.c
- * @author Lucien Senaneuch
- * @version V1.0
- * @date 16-mai-2016
- * @brief Gestion des moteurs du robots
- * Ce fichier contient les fonctions lié à la gestions du moteur :
- * - L'initialisation des PIN utilisé par les moteurs et encodeurs
- * - L'initialisation du Timer2 servant de bases de temps aux PWM pour
- * moteur1 et moteur2
- * - L'initialisation du Timer3 servant à échantilloner les capteurs magnétique de position
- * des 2 moteurs.
- * - La commandes des moteurs dans leurs 3 modes de fonctionnement (AVANT, ARRIERE, FREIN)
- * et de leurs vitesse (valeur de PWM entre 0 et 255).
- *
- * @note TABLE DE VERITEE DU DRIVER MOTEUR
- *
- * ENABLE | INPUTS | Moteurs
- * | |
- * 0 | X | Roue Libre
- *--------------------------------------------------------------------
- * | CMDA & !CMDB | Avant
- * 1 | !CMDA & CMDB | Arrière
- * | CMDA = CMDB | Frein
- ******************************************************************************
- ******************************************************************************
- */
-
- #include <stm32f10x.h>
-
- #include "system_dumby.h"
- #include "motor.h"
-
- /**
- * @brief Variables globales nombre d'incrémentation de capteur de position de la roue droite.
- * @note tourPositionG correspond exactement à la même valeur.
- */
- uint16_t tourD = 0;
-
- /**
- * @brief Variables globales nombre d'incrémentation de capteur de position de la roue gauche.
- * @note tourPositionG correspond exactement à la même valeur.
- */
- uint16_t tourG = 0;
- uint16_t tourPositionD;
- uint16_t tourPositionG;
-
- /**
- * @brief Variables globales des consignes de vitesses du moteur droit.
- * @note Variables utilisées dans le programme principal main.c
- */
- uint16_t G_speedRight=20;
-
- /**
- * @brief Variables globales des consignes de vitesses du moteur gauche.
- * @note Variables utilisées dans le programme principal main.c
- */
- uint16_t G_speedLeft=20;
-
- /**
- * @brief Variables globales des consignes de position du moteur gauche.
- * @note Variables utilisées dans le programme principal main.c
- */
- uint16_t G_lapsLeft;
-
- /**
- * @brief Variables globales des consignes de position du moteur droit.
- * @note Variables utilisées dans le programme principal main.c
- */
- uint16_t G_lapsRight;
-
- float integration1 = 0;
- float integration2 = 0;
- /**
- * @brief Coefficient PI
- */
- const float kp = 15;
- /**
- * @brief Coefficient PI
- */
- const float ki = 1.5;
- /**
- * @brief Valeurs de vitesse
- */
- float motD = 0, motG = 0;
- /**
- * @brief erreurs entre vitesse réelle et moteur (droite)
- */
- int erreurD;
-
- /**
- * @brief erreurs entre vitesse réelle et moteur (gauche)
- */
- int erreurG;
-
- uint16_t asservissement;
- uint16_t regulation_vitesseD, regulation_vitesseG;
- /** @addtogroup Projects
- * @{
- */
-
- /** @addtogroup Robot Motor
- * @{
- */
-
- /** @addtogroup Init Init GPIO - IT - Timers
- * @{
- */
-
- /**
- * @brief Assigne et définis le GPIO nécessaire pour le moteur.
- *
- * La fonction MAP_MotorPin va venir configurer le E/S du GPIO pour correspondre avec
- * le schéma électrique en ressource.
- * @note 2 pwm en alternate fonction : PA1,PA2. 4 entr�e timer.
- * PA8,PA9,PA10,PA11. 4 sortie ppull PB12,PB13,PB14,PB15
- *
- * @param None
- * @retval None
- *
- */
- void motorConfigure(void)
- {
- GPIO_InitTypeDef Init_Structure;
- TIM_TimeBaseInitTypeDef TIM2_TempsPWMsettings;
- TIM_OCInitTypeDef TIM2_Configure;
- TIM_ICInitTypeDef TIM_ICInitStructure;
- NVIC_InitTypeDef NVIC_InitStructure;
-
- // Configure les PIN A1 et A2 en output / alternate fonction
- Init_Structure.GPIO_Pin = GPIO_Pin_1|GPIO_Pin_2;
- Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
-
- Init_Structure.GPIO_Mode = GPIO_Mode_AF_PP;
- GPIO_Init(GPIOA, &Init_Structure);
-
- // Configure les PIN B12,B13,B14, et B15 en output ppull.
- Init_Structure.GPIO_Pin = GPIO_Pin_12|GPIO_Pin_13|GPIO_Pin_14|GPIO_Pin_15;
- Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
- Init_Structure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(GPIOB, &Init_Structure);
-
- // Configure les PIN A12 en output ppull - enable encodeurs
- Init_Structure.GPIO_Pin = GPIO_Pin_12;
- Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
- Init_Structure.GPIO_Mode = GPIO_Mode_Out_PP;
- GPIO_Init(GPIOA, &Init_Structure);
-
- // Configure les PIN A8,A9,A10, et A11 en input floating.
- Init_Structure.GPIO_Pin = GPIO_Pin_8|GPIO_Pin_9|GPIO_Pin_10|GPIO_Pin_11;
- Init_Structure.GPIO_Speed = GPIO_Speed_50MHz;
- Init_Structure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
- GPIO_Init(GPIOA, &Init_Structure);
-
- // Configuration du timer 2 (pwm moteurs*
- // On souhaite une résolution du PWM de 256 valeurs MOTOR1 TIM2
- TIM2_TempsPWMsettings.TIM_Period = 255;
- TIM2_TempsPWMsettings.TIM_Prescaler = 0;
- TIM2_TempsPWMsettings.TIM_ClockDivision=0;
- TIM2_TempsPWMsettings.TIM_CounterMode=TIM_CounterMode_Up;
-
- TIM_TimeBaseInit(TIM2, &TIM2_TempsPWMsettings);
-
- // Configuration du PWM sur le timer 2
- TIM2_Configure.TIM_OCMode=TIM_OCMode_PWM2;
- TIM2_Configure.TIM_OutputState = TIM_OutputState_Enable;
- TIM2_Configure.TIM_OutputNState = TIM_OutputNState_Enable;
- TIM2_Configure.TIM_Pulse = 256; // Constante initialisée à 256, pour un rapport cyclique nul
- TIM2_Configure.TIM_OCPolarity = TIM_OCPolarity_High;
-
- TIM_OC3Init(TIM2, &TIM2_Configure);
- TIM_OC3PreloadConfig(TIM2,TIM_OCPreload_Enable);
-
- TIM_OC2Init(TIM2, &TIM2_Configure);
- TIM_OC2PreloadConfig(TIM2,TIM_OCPreload_Enable);
-
- TIM_ARRPreloadConfig(TIM2, ENABLE);
- // Enable Counter
- TIM_Cmd(TIM2, ENABLE);
-
- TIM_CtrlPWMOutputs(TIM2,ENABLE);
-
- // Configuration de la capture de l'encodeur 1
- TIM_ICInitStructure.TIM_Channel = TIM_Channel_1;
- TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM_ICInitStructure.TIM_ICFilter = 0x0;
-
- TIM_ICInit(TIM1, &TIM_ICInitStructure);
- TIM_Cmd(TIM1, ENABLE);
- TIM_ITConfig(TIM1, TIM_IT_CC1, ENABLE);
-
- // Configuration de la capture de l'encodeur 1
- TIM_ICInitStructure.TIM_Channel = TIM_Channel_3;
- TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising;
- TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI;
- TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1;
- TIM_ICInitStructure.TIM_ICFilter = 0x0;
-
- TIM_ICInit(TIM1, &TIM_ICInitStructure);
- TIM_Cmd(TIM1, ENABLE);
- TIM_ITConfig(TIM1, TIM_IT_CC3, ENABLE);
-
- // Enable the TIM1 Capture interrupt
- NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
- NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
- NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
- NVIC_Init(&NVIC_InitStructure);
-
- // RAZ des variables
- regulation_vitesseD = 0;
- regulation_vitesseG = 0;
- asservissement =0;
- }
-
- /**
- * @}
- */
-
- /** @addtogroup Regulation Reg Speed
- * @{
- */
- /**
- * @brief Commande de plus haut niveau pour contrôler le moteur droit.
- *
- * @param Mode de fonctionnement du moteur, peut être égal aux constantes BRAKE, FORWARD, REVERSE définis dans moteur.h
- * @param Consigne de vitesse du moteur, défini par un pwm entre 0 et 255.
- * @retval None
- */
-
- void motorCmdRight(char mod, uint16_t pwm)
- {
- pwm = 256 - pwm;
-
- switch (mod) {
- case BRAKE:
- GPIO_SetBits(GPIOB, GPIO_Pin_12);
- GPIO_SetBits(GPIOB, GPIO_Pin_13);
- break;
- case FORWARD:
- GPIO_SetBits(GPIOB, GPIO_Pin_12);
- GPIO_ResetBits(GPIOB, GPIO_Pin_13);
- break;
-
- case REVERSE:
- GPIO_SetBits(GPIOB, GPIO_Pin_13);
- GPIO_ResetBits(GPIOB, GPIO_Pin_12);
- break;
-
- default:
- GPIO_ResetBits(GPIOB, 12);
- GPIO_ResetBits(GPIOB, 13);
- }
- TIM_SetCompare3(TIM2, pwm);
- }
-
- /**
- * @brief Commande de plus haut niveau pour contrôler le moteur Gauche.
- *
- * @param Mode de fonctionnement du moteur, peut être égal aux constantes BRAKE, FORWARD, REVERSE définis dans moteur.h
- * @param Consigne de vitesse du moteur, défini par un pwm entre 0 et 255.
- * @retval None
- */
-
- void motorCmdLeft(char mod, uint16_t pwm) {
- pwm = 256 - pwm;
- switch (mod) {
- case BRAKE:
- GPIO_SetBits(GPIOB, GPIO_Pin_14);
- GPIO_SetBits(GPIOB, GPIO_Pin_15);
- break;
- case FORWARD:
- GPIO_SetBits(GPIOB, GPIO_Pin_15);
- GPIO_ResetBits(GPIOB, GPIO_Pin_14);
- break;
-
- case REVERSE:
- GPIO_SetBits(GPIOB, GPIO_Pin_14);
- GPIO_ResetBits(GPIOB, GPIO_Pin_15);
- break;
-
- default:
- GPIO_ResetBits(GPIOB, GPIO_Pin_14);
- GPIO_ResetBits(GPIOB, GPIO_Pin_15);
- }
- TIM_SetCompare2(TIM2, pwm);
- }
-
- /**
- * @brief Commande de plus haut niveau pour contrôler la vitesse moteur Gauche.
- *
- * @param Consigne de vitesse du moteur, défini par un pwm entre 0 et 255.
- * @retval None
- */
- void motorSpeedUpdateLeft(uint16_t pwm) {
- pwm = 256 - pwm;
- TIM_SetCompare2(TIM2, pwm);
- }
-
- /**
- * @brief Commande de plus haut niveau pour contrôler la vitesse moteur Droit.
- *
- * @param Consigne de vitesse du moteur, défini par un pwm entre 0 et 255.
- * @retval None
- */
- void motorSpeedUpdateRight(uint16_t pwm) {
- pwm = 256 - pwm;
- TIM_SetCompare3(TIM2, pwm);
- }
-
- /**
- * @brief Régulation des deux moteurs.
- *
- * Modifie le le mode de fonctionnement des roues, puis
- * va mettre à jour les variables globale G_laps* et G_speed*
- * utilisé dans main.c pour l'asservissement.
- *
- * @param Mode de fonctionnement la roue droite (BRAKE, REVERSE, FORWARD)
- * @param Mode de fonctionnement la roue droite (BRAKE, REVERSE, FORWARD)
- * @param Nombre de tour de roue droite à effectuer.
- * @param Nombre de tour de roue gauche à effectuer.
- * @param Vitesse de la roue de droite.
- * @param Vitesse de la roue de gauche.
- *
- * @retval None
- */
- void motorRegulation(char modRight, char modLeft, uint16_t lapsRight,
- uint16_t lapsLeft, uint16_t speedRight, uint16_t speedLeft) {
- /*Moteur Droit*/
- switch (modRight) {
- case BRAKE:
- GPIO_SetBits(GPIOB, GPIO_Pin_12);
- GPIO_SetBits(GPIOB, GPIO_Pin_13);
- break;
- case FORWARD:
- GPIO_SetBits(GPIOB, GPIO_Pin_12);
- GPIO_ResetBits(GPIOB, GPIO_Pin_13);
- break;
-
- case REVERSE:
- GPIO_SetBits(GPIOB, GPIO_Pin_13);
- GPIO_ResetBits(GPIOB, GPIO_Pin_12);
- break;
-
- default:
- GPIO_ResetBits(GPIOB, 12);
- GPIO_ResetBits(GPIOB, 13);
- }
-
- /* Moteur Gauche */
- switch (modLeft) {
- case BRAKE:
- GPIO_SetBits(GPIOB, GPIO_Pin_14);
- GPIO_SetBits(GPIOB, GPIO_Pin_15);
- break;
- case FORWARD:
- GPIO_SetBits(GPIOB, GPIO_Pin_15);
- GPIO_ResetBits(GPIOB, GPIO_Pin_14);
- break;
-
- case REVERSE:
- GPIO_SetBits(GPIOB, GPIO_Pin_14);
- GPIO_ResetBits(GPIOB, GPIO_Pin_15);
- break;
-
- default:
- GPIO_ResetBits(GPIOB, GPIO_Pin_14);
- GPIO_ResetBits(GPIOB, GPIO_Pin_15);
- }
-
- if ((speedRight == 0 && lapsRight > 0)
- || (speedLeft == 0 && lapsLeft > 0)) {
- while (1);
- }
-
- G_lapsLeft = lapsLeft;
- G_speedLeft = speedLeft;
- G_lapsRight = lapsRight;
- G_speedRight = speedRight;
- asservissement = 1;
- tourPositionD = 0;
- tourPositionG = 0;
- }
-
- /**
- * @brief Gestion de l'interuption des capteurs de positions.
- *
- * Fonction de callback permettant d'incrémenter le nombre
- * de tour des deux roues.
- *
- * @param None
- * @retval None
- */
- void TIM1_CC_IRQHandler(void) {
- if (TIM_GetITStatus(TIM1, TIM_IT_CC1) == SET) {
- TIM_ClearITPendingBit(TIM1, TIM_IT_CC1);
- tourD++;
- tourPositionD++;
- }
-
- if (TIM_GetITStatus(TIM1, TIM_IT_CC3) == SET) {
- TIM_ClearITPendingBit(TIM1, TIM_IT_CC3);
- tourG++;
- tourPositionG++;
- }
- }
-
- /**
- * @brief Gestion de l'asservissement des moteurs
- *
- * @param None
- * @retval None
- */
- void motorManagement(void) {
- if (regulation_vitesseD) {
- erreurD = (signed int) G_speedRight - (signed int) tourD;
- motD = kp * erreurD + integration1;
- integration1 += ki * erreurD;
-
- if (motD > 255) motD = 255;
-
- if (motD < 0) motD = 0;
-
- motD = (uint16_t) motD;
- motorSpeedUpdateRight(motD);
- tourD = 0;
- regulation_vitesseD = 0;
-
- if (G_lapsRight - tourPositionD < 0) {
- motorCmdRight(BRAKE, 255);
- }
- }
-
- if (regulation_vitesseG) {
- erreurG = (signed int) G_speedLeft - (signed int) tourG;
- motG = kp * erreurG + integration2;
-
- integration2 += ki * erreurG;
-
- if (motG > 255) motG = 255;
-
- if (motG < 0) motG = 0;
-
- motG = (uint16_t) motG;
-
- motorSpeedUpdateLeft(motG);
- tourG = 0;
- regulation_vitesseG = 0;
-
- if (G_lapsLeft - tourPositionG < 0) {
- motorCmdLeft(BRAKE, 255);
- }
- }
-
- if (G_lapsLeft - tourPositionG < 0 && G_lapsRight - tourPositionD < 0
- && asservissement == 1) {
-
- motorCmdLeft(BRAKE, 255);
- motorCmdRight(BRAKE, 255);
- asservissement = 0;
- erreurD = 0;
- erreurG = 0;
- integration1 = 0;
- integration2 = 0;
- Dumber.busyState = FALSE;
- Dumber.cpt_inactivity = 0;
- }
- }
-
- /**
- * @}
- */
-
- /**
- * @}
- */
-
- /**
- * @}
- */
- /*void regulationCBK(void)
- {
- if (regulation_vitesseD == 1)
- {
- erreurD = (signed int)G_speedRight - (signed int)tourD;
- motD = kp * erreurD +integration1;
-
- integration1 += ki * erreurD;
-
- if (motD>255) motD=255;
-
- if (motD<0) motD=0;
-
- motD=(uint16_t)motD;
- majVitesseMotorD(motD);
- tourD = 0;
- regulation_vitesseD=0;
-
- if (G_lapsRight-tourPositionD < 0) cmdRightMotor(BRAKE,0);
- }
-
- if (regulation_vitesseG == 1)
- {
- erreurG = (signed int)G_speedLeft - (signed int)tourG;
- motG = kp* erreurG + integration2;
-
- integration2 += ki * erreurG;
-
- if(motG>255) motG=255;
-
- if(motG<0) motG=0;
-
- motG=(uint16_t)motG;
-
- majVitesseMotorG(motG);
- tourG = 0;
- regulation_vitesseG=0;
-
- if (G_lapsLeft-tourPositionG < 0) cmdLeftMotor(BRAKE,0);
- }
-
- if (G_lapsLeft-tourPositionG < 0 && G_lapsRight-tourPositionD < 0 && asservissement == 1)
- {
- cmdLeftMotor(BRAKE,0);
- cmdRightMotor(BRAKE,0);
-
- asservissement = 0;
- erreurD=0;
- erreurG=0;
- integration1=0;
- integration2=0;
- Dumber.busyState=FALSE;
- Dumber.cpt_inactivity = 0;
- }
- }*/
-
|