Code pour le projet de robot autonome : Partie 1

Afin de vous simplifier le travail dans le cadre du projet robot, les éléments de code de taille importante à insérer au fur et à mesure du projet sont regroupés sur cette page. A noter qu’en cas d’incohérence avec le sujet, c’est celui-ci qui fait est juste a priori. Si c’est le cas, merci de me le signaler.

Pour formater proprement vos fichiers dans MPLABX, vous pouvez utiliser le raccourci de formatage automatique ALT+MAJ+F, après avoir au préalable sélectionné le texte à formater.

Main initial

#include <stdio.h>
#include <stdlib.h>
#include <xc.h>
#include “ChipConfig.h”
#include “IO.h”

int main (void){
/***************************************************************************************************/
//Initialisation de l’oscillateur
/****************************************************************************************************/
InitOscillator();

/****************************************************************************************************/
// Configuration des entrées sorties
/****************************************************************************************************/
InitIO();

LED_BLANCHE = 1;
LED_BLEUE = 1;
LED_ORANGE = 1;

/****************************************************************************************************/
// Boucle Principale
/****************************************************************************************************/
while(1){
} // fin main
}

Partie Timer

#include <xc.h>
#include “timer.h”
#include “IO.h”

//Initialisation d’un timer 32 bits
void InitTimer23(void) {
T3CONbits.TON = 0; // Stop any 16-bit Timer3 operation
T2CONbits.TON = 0; // Stop any 16/32-bit Timer3 operation
T2CONbits.T32 = 1; // Enable 32-bit Timer mode
T2CONbits.TCS = 0; // Select internal instruction cycle clock
T2CONbits.TCKPS = 0b00; // Select 1:1 Prescaler
TMR3 = 0x00; // Clear 32-bit Timer (msw)
TMR2 = 0x00; // Clear 32-bit Timer (lsw)
PR3 = 0x0262; // Load 32-bit period value (msw)
PR2 = 0x5A00; // Load 32-bit period value (lsw)
IPC2bits.T3IP = 0x01; // Set Timer3 Interrupt Priority Level
IFS0bits.T3IF = 0; // Clear Timer3 Interrupt Flag
IEC0bits.T3IE = 1; // Enable Timer3 interrupt
T2CONbits.TON = 1; // Start 32-bit Timer
/* Example code for Timer3 ISR */
}

//Interruption du timer 32 bits sur 2-3
void __attribute__((interrupt, no_auto_psv)) _T3Interrupt(void) {
IFS0bits.T3IF = 0; // Clear Timer3 Interrupt Flag
LED_ORANGE = !LED_ORANGE;
}

//Initialisation d’un timer 16 bits
void InitTimer1(void)
{
//Timer1 pour horodater les mesures (1ms)
T1CONbits.TON = 0; // Disable Timer
T1CONbits.TCKPS = 0b01; //Prescaler
//11 = 1:256 prescale value
//10 = 1:64 prescale value
//01 = 1:8 prescale value
//00 = 1:1 prescale value
T1CONbits.TCS = 0; //clock source = internal clock
PR1 = 0x1388;

IFS0bits.T1IF = 0; // Clear Timer Interrupt Flag
IEC0bits.T1IE = 1; // Enable Timer interrupt
T1CONbits.TON = 1; // Enable Timer
}

//Interruption du timer 1
void __attribute__((interrupt, no_auto_psv)) _T1Interrupt(void)
{
IFS0bits.T1IF = 0;
LED_BLANCHE = !LED_BLANCHE;
}

Robot.h

#ifndef ROBOT_H
#define ROBOT_H
typedef struct robotStateBITS {
union {

struct {
unsigned char taskEnCours;
float vitesseGaucheConsigne;
float vitesseGaucheCommandeCourante;
float vitesseDroiteConsigne;
float vitesseDroiteCommandeCourante;}
;}
;} ROBOT_STATE_BITS;

extern volatile ROBOT_STATE_BITS robotState;
#endif /* ROBOT_H */

ToolBox

#include “Toolbox.h”
float Abs(float value)
{
if (value >= 0)
return value;
else return -value;
}

float Max(float value, float value2)
{
if (value > value2)
return value;
else
return value2;
}

float Min(float value, float value2)
{
if (value < value2)
return value;
else
return value2;
}

float LimitToInterval(float value, float lowLimit, float highLimit)
{
if (value > highLimit)
value = highLimit;
else if (value < lowLimit)
value = lowLimit;

return value;
}

float RadianToDegree(float value)
{
return value / PI * 180.0;
}

float DegreeToRadian(float value)
{
return value * PI / 180.0;
}

Partie PWM

//Partie PWM
#include <xc.h> // library xc.h inclut tous les uC
#include “IO.h”
#include “PWM.h”
#include “Robot.h”
#include “ToolBox.h”

#define PWMPER 40.0
unsigned char acceleration = 20;

void InitPWM(void)
{
PTCON2bits.PCLKDIV = 0b000; //Divide by 1
PTPER = 100*PWMPER; //Période en pourcentage

//Réglage PWM moteur 1 sur hacheur 1
IOCON1bits.POLH = 1; //High = 1 and active on low =0
IOCON1bits.POLL = 1; //High = 1
IOCON1bits.PMOD = 0b01; //Set PWM Mode to Redundant
FCLCON1 = 0x0003; //Désactive la gestion des faults

//Reglage PWM moteur 2 sur hacheur 6
IOCON6bits.POLH = 1; //High = 1
IOCON6bits.POLL = 1; //High = 1
IOCON6bits.PMOD = 0b01; //Set PWM Mode to Redundant
FCLCON6 = 0x0003; //Désactive la gestion des faults

/* Enable PWM Module */
PTCONbits.PTEN = 1;
}

void PWMSetSpeed(float vitesseEnPourcents)
{
robotState.vitesseGaucheCommandeCourante = vitesseEnPourcents;
MOTEUR_GAUCHE_L_PWM_ENABLE = 0; //Pilotage de la pin en mode IO
MOTEUR_GAUCHE_L_IO_OUTPUT = 1; //Mise à 1 de la pin
MOTEUR_GAUCHE_H_PWM_ENABLE = 1; //Pilotage de la pin en mode PWM
MOTEUR_GAUCHE_DUTY_CYCLE = Abs(robotState.vitesseGaucheCommandeCourante*PWMPER);
}

Definition des pins Hacheurs

//Définitions des pins pour les hacheurs moteurs
#define MOTEUR1_IN1 _LATB14
#define MOTEUR1_IN2 _LATB15

//Configuration spécifique du moteur gauche
#define MOTEUR_GAUCHE_H_IO_OUTPUT MOTEUR1_IN1
#define MOTEUR_GAUCHE_L_IO_OUTPUT MOTEUR1_IN2
#define MOTEUR_GAUCHE_L_PWM_ENABLE IOCON1bits.PENL
#define MOTEUR_GAUCHE_H_PWM_ENABLE IOCON1bits.PENH
#define MOTEUR_GAUCHE_DUTY_CYCLE PDC1

Rampes de vitesse

void PWMUpdateSpeed()
{
// Cette fonction est appelée sur timer et permet de suivre des rampes d’accélération
if (robotState.vitesseDroiteCommandeCourante < robotState.vitesseDroiteConsigne)
robotState.vitesseDroiteCommandeCourante = Min(
robotState.vitesseDroiteCommandeCourante + acceleration,
robotState.vitesseDroiteConsigne);
if (robotState.vitesseDroiteCommandeCourante > robotState.vitesseDroiteConsigne)
robotState.vitesseDroiteCommandeCourante = Max(
robotState.vitesseDroiteCommandeCourante – acceleration,
robotState.vitesseDroiteConsigne);

if (robotState.vitesseDroiteCommandeCourante > 0)
{
MOTEUR_DROIT_L_PWM_ENABLE = 0; //pilotage de la pin en mode IO
MOTEUR_DROIT_L_IO_OUTPUT = 1; //Mise à 1 de la pin
MOTEUR_DROIT_PWM_ENABLE_H = 1; //Pilotage de la pin en mode PWM
}
else
{
MOTEUR_DROIT_H_PWM_ENABLE = 0; //pilotage de la pin en mode IO
MOTEUR_DROIT_H_IO_OUTPUT = 1; //Mise à 1 de la pin
MOTEUR_DROIT_L_PWM_ENABLE = 1; //Pilotage de la pin en mode PWM
}
MOTEUR_DROIT_DUTY_CYCLE = Abs(robotState.vitesseDroiteCommandeCourante)*PWMPER;

if (robotState.vitesseGaucheCommandeCourante < robotState.vitesseGaucheConsigne)
robotState.vitesseGaucheCommandeCourante = Min(
robotState.vitesseGaucheCommandeCourante + acceleration,
robotState.vitesseGaucheConsigne);
if (robotState.vitesseGaucheCommandeCourante > robotState.vitesseGaucheConsigne)
robotState.vitesseGaucheCommandeCourante = Max(
robotState.vitesseGaucheCommandeCourante – acceleration,
robotState.vitesseGaucheConsigne);

if (robotState.vitesseGaucheCommandeCourante > 0)
{
MOTEUR_GAUCHE_L_PWM_ENABLE = 0; //pilotage de la pin en mode IO
MOTEUR_GAUCHE_L_IO_OUTPUT = 1; //Mise à 1 de la pin
MOTEUR_GAUCHE_H_PWM_ENABLE = 1; //Pilotage de la pin en mode PWM
}
else
{
MOTEUR_GAUCHE_H_PWM_ENABLE = 0; //pilotage de la pin en mode IO
MOTEUR_GAUCHE_H_IO_OUTPUT = 1; //Mise à 1 de la pin
MOTEUR_GAUCHE_L_PWM_ENABLE = 1; //Pilotage de la pin en mode PWM
}
MOTEUR_GAUCHE_DUTY_CYCLE = Abs(robotState.vitesseGaucheCommandeCourante) * PWMPER;
}

Conversions analogiques numériques

#include <xc.h>
#include “adc.h”
#include “main.h”

unsigned char ADCResultIndex = 0;
static unsigned int ADCResult[4];
unsigned char ADCConversionFinishedFlag;

/****************************************************************************************************/
// Configuration ADC
/****************************************************************************************************/
void InitADC1(void)
{
//cf. ADC Reference Manual page 47

//Configuration en mode 12 bits mono canal ADC avec conversions successives sur 4 entrées
/************************************************************/
//AD1CON1
/************************************************************/
AD1CON1bits.ADON = 0; // ADC module OFF – pendant la config
AD1CON1bits.AD12B = 1; // 0 : 10bits – 1 : 12bits
AD1CON1bits.FORM = 0b00; // 00 = Integer (DOUT = 0000 dddd dddd dddd)
AD1CON1bits.ASAM = 0; // 0 = Sampling begins when SAMP bit is set
AD1CON1bits.SSRC = 0b111; // 111 = Internal counter ends sampling and starts conversion (auto-convert)

/************************************************************/
//AD1CON2
/************************************************************/
AD1CON2bits.VCFG = 0b000; // 000 : Voltage Reference = AVDD AVss
AD1CON2bits.CSCNA = 1; // 1 : Enable Channel Scanning
AD1CON2bits.CHPS = 0b00; // Converts CH0 only
AD1CON2bits.SMPI = 2; // 2+1 conversions successives avant interrupt
AD1CON2bits.ALTS = 0;
AD1CON2bits.BUFM = 0;

/************************************************************/
//AD1CON3
/************************************************************/
AD1CON3bits.ADRC = 0; // ADC Clock is derived from Systems Clock
AD1CON3bits.ADCS = 15; // ADC Conversion Clock TAD = TCY * (ADCS + 1)
AD1CON3bits.SAMC = 15; // Auto Sample Time

/************************************************************/
//AD1CON4
/************************************************************/
AD1CON4bits.ADDMAEN = 0; // DMA is not used

/************************************************************/
//Configuration des ports
/************************************************************/
//ADC utilisés : 16(G9)-11(C11)-6(C0)
ANSELCbits.ANSC0 = 1;
ANSELCbits.ANSC11 = 1;
ANSELGbits.ANSG9 = 1;

AD1CSSLbits.CSS6=1; // Enable AN6 for scan
AD1CSSLbits.CSS11=1; // Enable AN11 for scan
AD1CSSHbits.CSS16=1; // Enable AN16 for scan

/* Assign MUXA inputs */
AD1CHS0bits.CH0SA = 0;// CH0SA bits ignored for CH0 +ve input selection
AD1CHS0bits.CH0NA = 0;// Select VREF- for CH0 -ve inpu

IFS0bits.AD1IF = 0; // Clear the A/D interrupt flag bit
IEC0bits.AD1IE = 1; // Enable A/D interrupt
AD1CON1bits.ADON = 1; // Turn on the A/D converter
}

/* This is ADC interrupt routine */
void __attribute__((interrupt, no_auto_psv)) _AD1Interrupt(void)
{
IFS0bits.AD1IF = 0;
ADCResult[0] = ADC1BUF0;// Read the AN-scan input 1 conversion result
ADCResult[1] = ADC1BUF1;// Read the AN3 conversion result
ADCResult[2] = ADC1BUF2;// Read the AN5 conversion result
ADCConversionFinishedFlag = 1;
}

void ADC1StartConversionSequence()
{
AD1CON1bits.SAMP = 1 ; //Lance une acquisition ADC
}

unsigned int * ADCGetResult(void)
{
return ADCResult;
}

unsigned char ADCIsConversionFinished(void)
{
return ADCConversionFinishedFlag;
}

void ADCClearConversionFinishedFlag(void)
{
ADCConversionFinishedFlag = 0;
}

Réglage automatique des timers

void SetFreqTimer1(float freq)
{
T1CONbits.TCKPS = 0b00; //00 = 1:1 prescaler value
if(FCY /freq > 65535)
{
T1CONbits.TCKPS = 0b01; //01 = 1:8 prescaler value
if(FCY /freq / 8 > 65535)
{
T1CONbits.TCKPS = 0b10; //10 = 1:64 prescaler value
if(FCY /freq / 64 > 65535)
{
T1CONbits.TCKPS = 0b11; //11 = 1:256 prescaler value
PR1 = (int)(FCY / freq / 256);
}
else
PR1 = (int)(FCY / freq / 64);
}
else
PR1 = (int)(FCY / freq / 8);
}
else
PR1 = (int)(FCY / freq);
}

Définition des états de la machine à état de gestion du robot

#define STATE_ATTENTE 0
#define STATE_ATTENTE_EN_COURS 1
#define STATE_AVANCE 2
#define STATE_AVANCE_EN_COURS 3
#define STATE_TOURNE_GAUCHE 4
#define STATE_TOURNE_GAUCHE_EN_COURS 5
#define STATE_TOURNE_DROITE 6
#define STATE_TOURNE_DROITE_EN_COURS 7
#define STATE_TOURNE_SUR_PLACE_GAUCHE 8
#define STATE_TOURNE_SUR_PLACE_GAUCHE_EN_COURS 9
#define STATE_TOURNE_SUR_PLACE_DROITE 10
#define STATE_TOURNE_SUR_PLACE_DROITE_EN_COURS 11
#define STATE_ARRET 12
#define STATE_ARRET_EN_COURS 13
#define STATE_RECULE 14
#define STATE_RECULE_EN_COURS 15

#define PAS_D_OBSTACLE 0
#define OBSTACLE_A_GAUCHE 1
#define OBSTACLE_A_DROITE 2
#define OBSTACLE_EN_FACE 3

Machine à état de gestion du robot

unsigned char stateRobot;

void OperatingSystemLoop(void)
{
switch (stateRobot)
{
case STATE_ATTENTE:
timestamp = 0;
PWMSetSpeedConsigne(0, MOTEUR_DROIT);
PWMSetSpeedConsigne(0, MOTEUR_GAUCHE);
stateRobot = STATE_ATTENTE_EN_COURS;

case STATE_ATTENTE_EN_COURS:
if (timestamp > 1000)
stateRobot = STATE_AVANCE;
break;

case STATE_AVANCE:
PWMSetSpeedConsigne(30, MOTEUR_DROIT);
PWMSetSpeedConsigne(30, MOTEUR_GAUCHE);
stateRobot = STATE_AVANCE_EN_COURS;
break;
case STATE_AVANCE_EN_COURS:
SetNextRobotStateInAutomaticMode();
break;

case STATE_TOURNE_GAUCHE:
PWMSetSpeedConsigne(30, MOTEUR_DROIT);
PWMSetSpeedConsigne(0, MOTEUR_GAUCHE);
stateRobot = STATE_TOURNE_GAUCHE_EN_COURS;
break;
case STATE_TOURNE_GAUCHE_EN_COURS:
SetNextRobotStateInAutomaticMode();
break;

case STATE_TOURNE_DROITE:
PWMSetSpeedConsigne(0, MOTEUR_DROIT);
PWMSetSpeedConsigne(30, MOTEUR_GAUCHE);
stateRobot = STATE_TOURNE_DROITE_EN_COURS;
break;
case STATE_TOURNE_DROITE_EN_COURS:
SetNextRobotStateInAutomaticMode();
break;

case STATE_TOURNE_SUR_PLACE_GAUCHE:
PWMSetSpeedConsigne(15, MOTEUR_DROIT);
PWMSetSpeedConsigne(-15, MOTEUR_GAUCHE);
stateRobot = STATE_TOURNE_SUR_PLACE_GAUCHE_EN_COURS;
break;
case STATE_TOURNE_SUR_PLACE_GAUCHE_EN_COURS:
SetNextRobotStateInAutomaticMode();
break;

case STATE_TOURNE_SUR_PLACE_DROITE:
PWMSetSpeedConsigne(-15, MOTEUR_DROIT);
PWMSetSpeedConsigne(15, MOTEUR_GAUCHE);
stateRobot = STATE_TOURNE_SUR_PLACE_DROITE_EN_COURS;
break;
case STATE_TOURNE_SUR_PLACE_DROITE_EN_COURS:
SetNextRobotStateInAutomaticMode();
break;

default :
stateRobot = STATE_ATTENTE;
break;
}
}

unsigned char nextStateRobot=0;

void SetNextRobotStateInAutomaticMode()
{
unsigned char positionObstacle = PAS_D_OBSTACLE;

//Détermination de la position des obstacles en fonction des télémètres
if ( robotState.distanceTelemetreDroit < 30 &&
robotState.distanceTelemetreCentre > 20 &&
robotState.distanceTelemetreGauche > 30) //Obstacle à droite
positionObstacle = OBSTACLE_A_DROITE;
else if(robotState.distanceTelemetreDroit > 30 &&
robotState.distanceTelemetreCentre > 20 &&
robotState.distanceTelemetreGauche < 30) //Obstacle à gauche
positionObstacle = OBSTACLE_A_GAUCHE;
else if(robotState.distanceTelemetreCentre < 20) //Obstacle en face
positionObstacle = OBSTACLE_EN_FACE;
else if(robotState.distanceTelemetreDroit > 30 &&
robotState.distanceTelemetreCentre > 20 &&
robotState.distanceTelemetreGauche > 30) //pas d’obstacle
positionObstacle = PAS_D_OBSTACLE;

//Détermination de l’état à venir du robot
if (positionObstacle == PAS_D_OBSTACLE)
nextStateRobot = STATE_AVANCE;
else if (positionObstacle == OBSTACLE_A_DROITE)
nextStateRobot = STATE_TOURNE_GAUCHE;
else if (positionObstacle == OBSTACLE_A_GAUCHE)
nextStateRobot = STATE_TOURNE_DROITE;
else if (positionObstacle == OBSTACLE_EN_FACE)
nextStateRobot = STATE_TOURNE_SUR_PLACE_GAUCHE;

//Si l’on n’est pas dans la transition de l’étape en cours
if (nextStateRobot != stateRobot – 1)
stateRobot = nextStateRobot;
}