Newer
Older
#include "asservissement.h"
#include "misc.h"
#define DBG_SIZE 250
/* DEBUG AA */
#include "../hardware/leds.h"
//int roueGauche[DBG_SIZE];
//int roueDroite[DBG_SIZE];
float vitesseLin[DBG_SIZE];
float vitesseLinE[DBG_SIZE];
float linearDuty[DBG_SIZE];
float vitesseAng[DBG_SIZE];
float vitesseAngE[DBG_SIZE];
//float posx[DBG_SIZE];
//float posy[DBG_SIZE];
//float angle[DBG_SIZE];
uint32_t dbgInc = 0;
Command* Asservissement::commandDebugTest = NULL;
int Asservissement::counter = 0;
Asservissement * Asservissement::asservissement = NULL; //Pour que nos variables static soient défini
bool Asservissement::matchFini = false;
const uint16_t Asservissement::nb_ms_between_updates = MS_BETWEEN_UPDATE;
Asservissement::Asservissement(Odometrie* _odometrie) :
seuil_collision(SEUIL_COLISION),
buffer_collision(0xffffffff)
{
odometrie = _odometrie;
linearDutySent = 0;
angularDutySent = 0;
Asservissement::asservissement = this;
asserCount = 0;
#ifdef CAPTEURS
sensors = Sensors::getSensors();
#endif
#ifdef ROBOTHW //on définie les interruptions possibles dues à certains ports
*((uint32_t *)(STK_CTRL_ADDR)) = 0x03; // CLKSOURCE:0 ; TICKINT: 1 ; ENABLE:1
*((uint32_t *)(STK_LOAD_ADDR)) = 9000*nb_ms_between_updates; // valeur en ms*9000 (doit etre inférieur à 0x00FFFFFF=16 777 215)
// le micro controlleur tourne à une frequence f (72Mhz ici), la valeur à mettre est (0.001*(f/8))*(temps en ms entre chaque update)
NVIC_InitTypeDef SysTick_IRQ;
SysTick_IRQ.NVIC_IRQChannel = SysTick_IRQn;
SysTick_IRQ.NVIC_IRQChannelCmd = ENABLE;
SysTick_IRQ.NVIC_IRQChannelPreemptionPriority = 0;
SysTick_IRQ.NVIC_IRQChannelSubPriority = 1;
NVIC_Init(&SysTick_IRQ);
#endif
}
void Asservissement::setLinearSpeed(Vitesse vitesse)
{
vitesseLineaire = vitesse;
}
void Asservissement::setAngularSpeed(VitesseAngulaire vitesse)
{
vitesseAngulaire = vitesse;
}
void Asservissement::setCommandSpeeds(Command* command)
{
if (command != NULL)
{
setLinearSpeed(command->getLinearSpeed());
setAngularSpeed(command->getAngularSpeed());
}
else
{
setLinearSpeed(0.0f);
setAngularSpeed(0.0f);
}
}
Distance Asservissement::getLinearSpeed()
{
}
Angle Asservissement::getAngularSpeed()
{
}
void Asservissement::update(void)
#ifdef CAPTEURS
AnalogSensor::startConversion(); // On lance la conversion des données que l'on reçois des capteurs pour les avoir au bon moment
#endif
*/
asserCount++;
/*
#ifdef ROUES
//On arrete le robot pour être sur que tout soit réinitialisé
if(asserCount > CPT_BEFORE_RAZ)
{
roues.gauche.tourne(0);
roues.droite.tourne(0);
}
if (1/*asserCount< 85000/MS_BETWEEN_UPDATE && !Asservissement::matchFini*/)
{
PositionPlusAngle positionPlusAngleActuelle = odometrie->getPos(); //Variable juste pour avoir un code plus lisible par la suite
Angle vitesse_angulaire_atteinte = odometrie->getVitesseAngulaire(); //idem
Distance vitesse_lineaire_atteinte = odometrie->getVitesseLineaire(); //idem
#ifdef ROUES
/*
#ifdef CAPTEURS
bool testcap = sensors->detectedSharp()->getSize() > 0;
#else
bool testcap = false;
/*if (testcap)
{
Command::freinageDUrgence(true);
// linearDutySent = 0;
// angularDutySent = 0;
}
else
Command::freinageDUrgence(false);*/
#endif //ROUES
//Puis on les récupéres
float vitesse_lineaire_a_atteindre = getLinearSpeed();
// vitesse_lineaire_a_atteindre += ACCELERATION_LINEAIRE_MAX;
// vitesse_lineaire_a_atteindre = (vitesse_lineaire_a_atteindre >= 4.0) ? 4.0f : vitesse_lineaire_a_atteindre;
float vitesse_angulaire_a_atteindre = getAngularSpeed();
// vitesse_angulaire_a_atteindre += ACCELERATION_ANGULAIRE_MAX;
// vitesse_angulaire_a_atteindre = (vitesse_angulaire_a_atteindre >= VITESSE_ANGULAIRE_MAX) ? VITESSE_ANGULAIRE_MAX : vitesse_angulaire_a_atteindre;
//vitesse_angulaire_a_atteindre = (vitesse_angulaire_a_atteindre <= -VITESSE_ANGULAIRE_MAX) ? -VITESSE_ANGULAIRE_MAX : vitesse_angulaire_a_atteindre;
// le buffer de collision se vide si l'accélération demandé est trop forte. Normalement la commande vérifie ça.
/* //Il faudrai qu'il passe de marche arriére à marche avant à toute vitesse pour avoir une collision ...
buffer_collision <<= 1;
buffer_collision |= fabs((vitesse_lineaire_atteinte - vitesse_lineaire_a_atteindre)) < seuil_collision && fabs((vitesse_angulaire_atteinte - vitesse_angulaire_a_atteindre)) < SEUIL_COLISION_ANG;
*/
#ifdef ROUES
//on filtre l'erreur de vitesse lineaire et angulaire
linearDutySent = pid_filter_distance.getFilteredValue(vitesse_lineaire_a_atteindre-vitesse_lineaire_atteinte);
angularDutySent = pid_filter_angle.getFilteredValue(vitesse_angulaire_a_atteindre-vitesse_angulaire_atteinte);
//Et on borne la somme de ces valeurs filtrée entre -> voir ci dessous
float limit = 1.0f;
linearDutySent = MIN(MAX(linearDutySent, -limit),limit);
angularDutySent = MIN(MAX(angularDutySent, -limit),limit);
//On évite que le robot fasse du bruit quand il est à l'arrêt
// linearDutySent = fabs(linearDutySent) > 0.05 || vitesse_lineaire_a_atteindre > 0.01 ? linearDutySent : 0;
// angularDutySent = fabs(angularDutySent) > 0.05 || vitesse_angulaire_a_atteindre > 0.0001 ? angularDutySent : 0;
// test d'arret complet si c'est l'ordre qu'on lui donne
if (vitesse_lineaire_a_atteindre == 0.0f && vitesse_angulaire_a_atteindre == 0.0f)
{
linearDutySent = 0.0f;
angularDutySent = 0.0f;
if (0/*buffer_collision == 0x0 */)//|| GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_11) == Bit_RESET) //Actif si le buffer_de colision est vide.
{ //Si on détecte quelque chose, on s'arréte
roues.gauche.tourne(0.);
roues.droite.tourne(0.);
}
else
{ //Sinon les roues tourne de façon borné et le fait d'avoir filtrées les valeurs permet de compenser les erreurs passées et de faire tournées chaque roues de façon
// à tourner et avancer correctement
limit = 1.0f;
roues.gauche.tourne(0.4*MIN(MAX(+linearDutySent-angularDutySent, -limit),limit));
roues.droite.tourne(0.4*MIN(MAX(+linearDutySent+angularDutySent, -limit),limit));
vitesseLin[dbgInc] = vitesse_lineaire_atteinte;
vitesseLinE[dbgInc] = vitesse_lineaire_a_atteindre;
linearDuty[dbgInc] = linearDutySent;
vitesseAng[dbgInc] = vitesse_angulaire_atteinte;
vitesseAngE[dbgInc] = vitesse_angulaire_a_atteindre;
angularDuty[dbgInc] = angularDutySent;
}
else
{
roues.gauche.tourne(0.0);
roues.droite.tourne(0.0);
dbgInc++;
}
}
else
{
}
#else
}
#endif
}
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
// allume ou éteint une LED
void xallumerLED()
{
#ifdef STM32F10X_MD // stm32 h103
GPIO_WriteBit(GPIOC, GPIO_Pin_12, Bit_RESET);
#endif
#ifdef STM32F10X_CL // stm32 h107
GPIO_WriteBit(GPIOC, GPIO_Pin_6, Bit_SET); // LED verte
#endif
}
void xeteindreLED()
{
#ifdef STM32F10X_MD // stm32 h103
GPIO_WriteBit(GPIOC, GPIO_Pin_12, Bit_SET);
#endif
#ifdef STM32F10X_CL // stm32 h107
GPIO_WriteBit(GPIOC, GPIO_Pin_6, Bit_RESET); // LED verte
#endif
}
// 2ème LED du stm h107 (LED jaune)
#ifdef STM32F10X_CL
void xallumerLED2()
{
GPIO_WriteBit(GPIOC, GPIO_Pin_7, Bit_SET);
}
void xeteindreLED2()
{
GPIO_WriteBit(GPIOC, GPIO_Pin_7, Bit_RESET);
}
#endif
int currentTimer = 0;
//PositionPlusAngle posOdo;
#ifdef ROBOTHW
//pour lancer l'update à chaque tic d'horloge
extern "C" void SysTick_Handler()
{
/*
static Roue* ascensseur = NULL;
if (ascensseur==NULL)
ascensseur = new Roue(TIM5, 3, GPIOA, GPIO_Pin_2, GPIOD, GPIO_Pin_5);
if (GPIO_ReadInputDataBit(GPIOE, GPIO_Pin_4) == Bit_SET)
ascensseur->tourne(-1.0f);
else
ascensseur->tourne(1.0f);
//currentTimer++;
float xx = Odometrie::odometrie->getPos().position.x;
if (Asservissement::counter>100)
{
allumerLED();
if (Asservissement::counter>200)
Asservissement::counter = 0;
}
else
Sensors* sensors = Sensors::getSensors();
if (sensors != NULL)
sensors->update();
#endif
*/
StrategieV2::update();
/*Asservissement::commandDebugTest->update();
if (Asservissement::commandDebugTest->fini())
{
Asservissement::asservissement->setAngularSpeed(0.);
Asservissement::asservissement->setLinearSpeed(0.);
}
else
Asservissement::asservissement->setCommandSpeeds(Asservissement::commandDebugTest);*/
/*if (StrategieV2::strategie != NULL)
StrategieV2::strategie->update();*/
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
Asservissement::asservissement->update();
/*
static int i = 0;
if (i % 200 == 0)
{
// ascensseur->tourne(0.9f);
// Asservissement::asservissement->roues.gauche.tourne(0.7f);
// Asservissement::asservissement->roues.droite.tourne(-0.5f);
xeteindreLED();
xallumerLED2();
}
else if (i % 100 == 0)
{
// ascensseur->tourne(-0.7f);
// Asservissement::asservissement->roues.gauche.tourne(-0.5f);
// Asservissement::asservissement->roues.droite.tourne(0.8f);
xallumerLED();
xeteindreLED2();
}
i++;
*/
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
/*
if (Odometrie::odometrie->ang < 0.2*3.1415/180.0 && Odometrie::odometrie->ang > -0.2*3.1415/180.0)
{
GPIO_WriteBit(GPIOC, GPIO_Pin_6, Bit_SET);
GPIO_WriteBit(GPIOC, GPIO_Pin_7, Bit_SET);
}
else
{
GPIO_WriteBit(GPIOC, GPIO_Pin_7, Bit_RESET);
GPIO_WriteBit(GPIOC, GPIO_Pin_6, Bit_RESET);
}
*/
/*
Odometrie::odometrie->update();
if (Odometrie::odometrie->posX > -5000.0)
{
GPIO_WriteBit(GPIOC, GPIO_Pin_6, Bit_RESET);
GPIO_WriteBit(GPIOC, GPIO_Pin_7, Bit_SET);
}
else if (Odometrie::odometrie->posX < -5000.0)
{
GPIO_WriteBit(GPIOC, GPIO_Pin_7, Bit_RESET);
GPIO_WriteBit(GPIOC, GPIO_Pin_6, Bit_SET);
}
else
{
GPIO_WriteBit(GPIOC, GPIO_Pin_7, Bit_RESET);
GPIO_WriteBit(GPIOC, GPIO_Pin_6, Bit_RESET);
}
*/
/*
static CapteurCouleur* c = NULL;
static int seuilMax = 200;
static int seuilMin = 120;
if (c == NULL)
c = new CapteurCouleur(TIM2, GPIOA, GPIO_Pin_6);
static uint16_t ticks[DBG_SIZE];
static int nbBoucles = 0;
if (nbBoucles % 200 == 100)
GPIO_WriteBit(GPIOC, GPIO_Pin_12, Bit_SET);
if (nbBoucles % 200 == 199)
GPIO_WriteBit(GPIOC, GPIO_Pin_12, Bit_RESET);*/
/*
uint16_t nbTicks = c->getTickValue();
if (nbTicks > seuilMax)
xallumerLED2();
else
xeteindreLED2();
if (nbTicks < seuilMin)
xallumerLED();
else
xeteindreLED();*/
/*
if (nbBoucles < DBG_SIZE)
{
ticks[nbBoucles] = c->getTickValue();
nbBoucles++;
}
else
{
nbBoucles++;
}
void Asservissement::finMatch()
{
Asservissement::matchFini = true;
#ifdef ROBOTHW