// Robot RLSK // 25.05.2023 : avec machine d'état #include #include #include "clock.h" // Base Robot RSLK : PWM et détecteurs de piste //============================================= // Pierre-Yves Rochat, 2018, EPFL, pyr@pyr.ch // LED du Launchpad MSP432 : #define LedRougeInit P1DIR|=(1<<0);P1OUT&=~(1<<0) #define LedRougeOn P1OUT|=(1<<0) #define LedRougeOff P1OUT&=~(1<<0) #define LedRougeToggle P1OUT^=(1<<0) #define LedCoulInit P2DIR|=(1<<0)|(1<<1)|(1<<2);P2OUT&=~((1<<0)|(1<<1)|(1<<2)) #define LedCoulRougeOn P2OUT|=(1<<0) #define LedCoulRougeOff P2OUT&=~(1<<0) #define LedCoulRougeToggle P2OUT^=(1<<0) #define LedCoulVertOn P2OUT|=(1<<1) #define LedCoulVertOff P2OUT&=~(1<<1) #define LedCoulVertToggle P2OUT^=(1<<1) #define LedCoulBleuOn P2OUT|=(1<<2) #define LedCoulBleuOff P2OUT&=~(1<<2) #define LedCoulBleuToggle P2OUT^=(1<<2) #define Pous1Init P1REN|=(1<<1); P1OUT|=(1<<1) #define Pous1On !(P1IN&(1<<1)) #define Pous2Init P1REN|=(1<<4); P1OUT|=(1<<4) #define Pous2On !(P1IN&(1<<4)) // Moteurs du robot : #define MoteurDroiteInit P6DIR|=(1<<4);P6OUT&=~(1<<4);P6DIR|=(1<<5);P6OUT&=~(1<5) #define MoteurDroiteOn P6OUT|=(1<<4) #define MoteurDroiteOff P6OUT&=~(1<<4) #define MoteurDroiteRecule P6OUT|=(1<<5) #define MoteurDroiteAvance P6OUT&=~(1<<5) #define MoteurGaucheInit P4DIR|=(1<<6);P4OUT&=~(1<<6);P1DIR|=(1<<5);P1OUT&=~(1<<5) #define MoteurGaucheOn P4OUT|=(1<<6) #define MoteurGaucheOff P4OUT&=~(1<<6) #define MoteurGaucheRecule P1OUT|=(1<<5) #define MoteurGaucheAvance P1OUT&=~(1<<5) #define DetIrOn P3DIR|=(1<<7);P3OUT|=(1<<7) // Variables globales : volatile uint16_t PwmLedRouge; // géré par TA0 CCR1 volatile uint16_t PwmCoulRouge; // géré par TA0 CCR2 volatile uint16_t PwmCoulVert; // géré par TA0 CCR3 volatile uint16_t PwmCoulBleu; // géré par TA0 CCR4 volatile int16_t CommandeDroite; // géré par TA1 CCR1 volatile int16_t CommandeGauche; // géré par TA1 CCR2 volatile int16_t ValDetecteurs[8]; // int16_t LimiteDetecteurs[] = {12000, 10000, 10000, 10000, 10000, 10000, 10000, 12000}; // int8_t BitDetecteurs[8]; volatile uint8_t FinCycle; #define PeriodeCycle 50000 #define LimiteMoteur 100 #define LimiteLed 100 // Communication avec l'afficheur : uint16_t idxDet; void InitSerie() { // initialise la ligne série P3SEL0 |= BIT2 | BIT3; // UART2 pin en fonction secondaire P3.2 et P3.3 UCA2CTLW0 |= UCSWRST; // Reset de l'automate de la liaison série UCA2CTLW0 |= UCSSEL__SMCLK; // sélection de l'horloge UCA2BR0 = (uint8_t)((uint32_t)48000000/(16*4*9600))%256; // choisit le baud-rate UCA2BR1 = (uint8_t)((uint32_t)48000000/(16*4*9600))/256; UCA2MCTLW = 0x1000 | UCOS16 | 0x0020; UCA2CTLW0 &= ~UCSWRST; // Initialise l'automate de la liaison série idxDet = 0; } char car; void AfficheDetecteurs() { // envoie successivement les valeurs des détecteurs if (UCA2IFG & UCTXIFG) { // prêt à l'envoi UCA2TXBUF = ((((7-idxDet) & 0b1111)+8)<<4) | (ValDetecteurs[idxDet]>>11); idxDet++; if (idxDet>=8) idxDet = 0; } } int32_t Centre; #define FAC_DROITE1 1 #define FAC_DROITE2 1 #define FAC_DROITE3 1 #define FAC_DROITE4 1 void CalculeCentre() { Centre = 0; Centre += ValDetecteurs[0]*FAC_DROITE4; Centre += ValDetecteurs[1]*FAC_DROITE3; Centre += ValDetecteurs[2]*FAC_DROITE2; Centre += ValDetecteurs[3]*FAC_DROITE1; Centre -= ValDetecteurs[4]*FAC_DROITE1; Centre -= ValDetecteurs[5]*FAC_DROITE2; Centre -= ValDetecteurs[6]*FAC_DROITE3; Centre -= ValDetecteurs[7]*FAC_DROITE4; } void AfficheCentre() { PwmLedRouge = 0; PwmCoulBleu = 0; if (Centre < 0) { PwmLedRouge = -Centre/(1+3+5+7); } if (Centre > 0) { PwmCoulBleu = Centre/(1+3+5+7); } } uint8_t Poussoir() { // poussoir de la carte verte, ne plus utiliser ! if (UCA2IFG & UCRXIFG) { car = UCA2RXBUF; if (car=='P') return 1; } return 0; } #define FACTEUR_P 50 uint8_t etat; uint16_t timer = 0; // Programme principal : //====================== void main() { WDTCTL = WDTPW | WDTHOLD; Clock_Init48MHz(); P3SEL0 = 0; P3SEL1 = 0; LedRougeInit; LedCoulInit; Pous1Init; Pous2Init; PwmLedRouge; PwmCoulRouge = PwmCoulVert = PwmCoulBleu = 0; MoteurGaucheInit; MoteurDroiteInit; CommandeDroite = CommandeGauche = 0; InitSerie(); FinCycle = 0; DetIrOn; // allume l'IR des détecteurs // Initialisation des timers : NVIC->ISER[0] = 1 << ((TA0_0_IRQn) & 31); TA0CTL = TASSEL_2 | MC_2 | ID_1 ; // SMCLK, mode continu, DIV 2 TA0CCTL0 = CCIE; // interruption TA0CCR0 TA0CCR0 = PeriodeCycle; // période du PWM et des mesures NVIC->ISER[0] = 1 << ((TA0_N_IRQn) & 31); TA0CCTL1 = CCIE; // interruption TA0 CCR1 TA0CCR1 = 0; TA0CCTL2 = CCIE; TA0CCR2 = 0; TA0CCTL3 = CCIE; TA0CCR3 = 0; TA0CCTL4 = CCIE; TA0CCR4 = 0; NVIC->ISER[0] = 1 << ((TA1_0_IRQn) & 31); TA1CTL = TASSEL_2 | MC_2 | ID_1 ; TA1CCTL0 = CCIE; TA1CCR0 = PeriodeCycle; NVIC->ISER[0] = 1 << ((TA1_N_IRQn) & 31); TA1CCTL1 = CCIE; TA1CCR1 = 0; TA1CCTL2 = CCIE; TA1CCR2 = 0; // Initialisation des ports : NVIC->ISER[1] = 1 << ((PORT2_IRQn) & 31); P2IES |= (1<<3); P2IE |= (1<<3); P2IFG &=~(1<<3); // Détecteur 2 sur P2.3 P2IES |= (1<<4); P2IE |= (1<<4); P2IFG &=~(1<<4); // Détecteur 6 sur P2.4 P2IES |= (1<<6); P2IE |= (1<<6); P2IFG &=~(1<<6); // Détecteur 7 sur P2.7 NVIC->ISER[1] = 1 << ((PORT3_IRQn) & 31); P3IES |= (1<<5); P3IE |= (1<<5); P3IFG &=~(61<<5); // Détecteur 0 sur P3.5 NVIC->ISER[1] = 1 << ((PORT5_IRQn) & 31); P5IES |= (1<<1); P5IE |= (1<<1); P5IFG &=~(1<<1); // Détecteur 1 sur P5.1 P5IES |= (1<<6); P5IE |= (1<<6); P5IFG &=~(1<<6); // Détecteur 5 sur P5.6 NVIC->ISER[1] = 1 << ((PORT6_IRQn) & 31); P6IES |= (1<<6); P6IE |= (1<<6); P6IFG &=~(1<<6); // Détecteur 4 sur P6.6 P6IES |= (1<<7); P6IE |= (1<<7); P6IFG &=~(1<<7); // Détecteur 3 sur P6.7 __enable_interrupt(); enum {Choix=0, ChoixP1=1, ChoixP2=2, ChoixP1P2=3, AttentePiste=8, Piste=9, Croisement=10, AttentePassageJaune=16, PassageJaune=17, AttenteBonus=32, Bonus=33 }; etat = Choix; while(1) { // boucle principale : calcul des PWM pour le cycle suivant if (FinCycle) { FinCycle = 0; AfficheDetecteurs(); CalculeCentre(); AfficheCentre(); CommandeDroite = 0; CommandeGauche = 0; switch (etat){ case Choix: if (Pous1On) { etat = ChoixP1; } if (Pous2On) { etat = ChoixP2; } break; case ChoixP1: if (Pous2On) { etat = ChoixP1P2; } if (!Pous1On) { timer = 100; etat = AttentePiste; } break; case ChoixP2: if (Pous1On) { etat = ChoixP1P2; } if (!Pous2On) { timer = 100; etat = AttentePassageJaune; } break; case ChoixP1P2: if ((!Pous1On) && (!Pous2On)) {timer = 100; etat = AttenteBonus; } break; case AttentePiste: if (timer==0){ etat = Piste; } break; case Piste: CommandeDroite = 7000+(Centre/FACTEUR_P); CommandeGauche = 7000-(Centre/FACTEUR_P); if (Pous2On) { etat = Choix; } // ... break; case AttentePassageJaune: if (Pous1On) { etat = Choix; } // ... break; case AttenteBonus: // ... break; } if (timer>0) timer--; } } } // Routines d'interruptions : //=========================== void PORT2_IRQHandler() { if (P2IFG & (1<<3)) { P2IFG &=~(1<<3); ValDetecteurs[2] = TA0R/2; } if (P2IFG & (1<<4)) { P2IFG &=~(1<<4); ValDetecteurs[6] = TA0R/2; } if (P2IFG & (1<<6)) { P2IFG &=~(1<<6); ValDetecteurs[7] = TA0R/2; } } void PORT3_IRQHandler() { if (P3IFG & (1<<5)) { P3IFG &=~(1<<5); ValDetecteurs[0] = TA0R/2; } } void PORT5_IRQHandler() { if (P5IFG & (1<<1)) { P5IFG &=~(1<<1); ValDetecteurs[1] = TA0R/2; } if (P5IFG & (1<<6)) { P5IFG &=~(1<<6); ValDetecteurs[5] = TA0R/2; } } void PORT6_IRQHandler() { if (P6IFG & (1<<6)) { P6IFG &=~(1<<6); ValDetecteurs[4] = TA0R/2; } if (P6IFG & (1<<7)) { P6IFG &=~(1<<7); ValDetecteurs[3] = TA0R/2; } } // Timer A0 : PWM des LED et début des cycles des capteurs void TA0_0_IRQHandler(void){ // TA0 CCR0 : fin (et début) des cycles volatile uint8_t i; P3DIR |= (1<<5); P3OUT |= (1<<5); // charge le condensateur P5DIR |= (1<<1); P5OUT |= (1<<1); P2DIR |= (1<<3); P2OUT |= (1<<3); P6DIR |= (1<<7); P6OUT |= (1<<7); P6DIR |= (1<<6); P6OUT |= (1<<6); P5DIR |= (1<<6); P5OUT |= (1<<6); P2DIR |= (1<<4); P2OUT |= (1<<4); P2DIR |= (1<<6); P2OUT |= (1<<6); TA0CCTL0 &=~ CCIFG; TA0R = 0; // début du cycle suivant if (PwmLedRouge>LimiteLed) LedRougeOn; // CCR1 if (PwmCoulRouge>LimiteLed) LedCoulRougeOn; //CCR2 if (PwmCoulVert>LimiteLed) LedCoulVertOn; //CCR3 if (PwmCoulBleu>LimiteLed) LedCoulBleuOn; // CCR4 TA0CCR1 = PwmLedRouge; TA0CCR2 = PwmCoulRouge; TA0CCR3 = PwmCoulVert; TA0CCR4 = PwmCoulBleu; FinCycle = 1; for (i=0; i<20; i++){} // charge des condensateurs P3DIR &=~(1<<5); // détecteur en entrée P5DIR &=~(1<<1); P2DIR &=~(1<<3); P6DIR &=~(1<<7); P6DIR &=~(1<<6); P5DIR &=~(1<<6); P2DIR &=~(1<<4); P2DIR &=~(1<<6); } void TA0_N_IRQHandler(void){ TA0CCTL0 &=~ CCIFG; switch (TA0IV) { case 2 : LedRougeOff; break; // TA0 CCR1 case 4 : LedCoulRougeOff; break; // TA0 CCR2 case 6 : LedCoulVertOff; break; // TA0 CCR3 case 8 : LedCoulBleuOff; break; // TA0 CCR4 default : break; } } // Timer A1 : PWM des moteurs void TA1_0_IRQHandler(void){ // TA1CCR0 : début des cycles PWM TA1CCTL0 &=~CCIFG; TA1R = 0; // début du cycle suivant MoteurDroiteOff; MoteurGaucheOff; if (CommandeDroite>LimiteMoteur) { TA1CCR1 = CommandeDroite*2; MoteurDroiteAvance; MoteurDroiteOn; } if (CommandeDroite<-LimiteMoteur) { TA1CCR1 = -CommandeDroite*2; MoteurDroiteRecule; MoteurDroiteOn; } if (CommandeGauche>LimiteMoteur) { TA1CCR2 = CommandeGauche*2; MoteurGaucheAvance; MoteurGaucheOn; } if (CommandeGauche<-LimiteMoteur) { TA1CCR2 = -CommandeGauche*2;MoteurGaucheRecule; MoteurGaucheOn; } } void TA1_N_IRQHandler(void){ TA1CCTL0 &=~CCIFG; switch (TA1IV) { case 2 : MoteurDroiteOff; break; // TA1 CCR1 case 4 : MoteurGaucheOff; break; // TA1 CCR2 default : break; } }