mirror of
https://github.com/RobotechLille/cdf2018-principal
synced 2024-05-05 13:21:45 +00:00
43 lines
1.2 KiB
C
43 lines
1.2 KiB
C
#include <stdbool.h>
|
|
#include <stdio.h>
|
|
#include <unistd.h>
|
|
|
|
#include "calibrage.h"
|
|
|
|
#include "movement.h"
|
|
#include "dimensions.h"
|
|
#include "securite.h"
|
|
#include "motor.h"
|
|
#include "common.h"
|
|
|
|
void calibrer(bool orange)
|
|
{
|
|
struct position pos = {0, 0, 0};
|
|
setPosition(&pos);
|
|
return;
|
|
|
|
// Calibrage contre mur court
|
|
/* pos.o = orange ? M_PI_2 : -M_PI_2; */
|
|
setDestination(&pos);
|
|
waitDestination();
|
|
disableAsservissement();
|
|
|
|
float frontL, frontR, backL, backR;
|
|
struct movAvg diffL, diffR;
|
|
initMovAvg(&diffL, CALIBRAGE_MOVAVG_SIZE);
|
|
initMovAvg(&diffR, CALIBRAGE_MOVAVG_SIZE);
|
|
for (int i = 0; i < CALIBRAGE_TEMPS_ACQUISITION; i += SENSOR_SAMPLING_INTERVAL) {
|
|
getAllDistance(&frontL, &frontR, &backL, &backR);
|
|
addMovAvg(&diffL, frontL - CALIBRAGE_DISTANCE_Y);
|
|
addMovAvg(&diffR, frontR - CALIBRAGE_DISTANCE_Y);
|
|
setMoteurTension(diffL.current * D_KP, diffR.current * D_KP);
|
|
usleep(SENSOR_SAMPLING_INTERVAL * 1000);
|
|
}
|
|
// Les moteurs n'auront peut-être pas atteint le point voulu exactement,
|
|
// on enregistre alors la différence
|
|
float diff = (diffL.current + diffR.current) / 2;
|
|
pos.y = diff;
|
|
|
|
|
|
}
|