2018-05-08 00:45:08 +02:00
|
|
|
#include <stdbool.h>
|
2018-05-05 15:56:39 +02:00
|
|
|
#include <stdio.h>
|
2018-05-06 18:35:26 +02:00
|
|
|
#include <unistd.h>
|
2018-05-01 14:51:41 +02:00
|
|
|
|
|
|
|
#include "buttons.h"
|
2018-05-06 18:35:26 +02:00
|
|
|
#include "diagnostics.h"
|
2018-05-01 14:51:41 +02:00
|
|
|
#include "lcd.h"
|
|
|
|
|
2018-05-08 00:45:08 +02:00
|
|
|
#include "CA.h"
|
2018-05-01 14:51:41 +02:00
|
|
|
#include "CF.h"
|
2018-05-08 00:45:08 +02:00
|
|
|
#include "actionneurs.h"
|
2018-05-05 15:56:39 +02:00
|
|
|
#include "imu.h"
|
2018-05-06 18:35:26 +02:00
|
|
|
#include "motor.h"
|
2018-05-06 12:50:03 +02:00
|
|
|
#include "position.h"
|
2018-05-01 14:51:41 +02:00
|
|
|
|
|
|
|
bool recu;
|
|
|
|
|
|
|
|
void setRecu()
|
|
|
|
{
|
|
|
|
recu = true;
|
|
|
|
}
|
|
|
|
|
2018-05-02 08:26:35 +02:00
|
|
|
bool diagFPGA(void* arg)
|
2018-05-01 14:51:41 +02:00
|
|
|
{
|
2018-05-06 18:35:26 +02:00
|
|
|
(void)arg;
|
2018-05-02 08:26:35 +02:00
|
|
|
|
2018-05-01 14:51:41 +02:00
|
|
|
recu = false;
|
2018-05-07 20:25:38 +02:00
|
|
|
registerRxHandlerCF(C2FD_PING, setRecu);
|
2018-05-01 14:51:41 +02:00
|
|
|
sendCF(C2FD_PING, NULL, 0);
|
|
|
|
|
|
|
|
for (int i = 0; i <= DIAGNOSTIC_SERIAL_TIMEOUT; i += DIAGNOSTIC_POLL_INTERVAL) {
|
|
|
|
if (recu) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
usleep(DIAGNOSTIC_POLL_INTERVAL * 1000);
|
|
|
|
}
|
2018-05-07 20:25:38 +02:00
|
|
|
registerRxHandlerCF(C2FD_PING, NULL);
|
2018-05-01 14:51:41 +02:00
|
|
|
return recu;
|
|
|
|
}
|
|
|
|
|
2018-05-02 08:26:35 +02:00
|
|
|
bool diagArduino(void* arg)
|
2018-05-01 14:51:41 +02:00
|
|
|
{
|
2018-05-06 18:35:26 +02:00
|
|
|
(void)arg;
|
2018-05-08 00:45:08 +02:00
|
|
|
|
|
|
|
recu = false;
|
|
|
|
registerRxHandlerCA(C2AD_PING, setRecu);
|
|
|
|
sendCA(C2AD_PING, NULL, 0);
|
|
|
|
|
|
|
|
for (int i = 0; i <= DIAGNOSTIC_SERIAL_TIMEOUT; i += DIAGNOSTIC_POLL_INTERVAL) {
|
|
|
|
if (recu) {
|
|
|
|
break;
|
|
|
|
}
|
|
|
|
usleep(DIAGNOSTIC_POLL_INTERVAL * 1000);
|
|
|
|
}
|
|
|
|
registerRxHandlerCA(C2AD_PING, NULL);
|
|
|
|
return recu;
|
2018-05-01 14:51:41 +02:00
|
|
|
}
|
|
|
|
|
2018-05-02 08:26:35 +02:00
|
|
|
bool diagCodeuse(void* arg)
|
|
|
|
{
|
2018-05-06 18:35:26 +02:00
|
|
|
int i = *((int*)arg);
|
2018-05-02 08:26:35 +02:00
|
|
|
long lCod, rCod;
|
|
|
|
getCoders(&lCod, &rCod);
|
|
|
|
float tension = DIAGNOSTIC_TENSION_TEST;
|
|
|
|
if (i % 2 == 1) { // Arrière
|
2018-05-06 18:35:26 +02:00
|
|
|
tension = -tension;
|
2018-05-02 08:26:35 +02:00
|
|
|
}
|
|
|
|
if (i < 2) {
|
2018-05-06 18:35:26 +02:00
|
|
|
setMoteurTension(tension, 0);
|
2018-05-02 08:26:35 +02:00
|
|
|
} else {
|
2018-05-06 18:35:26 +02:00
|
|
|
setMoteurTension(0, tension);
|
2018-05-02 08:26:35 +02:00
|
|
|
}
|
2018-05-06 18:35:26 +02:00
|
|
|
usleep(DIAGNOSTIC_TEMPS_ROTATION * 1000);
|
2018-05-02 08:26:35 +02:00
|
|
|
brake();
|
|
|
|
long lCodn, rCodn;
|
|
|
|
getCoders(&lCodn, &rCodn);
|
|
|
|
long diff;
|
|
|
|
if (i < 2) {
|
|
|
|
diff = lCodn - lCod;
|
|
|
|
} else {
|
|
|
|
diff = rCodn - rCod;
|
|
|
|
}
|
|
|
|
if (i % 2 == 0) { // Avant
|
|
|
|
return (diff > DIAGNOSTIC_CODEUSES_DIFF_MIN);
|
|
|
|
} else { // Arrière
|
|
|
|
return (-diff > DIAGNOSTIC_CODEUSES_DIFF_MIN);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2018-05-05 15:56:39 +02:00
|
|
|
bool diagIMU(void* arg)
|
|
|
|
{
|
2018-05-06 18:35:26 +02:00
|
|
|
(void)arg;
|
2018-05-05 15:56:39 +02:00
|
|
|
return connectedIMU();
|
|
|
|
}
|
|
|
|
|
2018-05-08 00:45:08 +02:00
|
|
|
bool diagJustRun(void* arg)
|
|
|
|
{
|
|
|
|
void (*fonction)(void) = arg;
|
|
|
|
fonction();
|
2018-05-09 09:57:11 +02:00
|
|
|
usleep(1000*1000);
|
2018-05-08 00:45:08 +02:00
|
|
|
return true;
|
|
|
|
}
|
|
|
|
|
|
|
|
void diagSetLoquetOuvert()
|
|
|
|
{
|
|
|
|
setLoquet(true);
|
|
|
|
}
|
|
|
|
void diagSetLoquetFerme()
|
|
|
|
{
|
|
|
|
setLoquet(false);
|
|
|
|
}
|
|
|
|
void diagSetPositionBalleAttente()
|
|
|
|
{
|
|
|
|
setPositionBalle(attente);
|
|
|
|
}
|
|
|
|
void diagSetPositionBalleEjection()
|
|
|
|
{
|
|
|
|
setPositionBalle(ejection);
|
|
|
|
}
|
|
|
|
void diagSetPositionBalleEvacuation()
|
|
|
|
{
|
|
|
|
setPositionBalle(evacuation);
|
|
|
|
}
|
|
|
|
void diagSetPropulsionOn()
|
|
|
|
{
|
|
|
|
setPropulsion(true);
|
|
|
|
}
|
|
|
|
void diagSetPropulsionOff()
|
|
|
|
{
|
|
|
|
setPropulsion(false);
|
|
|
|
}
|
|
|
|
|
|
|
|
void execDiagnostic(char* name, diagnosticFunc diag, void* arg)
|
2018-05-01 14:51:41 +02:00
|
|
|
{
|
|
|
|
clearLCD();
|
|
|
|
printToLCD(LCD_LINE_1, name);
|
|
|
|
printToLCD(LCD_LINE_2, "...");
|
2018-05-08 00:45:08 +02:00
|
|
|
bool res = diag(arg);
|
2018-05-01 14:51:41 +02:00
|
|
|
if (res) {
|
|
|
|
printToLCD(LCD_LINE_2, "Ok!");
|
|
|
|
usleep(DIAGNOSTIC_INTERVAL * 1000);
|
|
|
|
} else {
|
|
|
|
printToLCD(LCD_LINE_2, "Echec!");
|
|
|
|
pressedButton(BUT_BLOCK);
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
void runDiagnostics()
|
|
|
|
{
|
2018-05-02 08:26:35 +02:00
|
|
|
execDiagnostic("Lien FPGA", diagFPGA, NULL);
|
2018-05-08 00:45:08 +02:00
|
|
|
execDiagnostic("Lien Arduino", diagArduino, NULL);
|
2018-05-05 15:56:39 +02:00
|
|
|
execDiagnostic("Lien IMU", diagIMU, NULL);
|
2018-05-02 08:26:35 +02:00
|
|
|
int i;
|
2018-05-06 18:35:26 +02:00
|
|
|
i = 0;
|
|
|
|
execDiagnostic("Mot+Cod L AV", diagCodeuse, &i);
|
|
|
|
i = 1;
|
|
|
|
execDiagnostic("Mot+Cod L AR", diagCodeuse, &i);
|
|
|
|
i = 2;
|
|
|
|
execDiagnostic("Mot+Cod R AV", diagCodeuse, &i);
|
|
|
|
i = 3;
|
|
|
|
execDiagnostic("Mot+Cod R AR", diagCodeuse, &i);
|
2018-05-08 00:45:08 +02:00
|
|
|
|
|
|
|
execDiagnostic("Fermeture loquet", diagJustRun, &diagSetLoquetFerme);
|
2018-05-08 19:20:17 +02:00
|
|
|
execDiagnostic("Ouverture loquet", diagJustRun, &diagSetLoquetOuvert);
|
2018-05-08 00:45:08 +02:00
|
|
|
execDiagnostic("Reset barillet", diagJustRun, &barilletReset);
|
|
|
|
execDiagnostic("T+1 barillet", diagJustRun, &barilletSuivant);
|
|
|
|
execDiagnostic("T+2 barillet", diagJustRun, &barilletSkip);
|
|
|
|
execDiagnostic("Pousser balle", diagJustRun, &pousserBalle);
|
|
|
|
execDiagnostic("Pos. ejection", diagJustRun, &diagSetPositionBalleEjection);
|
|
|
|
execDiagnostic("Pos. evacuation", diagJustRun, &diagSetPositionBalleEvacuation);
|
2018-05-08 19:20:17 +02:00
|
|
|
execDiagnostic("Pos. attente", diagJustRun, &diagSetPositionBalleAttente);
|
2018-05-08 00:45:08 +02:00
|
|
|
execDiagnostic("Propulsion on", diagJustRun, &diagSetPropulsionOff);
|
2018-05-08 19:20:17 +02:00
|
|
|
execDiagnostic("Propulsion off", diagJustRun, &diagSetPropulsionOn);
|
2018-05-01 14:51:41 +02:00
|
|
|
}
|