asserv.c

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

Download (13.1 KB)

 
1
#include "asserv.h"
2

    
3
#include <dspic.h>
4
#include <math.h>
5
#define M_PI                3.14159265358979323846
6

    
7
#include "setup.h"
8

    
9
//declaration des variables
10
float xf,yf;
11
float xr,yr;
12

    
13
float vitesse;
14
float last_t=0, td=0;
15

    
16
short ancv1=0,ancv2=0;
17
short ancc1=0,ancc2=0;
18
short c1,c2;
19
short v1,v2;
20

    
21
short top_consigne=0, top_vitesse=0;
22

    
23
float errp1,errd1,erri1,errt1;
24
float errp2,errd2,erri2,errt2;
25
short err1,err2;
26

    
27
float consV1,consV2;
28
float consds,consdt;
29
float distance, angle;
30

    
31
float dt,ds,tand;
32

    
33
unsigned char time_flag=0;
34

    
35
Mode mode=Stop;
36
Way way=Forward;
37
State state=Stopped;
38

    
39
unsigned short id=0, cnt=10;
40
// declaration tableau
41
short tab0[600];
42
far short tab1[600], tab2[600]; // tableau de type far
43
far short ydata tab3[600], tab4[600]; // tableau de type far
44

    
45
extern unsigned char arret;
46

    
47
#define ANGLE_MAX        M_PI/10
48

    
49
#ifdef BIG_BOT // si la constante BIT_BOT est d?finie on ex?cute le code si dessous
50
        #define ENTRAX 216.7 //TODO 218.5
51
        //#define TICK1  0.0219196067692
52
        //#define HTICK  0.0109598033846
53
        #define TICK1  0.0221513716232
54
        #define HTICK  0.0110756858116
55
        #define DELTAVITESSE 0.16
56
        #define ERR_MINI 100
57
        #define PWM_MINI 300
58
        float vitesseMax=32;
59
        float x =280,y =-250,t=0;
60
        short kd1=20,ki1=1,kp1=55;
61
        short kd2=20,ki2=1,kp2=55;
62
#elif SMALL_BOT // sinon on exc?cute les constantes si dessous
63
        #define ENTRAX 103.0
64
        #define TICK1  0.0219196067692
65
        #define HTICK  0.0109598033846
66
        //#define TICK1  0.0221513716232
67
        //#define HTICK  0.0110756858116
68
        #define DELTAVITESSE 0.16
69
        #define ERR_MINI 100
70
        #define PWM_MINI 200
71
        float vitesseMax=32;
72
        float x =280,y =-250,t=0;
73
        short kd1=0,ki1=1,kp1=40;
74
        short kd2=0,ki2=1,kp2=40;
75
#elif ASSERV_DEG //  ASSERV_DEG est d?fini on exc?cute de code si dessous
76
// declaration des constante
77
        #define ENTRAX 219.5
78
        #define TICK1  0.0223993542529
79
        #define HTICK  0.0111996771264
80
        #define DELTAVITESSE 0.16
81
        #define ERR_MINI 100
82
        #define PWM_MINI 300
83
        //declaration de variable
84
        float vitesseMax=32;
85
        float x =280,y =-250,t=0;
86
        short kd1=0,ki1=1,kp1=70;
87
        short kd2=0,ki2=1,kp2=70;
88
#else // sinon erreur
89
        #error "no bot define"
90
#endif
91

    
92
/* la fonction setup_asserv permet d'ex?cuter les fonction setup_QEI1(); setup_QEI2();setup_PWM1();setup_timer1()  et initialise les coordonn?e x et y
93
    la fonction est de type void il retourn rien
94
 */
95
void setup_asserv()
96
{
97
        setup_QEI1();
98
        setup_QEI2();
99
        setup_PWM1();
100
        setup_timer1();
101
        xf=x;
102
        yf=y;
103
}
104

    
105
void setInitPos(unsigned short mx, unsigned short my)
106
/* fonction met a jour la position
107
   la fonction est de type void il retourn rien
108
*/
109
{
110
        x=mx/10.0;
111
        y=my/10.0;
112
        y=-y;
113
        xf=x;
114
        yf=y;
115
}
116
/*
117
fonction met a jour l'angle
118
la fonction est de type void il retourn rien
119
*/
120
void setInitTheta(unsigned short mt)
121
{
122
        t=mt/1800.0*M_PI;
123
        last_t=t;
124
}
125

    
126
void setDest(unsigned short mx, unsigned short my)
127
/*
128
fonction met a jour la destination
129
la fonction est de type void il retourn rien
130
*/
131
{
132
        xf=mx/10.0;
133
        yf=my/10.0;
134
        yf=-yf;
135
}
136

    
137
void setThetaDest(unsigned short mt)
138
/*
139
fonction met a jour angle de destination
140
la fonction est de type void il retourn rien
141
*/
142
{
143
        td=mt/1800.0*M_PI;
144
}
145

    
146
void setWay(Way mway)
147
/*fonction met a jour mway
148
la fonction est de type void il retourn rien
149
*/
150
{
151
        way=mway;
152
}
153
/*
154
fonction met a jour le mode de commande
155
la fonction est de type void il retourn rien
156
*/
157
void setMode(Mode mmode)
158
{
159
        mode=mmode;
160
}
161
/*
162
fonction met a jour la vitesse
163
la fonction est de type void il retourn rien
164
*/
165
void setVitesse(unsigned char mvitesse)
166
{
167
        vitesse = mvitesse;
168
}
169

    
170
unsigned short getX()
171
/*
172
la fonction retourne la position x
173
*/
174
{
175
        return (unsigned short)(x*10.0);
176
}
177

    
178
unsigned short getY()
179
{
180
        return (unsigned short)(y*-10.0);
181
}
182

    
183
State getState()
184
/*
185
la fonction retourne les information sur l'etat (state)
186
*/
187
{
188
        return state;
189
}
190

    
191
short getTheta()
192
/*
193
la fonction retourne l'angle theta
194
*/
195
{
196
        return (short)(t*1800.0/M_PI);
197
}
198
/*
199
la fonction retourne la distance
200
*/
201
unsigned short getDistance()
202
{
203
        return (unsigned short)(distance*10.0);
204
}
205

    
206
void setTopDest(unsigned short top, unsigned short top_power)
207
{
208
        top_consigne=(short)top;
209
        top_vitesse=(short)top_power;
210
}
211
/*
212
si power est superieur a zero on fait avancer le moteur ( soit gauche soit droite )
213
sinon on rend positive power puis on fait tourner le moteur dans le sens oppos?
214
*/
215
void setPower(short power)
216
{
217
        if(power>=0)
218
        {
219
                m1=power;
220
                BM1 = 0;
221
                AM1 = 1;
222
        }
223
        else
224
        {
225
                m1=-power;
226
                BM1 = 1;
227
                AM1 = 0;
228
        }
229
}
230
/*
231
la fonction retourne c2
232
*/
233
unsigned short getTop()
234
{
235
        return c2;
236
}
237
/*
238
  fonction d'asservissement du moteur
239
*/
240
void asserv(void)
241
{
242
        while(time_flag==0); // tant que time_flag=0 on fait rien
243

    
244
// rampe vitesse
245
        //if(vitesse<vitesseMax) vitesse+=DELTAVITESSE;
246

    
247
        distance = sqrt((x-xf)*(x-xf)+(y-yf)*(y-yf)); // si time_flag est different de 0 on met a jour la distance
248

    
249
// FSM
250
        switch(mode) // selon  l'etat de mode
251
        {
252
        case Stop:                // si mode=stop on met consds et consdt a 0                                                                                        // =================== Stop
253
                consds=0.0;
254
                consdt=0.0;
255
                break;
256
        case Fixe:                                                                                                        // =================== Fixe
257
                // consigne distance
258
                consds = distance / 4;//// si Fixe on met a consds
259
                if(consds > vitesse) consds = vitesse; //si consds superieur a la vitesse consds prend la valeur de la vitesse
260

    
261
                // consigne angle
262
                // on met a jour la valeur de l'angle
263
                if(y-yf<=0)
264
                {
265
                        if(x-xf<=0) consdt = asin((yf-y)/distance);
266
                        else consdt = -M_PI - asin((yf-y)/distance);
267
                }
268
                else
269
                {
270
                        if(x-xf<=0) consdt = asin((yf-y)/distance);
271
                        else consdt = M_PI - asin((yf-y)/distance);
272
                }
273
                consdt = -consdt-t;
274
                if(consdt>=M_PI/2 || consdt<=-M_PI/2)
275
                {
276
                        //consdt=M_PI-consdt;
277
                        consds=-consds;
278
                        if(consdt<=3*M_PI/8 & consdt>=-3*M_PI/8)
279
                        {
280
                                consds=0;
281
                        }
282
                }
283
                else
284
                {
285
                        if(consdt>=M_PI/8 || consdt<=-M_PI/8)
286
                        {
287
                                consds=0;
288
                        }
289
                }
290
                consdt=last_t-t;
291
                if(consdt>M_PI) consdt-=2*M_PI;
292
                if(consdt<-M_PI) consdt+=2*M_PI;
293

    
294
                consdt/=2.0;
295
                if(distance>40.0)
296
                {
297
                        mode = Linear;
298
                }
299

    
300
                break;
301
        case Linear:                                                                                                // =================== Linear
302
                // consigne distance
303
                // si mode linear on met a jour consds et si elle est superieur a la vitesse elle prend la valeur de la vitesse
304
                consds = distance / 4;
305
                if(consds > vitesse) consds = vitesse;
306

    
307
                // consigne angle
308
                // on met a jour la valeur de l'angle
309
                if(y-yf<=0)
310
                {
311
                        if(x-xf<=0) angle = asin((yf-y)/distance);
312
                        else angle = -M_PI - asin((yf-y)/distance);
313
                }
314
                else
315
                {
316
                        if(x-xf<=0) angle = asin((yf-y)/distance);
317
                        else angle = M_PI - asin((yf-y)/distance);
318
                }
319

    
320
                consdt = -angle-t;
321
                if(consdt>M_PI) consdt-=2*M_PI;// si consdt superieur a M_PI on decremente consdt
322
                if(consdt<-M_PI) consdt+=2*M_PI; // si consdt inferieur  a M_PI on incremente consdt
323

    
324
                if(way==Back) // si way egale a back
325
                {
326
                        consds = -consds;// consds prend son oppos?
327
                        consdt=consdt+M_PI;// consdt est mis a jour
328
                        if(consdt>M_PI) consdt-=2*M_PI; // si consdt est superieur M_Pi consdt est decremente de 2pi
329
                        if(consdt<-M_PI) consdt+=2*M_PI; // si consdt est inferieur de -2pi on decremente de 2*PI
330
                }
331

    
332
                if(distance<20.0) // si distance <20.0 on met le moteur en mode fixe et last_t prend la valeur de t
333
                {
334
                        mode = Fixe;
335
                        last_t = t;
336
                }
337
                else if(consdt>ANGLE_MAX || consdt<-ANGLE_MAX)
338
                 /* sinon si consdt > a l'angle max ou consdt < a angle_max
339
                    le moteur est mis en mode rotation et on met a jour xr et yr
340
                 */
341

    
342

    
343
                {
344
                        mode = Rotate;
345
                        xr=x;
346
                        yr=y;
347
                        if(way==Back) td = angle+M_PI; else td = angle;// si way
348
                }
349
                consdt/=2.0;
350

    
351
                break;
352
        case Rotate:                                                                                                // =================== Rotate
353
                // consigne distance
354
                // si on est en mode rotation on met consds a 0
355
                consds = 0;
356
                if(consds > vitesse) consds = vitesse;// si consds superieur a la vitesse elle prend la valeur de la vitesse
357

    
358
                // consigne angle
359
                // mise a jour de consdt
360
                consdt = -td-t;
361

    
362
                if(consdt>M_PI) consdt-=2*M_PI; // si superieur a M_PI on decremente consdt de 2*M_PI
363
                if(consdt<-M_PI) consdt+=2*M_PI; // si superieur a M_PI on incremente consdt de 2*M_PI
364

    
365
                if(consdt<ANGLE_MAX & consdt>-ANGLE_MAX) mode = Linear; //si consdt est inferieur a Angle_Max et consdt superieur a - Angle_Max le moteur est mis en mode lineaire
366

    
367
                break;
368
        }
369

    
370
        /*if(distance<30)
371
        {
372

373
        }
374
        else
375
        {
376
                if(consdt>ANGLE_MAX || consdt<-ANGLE_MAX)
377
                {
378
                        consds=0;
379
                        kt=2;
380
                }
381
                else kt=1;
382
                last_t=consdt+t;
383
        }*/
384

    
385
        if(consdt>M_PI) consdt-=2*M_PI; // si superieur a M_PI on decremente consdt de 2*M_PI
386
        if(consdt<-M_PI) consdt+=2*M_PI; // si inferieur a M_PI on decremente consdt de 2*M_PI
387

    
388

    
389
// calcul consigne moteur
390
        tand = (ENTRAX*tan(consdt))/(TICK1*20);
391
        if(consdt>=M_PI/2) tand=vitesse;
392
        if(consdt<=-M_PI/2) tand=-vitesse;
393
        if(tand>vitesse/2) tand=vitesse/2;
394
        if(tand<-vitesse/2) tand=-vitesse/2;
395
        consV1 = consds - tand;
396
        consV2 = consds + tand;
397

    
398
// pid moteur 1
399
        errp1 = kp1 * (consV1 - v1);
400
        errd1 = kd1 * ((consV1 - v1) - (consV1 - ancv1));
401
        err1 = (short)errp1 + (short)errd1;
402
        if(err1>4095) err1=4095;
403
        if(err1<-4095) err1=-4095;
404
        if(err1>-ERR_MINI && err1<ERR_MINI) err1=0;
405

    
406
// pid moteur 2
407
        errp2 = kp2 * (consV2 - v2);
408
        errd2 = kd2 * ((consV2 - v2) - (consV2 - ancv2));
409
        err2 = (short)errp2 + (short)errd2;
410
        if(err2>4095) err2=4095;
411
        if(err2<-4095) err2=-4095;
412
        if(err2>-ERR_MINI && err2<ERR_MINI) err2=0;
413

    
414
// anti marche arriere
415

    
416
        if(consds>5.0) //si consds  n'est pas dans l'interval [-5,5] on met err1 et err2 a 0
417
        {
418
                if(err1<0) err1=0;
419
                if(err2<0) err2=0;
420
        }
421
        if(consds<-5)
422
        {
423
                if(err1>0) err1=0;
424
                if(err2>0) err2=0;
425
        }
426

    
427
        if(arret!=0)// si le moteur n'est pas en arret on met a 0 le m1 et m2 et a 1 AM et BM des deux moteurs
428
        {
429
                m1 = 0;
430
                AM1 = 1;
431
                BM1 = 1;
432

    
433
                m2 = 0;
434
                AM2 = 1;
435
                BM2 = 1;
436
        }
437
        else // sinon si elle est en arret on fait avancer les moteur selon la valeur de err1 et err2 puis on met a jour m1 et m2
438
        {
439
                if(err1>0)
440
                {
441
                        BM1 = 0;
442
                        AM1 = 1;
443
                        m1 = (err1)+PWM_MINI;
444
                }
445
                else if(err1<0)
446
                {
447
                        AM1 = 0;
448
                        BM1 = 1;
449
                        m1 = (err1*-1)+PWM_MINI;
450
                }
451
                else
452
                {
453
                        AM1 = 1;
454
                        BM1 = 1;
455
                        m1 = 2048;
456
                }
457

    
458
                if(err2>0)
459
                {
460
                        BM2 = 0;
461
                        AM2 = 1;
462
                        m2 = (err2)+PWM_MINI;
463
                }
464
                else if(err2<0)
465
                {
466
                        AM2 = 0;
467
                        BM2 = 1;
468
                        m2 = (err2*-1)+PWM_MINI;
469
                }
470
                else
471
                {
472
                        AM2 = 1;
473
                        BM2 = 1;
474
                        m2 = 2048;
475
                }
476
        }
477

    
478
        time_flag=0;
479
    // on incremente le compteur et si elle est superieur a 6 et si id<600 on remplie les tableau de tab0 a tab4 et on incremente id a chaque fois que les tableau sont remplie
480
        if(cnt++>6)
481
        {
482
                if(id<600)
483
                {
484
                        tab0[id]=(short)x;
485
                        tab1[id]=(short)err1;
486
                        tab2[id]=(short)err2;
487
                        tab3[id]=(short)v1;
488
                        tab4[id]=(short)v2;
489
                        id++;
490
                }
491
                //si id>600 on met le compteur a 0
492
                cnt=0;
493
        }
494
}
495
/*
496
 fonction d'asservissement de l'angle du moteur
497
*/
498
void asserv_deg(void)
499
{
500

    
501

    
502
        while(time_flag==0); // tant que le flag=0 on fait rien
503

    
504
        consV2 = top_consigne - c2;
505
        if(consV2>top_vitesse) consV2 = top_vitesse; // si consV2 superieur a top_vitesse consV2 prend la valeur de top_vitesse
506
        if(consV2<-top_vitesse) consV2 = -top_vitesse;// sinon elle prend l'oppos? de top_vitesse
507

    
508
// pid moteur 1
509
// mis a jour des valeur du moteur 1
510
        errp1 = kp1 * (consV1 - v1);
511
        errd1 = kd1 * ((consV1 - v1) - (consV1 - ancv1));
512
        err1 = (short)errp1 + (short)errd1;
513
        if(err1>4095) err1=4095;
514
        if(err1<-4095) err1=-4095;
515
        if(err1>-PWM_MINI && err1<PWM_MINI) err1=0;
516

    
517
// pid moteur 2
518
// mis a jour des valeurs du moteur 2
519
        errp2 = kp2 * (consV2 - v2);
520
        errd2 = kd2 * ((consV2 - v2) - (consV2 - ancv2));
521
        err2 = (short)errp2 + (short)errd2;
522
        if(err2>4095) err2=4095;
523
        if(err2<-4095) err2=-4095;
524
        if(err2>-PWM_MINI && err2<PWM_MINI) err2=0;
525

    
526
        if(arret!=0) // si arret est different de 0 on met le moteur m1 et m2 a zero
527
        {
528
                m1 = 0;
529
                m2 = 0;
530
        }
531
        else
532
        // sinon si err2 superieur a 0 on fait avancer le moteur 2
533
        {
534
                if(err2>=0.0)
535
                {
536
                        BM2 = 1;
537
                        AM2 = 0;
538
                        m2 = (short)(err2); // m2 prend la valeur de err2 qui est converti en short
539
                }
540
                else
541
                // sinon on fait avancer le moteur dans le sens oppos? puis m2 prend l'oppos? de err2
542
                {
543
                        AM2 = 1;
544
                        BM2 = 0;
545
                        m2 = (short)(err2*-1);
546
                }
547
        }
548

    
549
        time_flag=0;// on met time_flag a 0
550
}
551

    
552
#ifdef ASSERV_POS // si ASSERV_POS est d?finie on ex?cute l'interruption du timer1
553
        void interrupt timer1_int(void) @ T1_VCTR
554
        {
555
                // calcul position
556
                //T1CK1 : external clock input
557
                #ifdef BIG_BOT
558
                        dt = atan((v1-v2)*TICK1/ENTRAX);
559
                        ds = (v1 + v2)*HTICK;
560
                #elif SMALL_BOT
561
                        dt = atan((v2-v1)*TICK1/ENTRAX);
562
                        ds = (v1 + v2)*HTICK;
563
                #else
564
                        #error "no bot define"
565
                #endif
566
                x += ds * cos(t+dt*.5);
567
                y -= ds * sin(t+dt*.5);
568
                t -= dt;
569
                if(t>M_PI) t-=2*M_PI;
570
                if(t<-M_PI) t+=2*M_PI;
571

    
572

    
573
                c1=POS1CNT;
574
                c2=POS2CNT;
575

    
576
                v1=c1-ancc1;
577
                v2=c2-ancc2;
578

    
579
                ancc1=c1;
580
                ancc2=c2;
581
                ancv1=v1;
582
                ancv2=v2;
583

    
584
                time_flag=1;
585
                T1IF = 0; // interrupt flag status
586
        }
587
#elif ASSERV_DEG
588
        void interrupt timer1_int(void) @ T1_VCTR
589
        {
590
                c1=POS1CNT;
591
                c2=POS2CNT;
592
                v1=c1-ancc1;
593
                v2=c2-ancc2;
594

    
595
                ancc1=c1;
596
                ancc2=c2;
597
                time_flag=1;
598
                T1IF = 0;
599
        }
600
#else
601
        #error "You have to define any asserv mode in configuration project"
602
#endif