c_tro_1.c

Mamadou Alpha Diallo, 10/25/2017 01:49 PM

Download (5.56 KB)

 
1
#include <dspic.h>
2

    
3
#include "setup.h"
4
#include "a6.h"
5
#include "asserv.h"
6
// configuration des registres
7
/* FBS : on fait une opperation logique afin de d?finir la configuration de ce registre
8
    BWRPU : boot segment program flash write protection
9
    BSS :   cod? sur trois bits
10
    NORBS : cod? sur  2 bits
11

12
    FSS: on fait une opperation logique afin de d?finir la configuration de ce registre
13
    FGS : on fait une opperation logique afin de d?finir la configuration de ce registre
14
    FOSCSEL ;
15
    FOSC : configuration de l'horloge
16
    FWDT ; bit de validation du chien de garde
17

18
    FPOR : registre de control du moteur
19
    FICD ;
20
    OSCIOFNC : OSC2DIO, pour choisir OSC2 en "clock output" ou en une broche entree/ sortie num?rique, selon on met 1 ou 0
21
*/
22
__CONFIG(FBS, BWRPU & BSSNB & NORBS);
23
__CONFIG(FSS, SWRPU & SSSNS & NORSS);
24
__CONFIG(FGS, GCPU  & GSSU);
25
__CONFIG(FOSCSEL, FRCPLL & IESOEN); // configuration du registre FOSCSEL : IESO bit 7 du registre FOSCSEL, FRCPLL est un oxillateur  PLL
26
__CONFIG(FOSC, FCKSMDIS & OSC2DIO & IOL1WAY);// FCKMDIS : activer ou desactiver la communication de l'horloge, IOL1WAY configuration de la bonche peripherique de l'horloge
27
__CONFIG(FWDT, WDTDIS & WINEN & WDTPRE128 & WDTPS32768);// : WDTPRE128 : prescaler de 128; WDTDIS: watchdog timer in window mode or not, selon la valeur du bit
28
__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
29
__CONFIG(FICD, DEBUGEN & JTAGDIS & ICS1); //JTAGDIS : on desavtive JTAG; ICS1 : selection de la voix de communication
30

    
31
extern unsigned char arret; // declaration de la variable arret, il est declar? en variable extern
32
extern unsigned char prog;  // declaration de la variable prog, il est declar? en variable extern
33

    
34
unsigned short i=0;
35
//short tab1[400], tab2[400], tab3[400], tab4[400];
36
//unsigned short id=0;
37

    
38
void main()
39
{
40
        A6_packet a6;
41
        unsigned char ch;
42
        unsigned char data[4];// tableau de type char de taille 4
43
        unsigned short val;
44
        short p;
45
        short vit;
46
    // appel de fionctions : horloge, io, A6 et asservissement
47
        setup_clock();
48
        setup_io();
49
        setup_A6();
50
        setup_channel();
51
        setup_asserv();
52

    
53
        for(p=0;p<1000;p++);// on attend 1000 it?ration de p : temporisation
54

    
55
#ifdef ASSERV_POS // on test si la constante ASSERV_POS est d?finie
56
        while(1)
57
        {
58
            // 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
59
                if(arret==1 && prog==1)
60
                {
61
                        channel_read(INIT_POS, data);
62
                        setInitPos(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));
63

    
64
                        channel_read(INIT_THETA, data);
65
                        setInitTheta(*(unsigned short*)(&data[0]));
66
                }
67
         //
68
                if(isChannelNew(DEST)) //si la nouvelle voie vaut DEST
69
                {
70
                        channel_read(DEST, data); //  On R?cupere les valeurs de DEST et on les stock dans data
71
                        setDest(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));
72
                }
73
                if(isChannelNew(THETA_DEST)) // si la nouvelle voie vaut THETA_DEST
74
                {
75
                        channel_read(THETA_DEST, data); // On R?cupere les valeurs de THETA_DEST et on les stock dans data
76
                        setThetaDest(*(unsigned short*)(&data[0]));
77
                }
78
                if(isChannelNew(MODE_ASSERV)) // si la nouvelle valeur vaut MODE_ASSERV
79
                {
80
                        channel_read(MODE_ASSERV, data); //  On R?cupere les valeurs de ASSERV et on les stock dans data
81
                        setMode(data[0]); // On stock la valeur dans data
82
                }
83
                if(isChannelNew(VIT)) // si la nouvel levaleur vaut VTT
84
                {
85
                        channel_read(VIT, data); //  On R?cupere les valeurs de VTT et on les stock dans data
86
                        vit=*(unsigned short*)(&data[0]); // On stock la valeur dans data
87
                        if(vit<0) // si la nouvelle valeur VTT est inferieur a 0
88
                        {
89
                                vit=-vit; // on inverse VTT
90
                                setWay(Back);
91
                        }
92
                        else
93
                        {
94
                                setWay(Forward);
95
                        }
96
                        setVitesse(vit);
97
                }
98

    
99
                asserv(); // on ex?cute le code de l'asservissement
100

    
101
                data[0] = getState(); // on met la valeur de getState dans data[0]
102
                data[1] = 0;  // on met a 0 la valeur de data[1]
103
                *(unsigned short*)(&data[2]) = 0;
104
                channel_set(ASSERV_STATE, data);
105

    
106
                *(unsigned short*)(&data[0]) = getX();
107
                *(unsigned short*)(&data[2]) = getY();
108
                channel_set(POS, data);
109

    
110
                *(unsigned short*)(&data[0]) = getTheta();
111
                *(unsigned short*)(&data[2]) = 0;
112
                channel_set(THETA, data);
113

    
114
                *(unsigned short*)(&data[0]) = getDistance();
115
                *(unsigned short*)(&data[2]) = 0;
116
                channel_set(DIST, data);
117
        }
118
#elif ASSERV_DEG //Si la constante ASSER_POS n'est pas definie alors elle vaut ASSER_DEG
119
        while(1)
120
        {
121
                if(arret==1 && prog==1)     //si arret & prog sont ?gale ? 1 alors on fait rien
122
                {
123

    
124
                }
125
                if(isChannelNew(TOP_DEST))  //si la nouvelle voie vaut TOP_Dest alors
126
                {
127
                        channel_read(TOP_DEST, data);   // On R?cupere les valeurs de TOP_DEST et on les stock dans data
128
                        setTopDest(*(unsigned short*)(&data[0]), *(unsigned short*)(&data[2]));//On met a jour la nouvelle destination
129
                }
130

    
131
                if(isChannelNew(POWER)) // si on es bien alimenter
132
                {
133
                        channel_read(POWER, data);  //On recuperer la valeur indiquant que le circuit est bien aliment?
134
                        setPower(*(short*)(&data[0])); //On stock la valeur dans data
135
                }
136

    
137
        asserv_deg();   //On execute l'asservisssment en position
138

    
139
                *(unsigned short*)(&data[0]) = getTop();    // stock dans data la valeur du compteur
140
                *(unsigned short*)(&data[2]) = 0;           //initialise la valeur de data
141
                channel_set(TOP, data);// on configure la valeur de TOP avec la valeur qui a dans data
142
        }
143
#else
144
        #error "You have to define any asserv mode in configuration project"
145
#endif
146
}