|
#include <dspic.h>
|
|
|
|
#include "setup.h"
|
|
#include "a6.h"
|
|
#include "asserv.h"
|
|
// configuration des registres
|
|
/* FBS : on fait une opperation logique afin de d?finir la configuration de ce registre
|
|
BWRPU : boot segment program flash write protection
|
|
BSS : cod? sur trois bits
|
|
NORBS : cod? sur 2 bits
|
|
|
|
FSS: on fait une opperation logique afin de d?finir la configuration de ce registre
|
|
FGS : on fait une opperation logique afin de d?finir la configuration de ce registre
|
|
FOSCSEL ;
|
|
FOSC : configuration de l'horloge
|
|
FWDT ; bit de validation du chien de garde
|
|
|
|
FPOR : registre de control du moteur
|
|
FICD ;
|
|
OSCIOFNC : OSC2DIO, pour choisir OSC2 en "clock output" ou en une broche entree/ sortie num?rique, selon on met 1 ou 0
|
|
*/
|
|
__CONFIG(FBS, BWRPU & BSSNB & NORBS);
|
|
__CONFIG(FSS, SWRPU & SSSNS & NORSS);
|
|
__CONFIG(FGS, GCPU & GSSU);
|
|
__CONFIG(FOSCSEL, FRCPLL & IESOEN); // configuration du registre FOSCSEL : IESO bit 7 du registre FOSCSEL, FRCPLL est un oxillateur PLL
|
|
__CONFIG(FOSC, FCKSMDIS & OSC2DIO & IOL1WAY);// FCKMDIS : activer ou desactiver la communication de l'horloge, IOL1WAY configuration de la bonche peripherique de l'horloge
|
|
__CONFIG(FWDT, WDTDIS & WINEN & WDTPRE128 & WDTPS32768);// : WDTPRE128 : prescaler de 128; WDTDIS: watchdog timer in window mode or not, selon la valeur du bit
|
|
__CONFIG(FPOR, PWMPORT & PWRTDIS);// PWMPORT : Les broches du module pwm sont contr?l?es par le registre de port lors de la r?initialisation de l'appareil, PWRTDIS : power-on reset timer value select bits
|
|
__CONFIG(FICD, DEBUGEN & JTAGDIS & ICS1); //JTAGDIS : on desavtive JTAG; ICS1 : selection de la voix de communication
|
|
|
|
extern unsigned char arret; // declaration de la variable arret, il est declar? en variable extern
|
|
extern unsigned char prog; // declaration de la variable prog, il est declar? en variable extern
|
|
|
|
unsigned short i=0;
|
|
//short tab1[400], tab2[400], tab3[400], tab4[400];
|
|
//unsigned short id=0;
|
|
|
|
void main()
|
|
{
|
|
A6_packet a6;
|
|
unsigned char ch;
|
|
unsigned char data[4];// tableau de type char de taille 4
|
|
unsigned short val;
|
|
short p;
|
|
short vit;
|
|
// appel de fionctions : horloge, io, A6 et asservissement
|
|
setup_clock();// unitialisation de l'horloge du microcontroleur
|
|
setup_io(); //dans cette fonction on initialise les port en digital, UART2 pins : on configure le UART; scs=1x ,HFIOFS=1( oxillateur interne ), f=2mhz
|
|
setup_A6(); //?
|
|
setup_channel(); // configuration des voie de communication avec le moteur
|
|
setup_asserv(); // initilise le timer, l'horloge ( a 80hz, le watchdog est desactive, configuration du pll), initialise le pwm
|
|
|
|
for(p=0;p<1000;p++);// on attend 1000 it?ration de p : temporisation
|
|
|
|
#ifdef ASSERV_POS // on test si la constante ASSERV_POS est d?finie
|
|
while(1)
|
|
/*
|
|
dans cette boucle on met a jour la destination du moteur, son angle de rotation, la vitesse du moteur
|
|
*/
|
|
|
|
{
|
|
// si le moteur est a l'arret et que la position est de 1 on lit la valeur de la position et de l'angle
|
|
if(arret==1 && prog==1)
|
|
{
|
|
channel_read(INIT_POS, data);
|
|
setInitPos(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));
|
|
|
|
channel_read(INIT_THETA, data);
|
|
setInitTheta(*(unsigned short*)(&data[0]));
|
|
}
|
|
//
|
|
if(isChannelNew(DEST)) //si la nouvelle voie vaut DEST
|
|
{
|
|
channel_read(DEST, data); // On R?cupere les valeurs de DEST et on les stock dans data
|
|
setDest(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));
|
|
}
|
|
if(isChannelNew(THETA_DEST)) // si la nouvelle voie vaut THETA_DEST
|
|
{
|
|
channel_read(THETA_DEST, data); // On R?cupere les valeurs de THETA_DEST et on les stock dans data
|
|
setThetaDest(*(unsigned short*)(&data[0]));
|
|
}
|
|
if(isChannelNew(MODE_ASSERV)) // si la nouvelle valeur vaut MODE_ASSERV
|
|
{
|
|
channel_read(MODE_ASSERV, data); // On R?cupere les valeurs de ASSERV et on les stock dans data
|
|
setMode(data[0]); // On stock la valeur dans data
|
|
}
|
|
if(isChannelNew(VIT)) // si la nouvel levaleur vaut VTT
|
|
{
|
|
channel_read(VIT, data); // On R?cupere les valeurs de VTT et on les stock dans data
|
|
vit=*(unsigned short*)(&data[0]); // On stock la valeur dans data
|
|
if(vit<0) // si la nouvelle valeur VTT est inferieur a 0
|
|
{
|
|
vit=-vit; // on inverse VTT
|
|
setWay(Back);
|
|
}
|
|
else
|
|
{
|
|
setWay(Forward);
|
|
}
|
|
setVitesse(vit);
|
|
}
|
|
|
|
asserv(); // on ex?cute le code de l'asservissement
|
|
|
|
data[0] = getState(); // on met la valeur de getState dans data[0]
|
|
data[1] = 0; // on met a 0 la valeur de data[1]
|
|
*(unsigned short*)(&data[2]) = 0;
|
|
channel_set(ASSERV_STATE, data);
|
|
|
|
*(unsigned short*)(&data[0]) = getX();// on recupere la valeur retourner par getx
|
|
*(unsigned short*)(&data[2]) = getY(); // on recupere la valeur retourner par getx
|
|
channel_set(POS, data);
|
|
|
|
*(unsigned short*)(&data[0]) = getTheta();// on recupere la valeur retourner par getTheta
|
|
*(unsigned short*)(&data[2]) = 0;
|
|
channel_set(THETA, data);
|
|
|
|
*(unsigned short*)(&data[0]) = getDistance();
|
|
*(unsigned short*)(&data[2]) = 0;
|
|
channel_set(DIST, data);
|
|
}
|
|
#elif ASSERV_DEG //Si la constante ASSER_POS n'est pas definie alors elle vaut ASSER_DEG
|
|
while(1)
|
|
{
|
|
if(arret==1 && prog==1) //si le moteur est a l'arret & de meme que prog ?gale ? 1 alors on fait rien
|
|
{
|
|
|
|
}
|
|
if(isChannelNew(TOP_DEST)) //si la nouvelle voie vaut TOP_Dest alors
|
|
{
|
|
channel_read(TOP_DEST, data); // On R?cupere les valeurs de TOP_DEST et on les stock dans data
|
|
setTopDest(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));//On met a jour la nouvelle destination
|
|
}
|
|
|
|
if(isChannelNew(POWER)) // si on es bien alimenter
|
|
{
|
|
channel_read(POWER, data); //On recuperer la valeur indiquant que le circuit est bien aliment?
|
|
setPower(*(short*)(&data[0])); //On stock la valeur dans data
|
|
}
|
|
|
|
asserv_deg(); //On execute l'asservisssment en position
|
|
|
|
*(unsigned short*)(&data[0]) = getTop(); // stock dans data la valeur du compteur
|
|
*(unsigned short*)(&data[2]) = 0; //initialise la valeur de data
|
|
channel_set(TOP, data);// on configure la valeur de TOP avec la valeur qui a dans data
|
|
}
|
|
#else
|
|
#error "You have to define any asserv mode in configuration project"
|
|
#endif
|
|
}
|