// -------------------------------------------------------------------------- // Projet POBOT Carte CNP. --- // Routines de Contrôle Moteurs via Drivers L298. Version 1.0. --- // --- // Créé le 16 Mars 2006 par Club POBOT. --- // --- // Gestion de 2 drivers L298 basés sur l'utilisation du Timer 2 et --- // de ses deux comparateurs. --- // --- // Cible : ATMega16/32. Quartz : 16 MHz. --- // Compilateur : avr-gcc (WinAvr) --- // --- // -------------------------------------------------------------------------- // Date Nom Version Commentaires --- // 31/03/06 G.Spinelli V1.1 Basé sur Driver Eric Pascual --- // 05/04/06 G.Spinelli V1.2 Ajout Arrêt Enable Moteur par Timer.--- // -------------------------------------------------------------------------- #include "CarteCNP.h" // ----- Déclaration des Variables Globales ----- volatile long lOdo_M1, lOdo_M2; // Odométrie absolue des moteurs volatile long lOdo_Robot; // Odométrie absolue du centre géométrique du robot volatile struct structFlags sMotor_Flags; #define bMotor_HalfStep sMotor_Flags.bit4 // Indique si 1/2 pas fini. #define bMotor_EmergencyStop sMotor_Flags.bit5 // Indique si Stop d'Urgence. // ----- Déclaration des Constantes Locales ----- #define SPEED_UPDATE_PERIOD 500 // -- Phases moteurs. Normallement : 0x0A,0x02,0x06,0x04,0x05,0x01,0x09,0x08 (Droit) -- // -- Phases moteurs. Normallement : 0x08,0x09,0x01,0x05,0x04,0x06,0x02,0x0A (Gauche) -- #define STEPS_COUNT 8 // -- Gestion Moteur 1 (ou Moteur Droit) -- #define MASK_PA_M1 0xFE // Seul le Bit PA0 est utilisé. unsigned char szStepPhasePA_M1[STEPS_COUNT] = { 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x01 }; #define MASK_PB_M1 0xCB // Seul les Bit PB2, 4 & 5 sont utilisés. unsigned char szStepPhasePB_M1[STEPS_COUNT] = { 0x10, 0x10, 0x14, 0x04, 0x24, 0x20, 0x20, 0x00 }; // --- Gestion Moteur 2 (ou Moteur Gauche) -- #define MASK_PA_M2 0xF9 // Seul les Bits PA2 & 1 sont utilisés. unsigned char szStepPhasePA_M2[STEPS_COUNT] = { 0x00, 0x02, 0x02, 0x02, 0x00, 0x04, 0x04, 0x04 }; #define MASK_PB_M2 0x3F // Seul les Bit PB7 & 6 sont utilisés. unsigned char szStepPhasePB_M2[STEPS_COUNT] = { 0x80, 0x80, 0x00, 0x40, 0x40, 0x40, 0x00, 0x80 }; // ----- Déclaration des Variables Locales ----- signed char cMotor_StepM1, cMotor_StepM2; // Sélecteurs de phases signed char cMotor_SensM1, cMotor_SensM2; // Sens de chaque Moteurs. long lMotor_ObjectiveM1, lMotor_ObjectiveM2; // Objectif en pas. long lOdo_TempM1, lOdo_TempM2; // Odometrie en cours. volatile unsigned char cMotor_Speed; // Vitesse actuelle unsigned char cMotor_RefSpeed; // Consignes de vitesse des moteurs unsigned int iMotor_DeltaSteps; // Nombre de Pas pour la décélération. /* Gestionnaire de l'interruption du Timer2 s'occupant des Moteurs M1 & M2. L'interruption est reçue lorsque le timer est égal à une valeur donnée. */ SIGNAL(SIG_OUTPUT_COMPARE2) { static unsigned int iElapsed_M = 0; static unsigned char cPortA, cPortB, cCounter; LED_ON(); // -- Ajustement de vitesse par rapport à la consigne, si nécessaire -- if (cMotor_Speed != cMotor_RefSpeed) { // ... et si le moment est venu de le faire (délai d'accélération) if (iElapsed_M >= SPEED_UPDATE_PERIOD) { if (cMotor_Speed < cMotor_RefSpeed) { cMotor_Speed++ ; iMotor_DeltaSteps++; } else { cMotor_Speed-- ; iMotor_DeltaSteps--; } iElapsed_M = 0; bMotor_Run = TRUE; } } if (cMotor_Speed != 0) { // -- Sélection du prochain pattern de phases & MAJ Compteur Odométrique -- cMotor_StepM1 = (cMotor_StepM1 + cMotor_SensM1) & (STEPS_COUNT - 1); cMotor_StepM2 = (cMotor_StepM2 + cMotor_SensM2) & (STEPS_COUNT - 1); bMotor_HalfStep = !bMotor_HalfStep; // -- Envoi du nouveau pattern de phases au contrôle du moteur -- cPortA = (PORTA & (MASK_PA_M1 & MASK_PA_M2)) | (szStepPhasePA_M1[cMotor_StepM1] | szStepPhasePA_M2[cMotor_StepM2]); cPortB = (PORTB & (MASK_PB_M1 & MASK_PB_M2)) | (szStepPhasePB_M1[cMotor_StepM1] | szStepPhasePB_M2[cMotor_StepM2]); PORTA = cPortA; PORTB = cPortB; // -- Calcul des Odometrie & Gestion arrêt si Objectif défini -- if (bMotor_HalfStep == FALSE) { lOdo_M1 = lOdo_M1 + cMotor_SensM1; lOdo_M2 = lOdo_M2 + cMotor_SensM2; lOdo_Robot = lOdo_Robot + ((cMotor_SensM1 + cMotor_SensM2)/2); lOdo_TempM1++; } } // -- Réarmement de l'Output Compare Timer avec la consigne de vitesse du moteur. -- // -- => Timer2 est déjà reparti en incrémentant à partir de 0. -- cCounter = 0xFF - (2 * cMotor_Speed); OCR2 = cCounter; iElapsed_M += cCounter; // -- Si nécessaire, teste si fin de trajectoire en vue ou ateinte -- if (bMotor_CommandRun == TRUE) { if ((lOdo_TempM1 + iMotor_DeltaSteps) > lMotor_ObjectiveM1) { cMotor_RefSpeed = 0; } if (lOdo_TempM1 >= lMotor_ObjectiveM1) { TIMSK &= ~_BV(OCIE2); // Disable IT Ouput Compare Timer2. TCCR2 = 0; // Timer2 Stoppé. cMotor_Speed = 0; bMotor_Run = FALSE; bMotor_CommandRun = FALSE; iTimer_CptM = TIME_DISABLE; // Attente avant RAZ Enable. } } // -- Teste si Arrêt d'Urgence -- if (bMotor_EmergencyStop == TRUE) { bMotor_EmergencyStop = FALSE; TIMSK &= ~_BV(OCIE2); // Disable IT Ouput Compare Timer2. TCCR2 = 0; // Timer2 Stoppé. cMotor_Speed = 0; bMotor_Run = FALSE; bMotor_CommandRun = FALSE; iTimer_CptM = TIME_DISABLE; // Attente avant RAZ Enable. } LED_OFF(); } // ---------------------------------------------- // ----- Gestion du Moteur lui-même ----- // ---------------------------------------------- // --- Initialisation Variables & Moteurs --- void Motors_Init(void) { cMotor_StepM1 = cMotor_StepM2 = 0; cMotor_SensM1 = cMotor_SensM2 = MOTOR_NULL; cMotor_RefSpeed = cMotor_Speed = 0; iMotor_DeltaSteps = 0; lOdo_M1 = lOdo_M2 = lOdo_Robot = 0; bMotor_HalfStep = FALSE; bMotor_On = FALSE; bMotor_Run = FALSE; bMotor_CommandRun = FALSE; PORTA = (PORTA & (MASK_PA_M1 & MASK_PA_M2)) | (szStepPhasePA_M1[0] | szStepPhasePA_M2[0]); PORTB = (PORTB & (MASK_PB_M1 & MASK_PB_M2)) | (szStepPhasePB_M1[0] | szStepPhasePB_M2[0]); TCCR2 = 0; // Timer 2 Stoppé. } // -- Activation des moteurs -- // -- Départ Timer 2 en CTC Mode & Résolution de 8 µS (à 16MHz, Presc=128). -- void Motors_Enable(void) { disable_interrupt(); pEnable_DM |= bEnable_DM; // Enable Drivers. bMotor_On = TRUE; OCR2 = 0; // Consigne Départ Timer. TCCR2 = _BV(WGM21) | _BV(CS22) | _BV(CS20); TIMSK |= _BV(OCIE2); // Enable IT Ouput Compare Timer2. iTimer_CptM = 0; enable_interrupt(); } // -- Désactivation des moteurs -- void Motors_Disable(void) { disable_interrupt(); TIMSK &= ~_BV(OCIE2); // Disable IT Ouput Compare Timer2. TCCR2 = 0; // Timer2 Stoppé. bMotor_Run = FALSE; pEnable_DM &= ~bEnable_DM; // Disable Drivers. bMotor_On = FALSE; enable_interrupt(); } // -- Arrêt en douceur moteurs, mais sans enlever le signal Enable -- void Motors_Stop(void) { cMotor_RefSpeed = 0; } // -- Arrêt en urgence des moteurs, mais sans enlever le signal Enable -- void Motors_Emergency_Stop(void) { bMotor_EmergencyStop = TRUE; } // -- Arrêt complet en douceur du moteurs & du Driver -- void Motors_Full_Stop(void) { Motors_Stop(); while (cMotor_Speed != 0) { } //Motors_Disable(); } // ------------------------------------------------------------ // ----- Gestion 'Bas Niveau' des déplacements du Robot ----- // ------------------------------------------------------------ // --- Demande au Moteur de se déplacer à une vitesse donnée --- unsigned char Motors_Move(unsigned char cSpeed, signed char cSensD, signed char cSensG) { if (bMotor_Run == FALSE) { cMotor_RefSpeed = cSpeed; iMotor_DeltaSteps = 0; lMotor_ObjectiveM1 = lMotor_ObjectiveM2 = 0; cMotor_SensM1 = cSensD; cMotor_SensM2 = cSensG; Motors_Enable(); } else { if ((cMotor_SensM1 != cSensD) || (cMotor_SensM2 != cSensG)) { return(FALSE); } else { cMotor_RefSpeed = cSpeed; } } bMotor_CommandRun = FALSE; // Ce n'est pas une Command devant finir automatiquement. return(TRUE); } // --- Demande au Moteur de se déplacer avec une vitesse donnée sur un objectif donné (en pas) --- unsigned char Motors_GoStep(unsigned char cSpeed, long lStepD, long lStepG) { if (bMotor_Run == FALSE) { cMotor_RefSpeed = cSpeed; iMotor_DeltaSteps = 0; lOdo_TempM1 = lOdo_TempM2 = 0; if (lStepD > 0) { cMotor_SensM1 = MOTOR_FWD; } else { if (lStepD == 0) { cMotor_SensM1 = MOTOR_NULL; } else { cMotor_SensM1 = MOTOR_REV; } } if (lStepG > 0) { cMotor_SensM2 = MOTOR_FWD; } else { if (lStepG == 0) { cMotor_SensM2 = MOTOR_NULL; } else { cMotor_SensM2 = MOTOR_REV; } } lMotor_ObjectiveM1 = (cMotor_SensM1 * lStepD); //lMotor_ObjectiveM2 = (cMotor_SensM2 * lStepG); Motors_Enable(); } else { if ((lMotor_ObjectiveM1 <= (cMotor_SensM1 * lStepD)) || (lMotor_ObjectiveM2 <= (cMotor_SensM2 * lStepG))) { return(FALSE); } else { cMotor_RefSpeed = cSpeed; lMotor_ObjectiveM1 = (cMotor_SensM1 * lStepD); lMotor_ObjectiveM2 = (cMotor_SensM2 * lStepG); } } bMotor_CommandRun = TRUE; // Commande devant finir automatiquement. return(TRUE); } // ------------------------------------------------------------ // ----- Gestion 'Haut Niveau' des déplacements du Robot ----- // ------------------------------------------------------------ // -- Avance/Recul rectiligne d'un nombre de pas donné, avec démarrage et arrêts progressifs -- void Motors_Straight_Line(unsigned char cSpeed, long lSteps) { Motors_GoStep(cSpeed, lSteps, lSteps); } // -- Rotation vers la gauche, sans mouvement du centre -- void Motors_Turn_Left(unsigned char cSpeed, long lSteps) { Motors_GoStep(cSpeed, lSteps, -lSteps); } // -- Rotation vers la droite, sans mouvement du centre -- void Motors_Turn_Right(unsigned char cSpeed, long lSteps) { Motors_GoStep(cSpeed, -lSteps, lSteps); } void Motors_Quarter_Turn_Left(unsigned char cSpeed) { Motors_GoStep(cSpeed, QUART_ROTATION_PAS, -QUART_ROTATION_PAS); } void Motors_Quarter_Turn_Right(unsigned char cSpeed) { Motors_GoStep(cSpeed, -QUART_ROTATION_PAS, QUART_ROTATION_PAS); } void Motors_Half_Turn_Left(unsigned char cSpeed) { Motors_GoStep(cSpeed, DEMI_ROTATION_PAS, -DEMI_ROTATION_PAS); } void Motors_Half_Turn_Rigth(unsigned char cSpeed) { Motors_GoStep(cSpeed, -DEMI_ROTATION_PAS, DEMI_ROTATION_PAS); }