Arcar5 Project P3

ginput

ginput code for race mode and obstacle for camera aquisition

/*
 ------------------------------------------------------------
 Copyright 2003-201x Haute �cole ARC Ing�ni�rie, Switzerland.
 All rights reserved.
 ------------------------------------------------------------
 Nom du fichier :	gInput.c
 Author and date :	Alaric Clot Lorenzin 2020

 Description dans le fichier gInput.h
 ------------------------------------------------------------
 */

#include "gInput.h"
#include "mSpi.h"
#include "mAd.h"
#include "mTimer.h"
#include "mVL6180x.h"
#include "mAccelMagneto.h"
#include "mGyro.h"
#include "mSwitch.h"
#include "mRS232.h"
#include "def.h"
#include "stdio.h"


//-----------------------------------------------------------------------------
// Configuration du gestionnaire
//-----------------------------------------------------------------------------
void gInput_Setup(void)
{
	sInput.bImageOk=0;
}

//-----------------------------------------------------------------------------
// Ex�cution du gestionnaire
//-----------------------------------------------------------------------------
void gInput_Execute(void)
{
	//-----------------------------------------------------------------------
	// Acquisition de l'image non bloquante de la cam�ra 2
	// Le temps d'acquisition provient des potentiom�tres
	//-----------------------------------------------------------------------
	sInput.bImageOk=mSpi_GetImageCam2_Pot((float)845,sInput.au16Tab,&sInput.u8PixelValMoy);
	sInput.fPot1Value=mAd_Read(kPot1);
	sInput.fPot2Value=mAd_Read(kPot2);
}

gcompute

gcompute code for race mode or obstacle

/*
------------------------------------------------------------
Copyright 2003-20xx Haute �cole ARC Ing�ni�rie, Switzerland. 
All rights reserved.
------------------------------------------------------------
Nom du fichier :	gCompute.c
Auteur et Date :	Alaric Clot Lorenzin

Description dans le fichier gCompute.h
------------------------------------------------------------
*/


#include "gCompute.h"

//-----------------------------------------------------------------------------
// Configuration du gestionnaire
//-----------------------------------------------------------------------------
void gCompute_Setup(void)
{
}

//-----------------------------------------------------------------------------
// Ex�cution du gestionnaire
//-----------------------------------------------------------------------------
void gCompute_Execute(void)
{
	//UInt8 u8PixelMoyComp;
	static UInt8 u8PixelMilieux = 71;
	UInt16 u16ReturnLignes;

	if(sInput.bImageOk==true)
	{
		vCompensation(sInput.au16Tab,sCompute.au8Compense,&sInput.u8PixelValMoy);
		u16ReturnLignes = DetectionLigne(sCompute.au8Compense,sInput.u8PixelValMoy,RANGE);//tableau,seuil,range
		u8PixelMilieux = (u16ReturnLignes>>4);
		u16ReturnLignes = (u16ReturnLignes&0x7);
		sCompute.u8ReturnLines = (UInt8)u16ReturnLignes;
		sCompute.fKp = fPotToGain(sInput.fPot1Value);
		sCompute.fSpeedValue = fPotToSpeed(sInput.fPot2Value);
		/*if(mSwitch_ReadPushBut(kPushButSW1)==true || u16ReturnLignes == 0)
		{
			PID(u8PixelMilieux,1, GAIN_KP_DROIT);
		}*/
		if(sOutput.u16State==vitesse_constante && u16ReturnLignes!=0x7)
		{
			sCompute.lfServo=PID(u8PixelMilieux,0, GAIN_KP_DROIT);
		}
		else if (sOutput.u16State==virage_droite || sOutput.u16State==virage_gauche ||sOutput.u16State==freinage_droite ||sOutput.u16State==freinage_gauche)
		{
			if((sOutput.u16State==virage_droite||sOutput.u16State==freinage_droite) && (u16ReturnLignes==0x6||u16ReturnLignes==0x2))//virage droite accepte que les bons pixels pour la régul sinon on ne modifie pas le virage
			{
				sCompute.lfServo=PID(u8PixelMilieux,0, GAIN_KP_DROIT);
			}
			if((sOutput.u16State==virage_gauche||sOutput.u16State==freinage_gauche) && (u16ReturnLignes==0x5||u16ReturnLignes==0x1))
			{
				sCompute.lfServo=PID(u8PixelMilieux,0, GAIN_KP_DROIT);
			}
		}
		else //if(u16ReturnLignes!=0x7)//else
		{
			sCompute.lfServo=PID(u8PixelMilieux,0,(float)GAIN_KP_DROIT);
		}

		sCompute.u8PixelMilieux = u8PixelMilieux;
		if(u16ReturnLignes == 0x7 && sOutput.u16State == vitesse_constante)
		{
			sCompute.u8Flag = 1;
		}
		else if(u16ReturnLignes>>2 == 0 && sOutput.u16State == init)
		{
			sCompute.u8Flag=0;
		}
	}
}

void gCompute_Execute_evitement(void)
{
	//UInt8 u8PixelMoyComp;
	static UInt8 u8PixelMilieux = 71;
	static UInt16 u16ReturnLignes;
	//rajout obstacle
	Int16 i16BordDiff_gauche=0;
	Int16 i16BordDiff_droite=0;
	//fin
	if(sInput.bImageOk==true)
	{
		//rajout detection obstacle
		vCompensation((const UInt16*) sInput.au16Tab,(UInt8* )sCompute_evitement.au8Compense,(UInt8*)&sInput.u8PixelValMoy);
		u16ReturnLignes = DetectionLigne_evitement((UInt8*)sCompute_evitement.au8Compense, sInput.u8PixelValMoy, RANGE, &i16BordDiff_gauche, &i16BordDiff_droite);//tableau,seuil,range
		u8PixelMilieux = (UInt8)(u16ReturnLignes>>8);
		u16ReturnLignes = (u16ReturnLignes&0x7);
		sCompute_evitement.u16ReturnLines = u16ReturnLignes;
		//Appel machine à etat ici
		vCircuit_evitement(u8PixelMilieux, (UInt8)u16ReturnLignes, i16BordDiff_gauche, i16BordDiff_droite, (void*)&sCompute_evitement);
		//fin
	}
}

goutput_header

/*
------------------------------------------------------------
Copyright 2003-20xx Haute �cole ARC Ing�ni�rie, Switzerland. 
All rights reserved.
------------------------------------------------------------
Nom du fichier : 	gOutput.h	
Auteur et Date :	Alaric Clot Lorenzin 2020

But : Gestionnaire de sortie

Modifications
Date		Faite	Ctrl		Description
------------------------------------------------------------
*/

#ifndef __GOUTPUT__
#define __GOUTPUT__
#include "global.h"
//-----------------------------------------------------------------------------
// Configuration du gestionnaire
//-----------------------------------------------------------------------------
void gOutput_Setup(void);

//-----------------------------------------------------------------------------
// Ex�cution du gestionnaire
//-----------------------------------------------------------------------------
void gOutput_Execute(void);
void gOutput_Execute_evitement(void);

#endif

goutput

output to servo and motors for evitement=obstacle or race mode

/*
------------------------------------------------------------
Copyright 2003-20xx Haute �cole ARC Ing�ni�rie, Switzerland. 
All rights reserved.
------------------------------------------------------------
Nom du fichier :	gOutput.c
Auteur et Date :	Alaric Clot Lorenzin 2020

Description dans le fichier gOutput.h
------------------------------------------------------------
*/


#include "gOutput.h"
#include "def.h"

//-------------------------------------------------------------------------
// D�claration des constantes
//-------------------------------------------------------------------------

//-----------------------------------------------------------------------------
// D�claration des variables globales
//-----------------------------------------------------------------------------
static Int32 aWakeIntMain=0;
static float fVitesse=VITESSE;
//-----------------------------------------------------------------------------
// Configuration du gestionnaire
//-----------------------------------------------------------------------------
void gOutput_Setup(void)
{
	//LED reglage
	mDac_SetDac0Output(1);//spot led on
	mLeds_Write(kMaskLed3,0);
	mLeds_Write(kMaskLed2,0);
	aWakeIntMain=mDelay_GetDelay(kPit2, 10/kPit1Period);
	if(mSwitch_ReadSwitch(kSw4)==true&&mSwitch_ReadSwitch(kSw3)==true)
	{
		fVitesse=4000;
	}
	else if(mSwitch_ReadSwitch(kSw4)==true)
	{
		fVitesse=2700;
	}
	else if(mSwitch_ReadSwitch(kSw3)==true)
	{
		fVitesse=3600;
	}
}

//-----------------------------------------------------------------------------
// Ex�cution du gestionnaire
//-----------------------------------------------------------------------------
void gOutput_Execute(void)
{
	static UInt8 ui8CountStop=0;
	if(sInput.bImageOk==true)
	{
		if(mSwitch_ReadSwitch(kSw2)==true)
		{
			if(sCompute.u8ReturnLines==0)
			{
				vControleServo(0);
				//mTimer_SetServoDuty(0,(double)0);
			}
			else
			{
				vControleServo(sCompute.lfServo);
			}
		}
		vLedOutput(sCompute.u8ReturnLines);
	}
//	if (mDelay_IsDelayDone(kPit2, aWakeIntMain))
//	{
//		mDelay_ReStart(kPit2, aWakeIntMain, 3 / kPit1Period);
		sOutput.u16State = vCircuit(sCompute.fSpeedValue, sCompute.u8Flag, sCompute.u8PixelMilieux, 0, sCompute.u8ReturnLines);
//	}
}

void gOutput_Execute_evitement(void)
{
	vLedOutput(sCompute_evitement.u16ReturnLines);
	vControleServo(sCompute_evitement.lfServo);
	mTimer_SetMotorDuty(sCompute_evitement.MotGauche, sCompute_evitement.MotGauche);
}

global structs

//Alaric Clot Lorenzin
#include "global.h"
struct Input sInput;
struct Compute sCompute;
struct Compute_evitement sCompute_evitement;
struct Output sOutput;

fonctions

function that we use

/*
 * fonctions.c
 *
 *  Created on: 19 oct. 2020
 *      Author: Alaric Clot Lorenzin
 */
#include "fonctions.h"

void vCompensation(const UInt16* const aUItableau, UInt8* aUIresultat, UInt8* cMoy)
{
	double lfTemp1 = 0;
	double lfSum = 0;
	for (int i = 0; i < 144; i++)
	{
		lfTemp1 = (double)aUItableau[13 + i] / fFcx(i+1);
		aUIresultat[i] = lfTemp1 * 250;//250
		lfSum += aUIresultat[i];
	}
	lfSum = (lfSum/143)*0.57;
	*cMoy = ((UInt8) lfSum);
}

//polynomial 4 used from vCompensation calculer avec matlab
double fFcx(UInt16 uiX)
{
	double lfTemp1 = 0,lfTemp2 = 0,lfTemp3 = 0, lfTemp4 = 0,lfResult=0;
	lfTemp1 = (5.05 * fPow(uiX, 4) * 0.000001);
	lfTemp2 = (1.453 * 0.001 * fPow(uiX, 3));
	lfTemp3 = (0.09416 * fPow(uiX, 2));
	lfTemp4 = (1.515* uiX);
	lfResult = lfTemp1 - lfTemp2 + lfTemp3 + lfTemp4 + 18.1;
	return lfResult;
}

//fx power uiPow in recursive
double fPow(double fX, UInt16 uiPow)
{
	if (uiPow > 1)
	{
		fX = fX * fPow(fX, uiPow - 1);//puissance recursive
	}
	return fX;
}

//Return value 1 2 3 if right, left or both lines detected
UInt32 DetectionLigne(UInt8* aUItableau, UInt8 UIseuil, UInt16 uiRange)
{
	static UInt8 u8LargeurPiste =0;
	Int16 Pixel_Bord[2]={0,0};
	static UInt8 Pixel_milieux = 0;
	UInt32 cLines = 0;
	Int16 EcartErreur = ((Int16)(143/2)-(Int16)PIXEL_MILIEUX);
	for (Int16 i = 0; i < (uiRange/2); i++)//verification lignes sur la range donnee
	{
		if (i > (Int16)(RANGE_MILIEUX) && aUItableau[i + PIXEL_MILIEUX]<UIseuil && Pixel_Bord[0]==0)//pixel bord droite, virage gauche
		{
			cLines = cLines | 1;
			Pixel_Bord[0] = i + PIXEL_MILIEUX;
			aUItableau[i + PIXEL_MILIEUX]=0;
		}
		if (i > (Int16)(RANGE_MILIEUX) && aUItableau[PIXEL_MILIEUX-i] < UIseuil && Pixel_Bord[1]==0)//pixel bord gauche, virage droite pixel 52 est mort rajouter i!=20 si prob
		{
			cLines = cLines | 2;
			Pixel_Bord[1] = PIXEL_MILIEUX-i;
			aUItableau[PIXEL_MILIEUX-i]=0;
		}
		if(i<(Int16)RANGE_MILIEUX && (aUItableau[PIXEL_MILIEUX+i] < UIseuil || aUItableau[PIXEL_MILIEUX-i] < UIseuil))
		{
			cLines = cLines | 4;
		}
		if(aUItableau[PIXEL_MILIEUX-i] > UIseuil)
		{
			aUItableau[PIXEL_MILIEUX-i]=255;
		}
		if(aUItableau[PIXEL_MILIEUX+i] > UIseuil)
		{
			aUItableau[PIXEL_MILIEUX+i]=255;
		}
	}
	if((cLines&0x3) == 3) //deux lignes
	{
		u8LargeurPiste = 95;
		Pixel_milieux = Pixel_Bord[0] - (u8LargeurPiste/2)  - EcartErreur;
	}
	else if((cLines&0x7)==1) //droite
	{
		Pixel_milieux = Pixel_Bord[0] - (u8LargeurPiste/2)  - EcartErreur;
	}
	else if((cLines&0x7)==2) //gauche
	{
		Pixel_milieux = Pixel_Bord[1] + (u8LargeurPiste/2)  - EcartErreur;
	}
	/*else if((cLines&0x7)==6 && Pixel_milieux!=0)//extremite gauche
	{
		Pixel_milieux = 120;
	}
	else if((cLines&0x7)==5&& Pixel_milieux!=140) //extremite droite
	{
		Pixel_milieux = 20;
	}*/

	cLines |= ((UInt16)Pixel_milieux<<4);
	return cLines;
	//return Pixel_milieux in 4 LSL
	//return 0x00 if no lines, 0x01 if line at the right, 0x02 if lines at the left, 0x03 if two lines, 0x04 if center line
}

//Output from DetectionLigne on LEDS
void vLedOutput(UInt8 cLED)
{
	UInt8 u8Center = cLED & 0x4;
	cLED = cLED & 0x3;
	mLeds_Write(kMaskLed1,0);
	mLeds_Write(kMaskLed2,0);
	mLeds_Write(kMaskLed3,0);
	mLeds_Write(kMaskLed4,0);
	if(u8Center == 4)
	{
		mLeds_Write(kMaskLed4,1); //led milieux
	}
	switch(cLED)
	{
	case 0 ://aucune ligne
		mLeds_Write(kMaskLed1,1);
		break;
	case 1://droite
		mLeds_Write(kMaskLed2,1);
		break;
	case 2://gauche
		mLeds_Write(kMaskLed3,1);
		break;
	case 3://droite ET gauche
		mLeds_Write(kMaskLed2,1);
		mLeds_Write(kMaskLed3,1);
	break;
	default ://ERROR STATE
		break;
	}
}

//ui8VitesseAttendue compris entre -1 et 1
UInt16 vCircuit(float fVitesseAtt, UInt8 ui8Stop, UInt8 u8Milieux, UInt8 u8EmergencyStop, UInt8 u8Lines)
{
	static enum State eState = init;
	static double fVitesse = 0;
	static UInt16 u16Brake;
	float aSpeedMotLeft,aSpeedMotRight;
	mTimer_GetSpeed(&aSpeedMotLeft, &aSpeedMotRight);
	if(u8EmergencyStop == 1)//verification si un arret d'urgence a ete fait
	{
		eState=init;
	}
	switch (eState)
	{
	case init :
		//mTimer_SetServoDuty(0U, 0.0);
		mTimer_SetMotorDuty(0.0 ,0.0);
		if(ui8Stop!=1 &&mSwitch_ReadPushBut(kPushButSW2)==true)//impossible de demarrer si pas de lignes ou une ligne au milieu
		{
			//PID(73,1);
			u16Brake=0;
			eState = vitesse_constante;
		}
		break;
	case vitesse_constante ://ligne droite
		u16Brake=0;
		fVitesse = fVitesseAtt;
		if(ui8Stop == 1)
		{
			eState = deceleration;
		}
		if(u8Milieux>(UInt8)PIXEL_MAX_GAUCHE)
		{
			eState= freinage_droite;
		}
		else if(u8Milieux<(UInt8)PIXEL_MAX_DROITE)
		{
			eState= freinage_gauche;
		}
		else
		{
			if(aSpeedMotLeft < 1650)
				vSetMotorSpeed(fVitesse, fVitesse);
			else
				vSetMotorSpeed(0, 0);
		}
		break;
	case freinage_gauche :
		vSetMotorSpeed(-fVitesse, -fVitesse);
		if(aSpeedMotLeft<=900)
		{
			eState = virage_gauche;
		}
		break;
	case freinage_droite :
		vSetMotorSpeed(-fVitesse, -fVitesse);
		if(aSpeedMotRight<=900)
		{
			eState = virage_droite;
		}
		break;
	case virage_gauche ://suppression detection ligne milieu en virage et differentiel
		if(u8Milieux<(UInt8)PIXEL_MAX_DROITE)
		{
			vSetMotorSpeed(fVitesse-DIFFERENTIEL_MOTEUR, fVitesse);
			//mTimer_SetMotorDuty(fVitesse-DIFFERENTIEL_MOTEUR-0.05, fVitesse+0.20+0.05);//0.2 optimal//0.35 pas mal //offset car un moteur différent de l'autre
		}
		else if((u8Milieux>(UInt8)PIXEL_MAX_DROITE && u8Milieux < PIXEL_MILIEUX)||(u8Lines==0))//retourne en ligne droite uniquement si notre pixel_milieux se retrouve dans l'intervalle PIXEL_MAX à Pixel_milieux vrai ou si croisement
		{
			eState = vitesse_constante;
		}
		break;
	case virage_droite ://suppression detection ligne milieu en virage et differentiel
		if(u8Milieux>(UInt8)PIXEL_MAX_GAUCHE)
		{
			vSetMotorSpeed(fVitesse, fVitesse-DIFFERENTIEL_MOTEUR);
			//mTimer_SetMotorDuty(fVitesse+0.20, fVitesse-DIFFERENTIEL_MOTEUR);
		}
		else if((u8Milieux<(UInt8)PIXEL_MAX_GAUCHE && u8Milieux > PIXEL_MILIEUX)||(u8Lines==0))//retourne en ligne droite uniquement si notre pixel_milieux se retrouve dans l'intervalle PIXEL_MAX à Pixel_milieux vrai ou si croisement
		{
			eState = vitesse_constante;
		}
		break;
	case deceleration ://freinage avec le timer dans l'appel (output)
		u16Brake++;
		vSetMotorSpeed(-fVitesse , -fVitesse);
		if(u16Brake>(UInt16)(TICKS_FREINAGE))
		{
			u16Brake=0;
			fVitesse = 0;
			vSetMotorSpeed(fVitesse , fVitesse);
			eState = init;
		}
		break;
	}
	return (UInt16)eState;
}

void vControleServo(double lfValue)
{
	//static UInt8 u8Flag_gauche=0, u8Flag_droite=0;
	lfValue=-lfValue+CORRECTION_SERVO; //on prends l'opposé car mauvais sens de correction
	/*if(lfValue<-0.15)
	{
		lfValue-=0;
	}*/
	if(lfValue>(double)BUTEE_SERVO) //on empeche le bloquage du servo à + de 0.75 virage gauche
	{
		lfValue=(double)BUTEE_SERVO;
	}
	if(lfValue<-(double)BUTEE_SERVO) //on empeche le bloquage du servo à - de 0.75 virage droite
	{
		lfValue=-(double)BUTEE_SERVO;
	}
	mTimer_SetServoDuty(0,lfValue+(double)CORRECTION_SERVO);//output sur servo
}


double PID (double lfMesure,UInt8 uReinit, float u16KP)
{
	if(lfMesure<0)
	{
		lfMesure=0;
	}
	//static double lfErreur_precedente;
	//static double lfSomme_erreurs;
	//double lfVariation_erreur;
	/*if(uReinit)
		{
			lfSomme_erreurs = 0;
		}
		if(lfMesure > 71 && lfSomme_erreurs > 0)
		{
			lfSomme_erreurs=0;
		}
		if(lfMesure < 71 && lfSomme_erreurs < 0)
		{
			lfSomme_erreurs=0;
		}*/
	double lfErreur;

	double lfConsigne = 71;

	// Paramètres du PID
	double Kp = u16KP/143.0;

	lfErreur = (double)lfConsigne - lfMesure;										// Calcul de l'erreur
	//lfSomme_erreurs += (lfErreur*Ki);											// Calcul de la somme (Partie I)
	//lfVariation_erreur = lfErreur -lfErreur_precedente;						// Calcul de la variation de l'erreur (Partie D)
	lfConsigne = (Kp*lfErreur);// + (lfSomme_erreurs) + (Kd*lfVariation_erreur);	// Régulation PID
	//lfErreur_precedente = lfErreur;											// Nouvelle valeur précédente
	return lfConsigne;
}

void vSetMotorSpeed(double lfSpeedLeft, double lfSpeedRight)
{
	float aSpeedMotLeft, aSpeedMotRight;
	float fEpsilonLeft, fEpsilonRight;
	float fEcartLeft=0, fEcartRight=0;

	//capteur de vitesse
	mTimer_GetSpeed(&aSpeedMotLeft, &aSpeedMotRight);
	aSpeedMotLeft/=VITESSE_MAX_MOT_GAUCHE;
	aSpeedMotRight/=VITESSE_MAX_MOT_DROITE;
	//Epsilon
	fEcartLeft=lfSpeedLeft-aSpeedMotLeft;
	fEcartRight=lfSpeedRight-aSpeedMotRight;

	//Epsilon sur KP
	fEpsilonLeft=(fEcartLeft/(float)KP_MOT);
	fEpsilonRight=(fEcartRight/(float)KP_MOT);

	//(Consigne-Epsilon sur KP)/VITESSE_MAX
	lfSpeedLeft=(lfSpeedLeft-fEpsilonLeft);
	lfSpeedRight=(lfSpeedRight-fEpsilonRight);
	mTimer_SetMotorDuty(lfSpeedLeft , lfSpeedRight);
	/*float aSpeedMotLeft, aSpeedMotRight;
	float fEpsilonLeft, fEpsilonRight;
	float fEcartLeft=0, fEcartRight=0;
	static double fErreurRight=0;
	static double fErreurLeft=0;
	if(lfSpeedLeft<0.1)
	{
		fErreurLeft=0;
	}
	if(lfSpeedRight<0.1)
	{
		fErreurRight=0;
	}
	//capteur de vitesse
	mTimer_GetSpeed(&aSpeedMotLeft, &aSpeedMotRight);
	aSpeedMotLeft/=VITESSE_MAX;//VITESSE_MAX_MOT_GAUCHE;
	aSpeedMotRight/=VITESSE_MAX;//VITESSE_MAX_MOT_DROITE;
	//Epsilon
	fEcartLeft=lfSpeedLeft-aSpeedMotLeft;
	fEcartRight=lfSpeedRight-aSpeedMotRight;

	//Epsilon sur KP
	fEpsilonLeft=(fEcartLeft/(float)KP_MOT);
	fEpsilonRight=(fEcartRight/(float)KP_MOT);

	fErreurRight+=fEpsilonRight;
	fErreurLeft+=fEcartLeft;

	//(Consigne-Epsilon sur KP)/VITESSE_MAX
	aSpeedMotLeft=(lfSpeedLeft-fEpsilonLeft) + (0.2*fErreurLeft);
	aSpeedMotRight=(lfSpeedRight-fEpsilonRight) + (0.2*fErreurRight);
	if(lfSpeedLeft<-0.1 && lfSpeedRight<-0.1)
		mTimer_SetMotorDuty(lfSpeedLeft , lfSpeedRight);
	else
		mTimer_SetMotorDuty(aSpeedMotLeft , aSpeedMotRight);*/
	//mTimer_SetMotorDuty(1 , 1);
}

float fPotToSpeed(float fADValue)
{
	float fValue=0;
	fADValue+=1;//varie maintenant de 0 à 2
	fValue=(fADValue/2.0);
	return fValue;
}

float fPotToGain(float fADValue)
{
	fADValue+=1;//0 to 2
	fADValue=(fADValue*2);//0 à 4 le Kp
	return fADValue;
}

UInt32 DetectionLigne_evitement(UInt8* aUItableau, UInt8 UIseuil, UInt16 uiRange, Int16* IEcartGauche, Int16* IEcartDroite)
{
	static UInt16 uLastPixelBord[2]={0,0};
	UInt16 Pixel_Bord[2]={0,0};
	static UInt8 u8LargeurPiste =0;
	static UInt8 Pixel_milieux = 0;
	UInt32 cLines = 0;
	Int16 EcartErreur = ((Int16)(143/2)-(Int16)PIXEL_MILIEUX);

	for (int i = 0; i < (uiRange/2); i++)//verification lignes sur la range donnee
	{
		if (i>5 && aUItableau[i + PIXEL_MILIEUX]<UIseuil && Pixel_Bord[0]==0)//pixel bord droite, virage gauche
		{
			cLines = cLines | 1;
			Pixel_Bord[0] = i + PIXEL_MILIEUX;
			aUItableau[i + PIXEL_MILIEUX]=0;
		}
		if (i>5 && aUItableau[PIXEL_MILIEUX-i] < UIseuil && Pixel_Bord[1]==0 && (i!=20))//pixel bord gauche, virage droite pixel 52 est mort
		{
			cLines = cLines | 2;
			Pixel_Bord[1] = PIXEL_MILIEUX-i;
			aUItableau[PIXEL_MILIEUX-i]=0;
		}

		//traitement image monitoring
		if(aUItableau[PIXEL_MILIEUX-i] > UIseuil)
		{
			aUItableau[PIXEL_MILIEUX-i]=255;
		}
		if(aUItableau[PIXEL_MILIEUX+i] > UIseuil)
		{
			aUItableau[PIXEL_MILIEUX+i]=255;
		}
		//ligne milieu
		/*if(i<RANGE_MILIEUX && (aUItableau[PIXEL_MILIEUX+i] < UIseuil || aUItableau[PIXEL_MILIEUX-i] < UIseuil))
		{
			cLines = cLines | 4;
		}*/
	}

	if((cLines&0x3) == 3) //deux lignes
	{
		u8LargeurPiste = 95;
		Pixel_milieux = Pixel_Bord[0] - (u8LargeurPiste/2)  - EcartErreur;
	}
	else if((cLines&0x7)==1) //droite
	{
		Pixel_milieux = Pixel_Bord[0] - (u8LargeurPiste/2)  - EcartErreur;
	}
	else if((cLines&0x7)==2) //gauche
	{
		Pixel_milieux = Pixel_Bord[1] + (u8LargeurPiste/2)  - EcartErreur;
	}
	else if((cLines&0x7)==6 && Pixel_milieux!=0)//extremite gauche
	{
		Pixel_milieux = 120;
	}
	else if((cLines&0x7)==5&& Pixel_milieux!=140) //extremite droite
	{
		Pixel_milieux = 20;
	}
	cLines |= ((UInt16)Pixel_milieux<<8);

	//obstacle ?
	if(Pixel_Bord[0]==0 && uLastPixelBord[0] < 120  && uLastPixelBord[0] > 0)
	{
		*IEcartDroite=1;
	}
	else
	{
		*IEcartDroite=0;
	}
	if(Pixel_Bord[1]==0 && uLastPixelBord[1] > 25)
	{
		*IEcartGauche=1;
	}
	else
	{
		*IEcartGauche=0;
	}

	uLastPixelBord[0]=Pixel_Bord[0];
	uLastPixelBord[1]=Pixel_Bord[1];
	return cLines;
	//return Pixel_milieux in 8 LSL
	//return 0x8 if obstacle right, 0x16 if obstacle left
	//return 0x00 if no lines, 0x01 if line at the right, 0x02 if lines at the left, 0x03 if two lines, 0x04 if center line
}

double PID_evitement (UInt8 u8Consigne, double lfMesure, float u16KP)
{
	if(lfMesure<0)
	{
		lfMesure=0;
	}
	double lfConsigne = u8Consigne;
	double lfErreur=0;
	// Paramètres du PID
	double Kp = u16KP/143.0;

	lfErreur = (double)lfConsigne - lfMesure;										// Calcul de l'erreur
	lfConsigne = (Kp*lfErreur);	// Régulation P
	return lfConsigne;
}

UInt16 vCircuit_evitement(UInt8 u8Milieux,UInt8 u8Lines, Int16 u16BordGaucheDiff, Int16 u16BordDroiteDiff, void* pSC)
{
	struct Compute_evitement* pCompute = pSC;
	static enum State_evitement eState = init;
	static UInt8 u8LeftRight=0;
	static UInt32 u32Ticks = 0;
	if(mSwitch_ReadPushBut(kPushButSW1)==true)//verification si arret urgence
	{
		eState=init;
	}
	switch (eState)
	{
	case init_evitement :
		pCompute->MotDroite=0;
		pCompute->MotGauche=0;
		pCompute->lfServo=PID(u8Milieux, 0, GAIN_KP_DROIT);
		if(mSwitch_ReadPushBut(kPushButSW2)==true)//impossible de demarrer si pas de lignes ou une ligne au milieu
		{
			eState = suivi;
		}
		break;
	case suivi :
		mLeds_Write(kMaskLed4,0);
		if(u32Ticks>0)
		{
			u32Ticks++;
		}
		pCompute->MotDroite=VITESSE_EVT;
		pCompute->MotGauche=VITESSE_EVT;

		pCompute->lfServo=PID(u8Milieux, 0, GAIN_KP_DROIT);

		if((u16BordGaucheDiff==1||(u8LeftRight==2&&u8Lines==0)) && (u32Ticks > 1000 || u32Ticks==0))
		{
			u32Ticks=0;
			eState = evit_gauche;
			mLeds_Write(kMaskLed4,1); //led milieux
		}
		if((u16BordDroiteDiff==1||(u8LeftRight==1&&u8Lines==0)) && (u32Ticks > 1000 || u32Ticks==0))
		{
			u32Ticks=0;
			eState = evit_droite;
			mLeds_Write(kMaskLed4,1); //led milieux
		}
		break;

	case evit_droite:
			u32Ticks++;
			pCompute->MotDroite=VITESSE_EVT;
			pCompute->MotGauche=VITESSE_EVT;
			pCompute->lfServo=PID_evitement(90, u8Milieux, GAIN_KP_DROIT);
			if(u32Ticks>100)
			{
				eState=suivi;
			}
		break;
	case evit_gauche:
			u32Ticks++;
			pCompute->MotDroite=VITESSE_EVT;
			pCompute->MotGauche=VITESSE_EVT;
			pCompute->lfServo=PID_evitement(50,u8Milieux, GAIN_KP_DROIT);
			if(u32Ticks>100)
			{
				eState=suivi;
			}
		break;
	}
	if(u8Lines==1)
	{
		u8LeftRight=1;//droite
	}
	else if(u8Lines==2)
	{
		u8LeftRight=2;//gauche
	}
	else
	{
		u8LeftRight=0;
	}

	return (UInt16)eState;
}

global structs header

//Alaric Clot Lorenzin
#ifndef GLOBAL_H
#define GLOBAL_H

#include "fonctions.h"
#include "gCompute.h"
#include "gInput.h"
#include "gOutput.h"

struct Input
{
	bool bImageOk;
	UInt16 au16Tab[200];
	UInt8 u8PixelValMoy;
	float fPot1Value;
	float fPot2Value;
};

struct Compute
{
	double lfServo;
	UInt8 au8Compense[200];
	UInt16 u8ReturnLines;
	UInt8 u8Flag;
	UInt8 u8PixelMilieux;
	UInt8 u8Virage;
	UInt8 u8BordDiff;
	float fSpeedValue;
	float fKp;
};

struct Compute_evitement
{
	double lfServo;
	float MotGauche;
	float MotDroite;
	UInt16 au8Compense[200];
	UInt16 u16ReturnLines;
};


struct Output
{
	UInt16 u16State;
};

extern struct Input sInput;
extern struct Output sOutput;
extern struct Compute sCompute;
extern struct Compute_evitement sCompute_evitement;
#endif //GLOBAL_H

function_header

/*
 * fonctions.h
 *
 *  Created on: 19 oct. 2020
 *      Author: Alaric Clot Lorenzin
 */

#ifndef FONCTIONS_H_
#define FONCTIONS_H_

#include "MK64F12.h"
#include "mSpi.h"
#include "mDac.h"
#include "mAccelMagneto.h"
#include "mGyro.h"
#include "mWifi.h"
#include "mTimer.h"
#include "mCpu.h"
#include "mSwitch.h"
#include "mLeds.h"
#include "mAd.h"
#include "mDelay.h"
#include "mRs232.h"
#include "mVL6180x.h"
#include "stdio.h"
#include "string.h"
#include <stdlib.h>
#include "stbp.h"
//#include "gInput.h"
//#include "gCompute.h"
//#include "gOutput.h"
#include "gMonitoring.h"
#include "global.h"

#define PIXEL_MILIEUX (72)//72//71//78
#define CORRECTION_SERVO (0)
#define RANGE (110)
#define RANGE_MILIEUX (10)//10

#define BUTEE_SERVO (0.9)//0.74 optimal

#define GAIN_KP_DROIT (2.8)//3 3/143 //2.8
//#define GAIN_KP_VIRAGE (3)//2.8

#define GAIN_KD (0.001)//0.01 //20
#define GAIN_KI (0.00005)//0.0001 //0.00005 0.000001

#define GAIN_KP_MOTEUR (0.0004)
#define GAIN_KD_MOTEUR (10)

#define VITESSE (4000) //0.65 pas mal
#define VITESSE_EVT (0.45) //0.65 pas mal
#define DROITE (1)
#define GAUCHE (2)

#define PIXEL_MAX_GAUCHE (100)//105//107 //105//111
#define PIXEL_MAX_DROITE (38)//35//47//35  //29 optimal //32

#define DIFFERENTIEL_MOTEUR (0.7)//0.7 optimal

#define STOP_TICKS (30)
#define TICKS_FREINAGE (100)
#define BRAKE (15)
#define FREINAGE (1500)//900

#define KP_MOT (50)//50
#define VITESSE_MAX_MOT_GAUCHE (5500)
#define VITESSE_MAX_MOT_DROITE (6900)
#define VITESSE_MAX (6500)
//enums
enum State
{
	init=0,
	vitesse_constante,
	virage_gauche,
	virage_droite,
	deceleration,
	freinage_gauche,
	freinage_droite
};

enum State_evitement
{
	init_evitement=0,
	suivi,
	evit_gauche,
	evit_droite
};


//declarations fonctions
double fFcx(UInt16 uiX);
//fonction de puissance recursive
double fPow(double fX, UInt16 uiPow);
//fonction de compensation avec polynome ordre 4
void vCompensation(const UInt16* const aUItableau, UInt8* aUIresultat, UInt8* cMoy);
//detection de lignes gauche droite milieux
UInt32 DetectionLigne(UInt8* aUItableau, UInt8 UIseuil,UInt16 uiRange);
//output etat lignes sur leds
void vLedOutput(UInt8 cLED);
//machine a etat fonctionnement
UInt16 vCircuit(float fVitesseAttendue, UInt8 ui8Stop, UInt8 u8Milieux, UInt8 u8EmergencyStop, UInt8 u8Lines);
//fonction pilotant le servo
void vControleServo(double lfValue);
//PID avec mesure lf et uReinit pour reinitialsier le I
double PID (double lf,UInt8 uReinit, float u16KP);
double PID_evitement (UInt8 u8Consigne, double lfMesure, float u16KP);
//detection virage (pas utilisee)
UInt8 u8DetectionVirage(UInt8 u8Lignes);
//vDetectObstacle, detection obstacle
float fDetectObstacle(UInt8 u8Milieux, UInt8 u8Lines, double lfServo);
//Regul Mot
void vSetMotorSpeed(double lfSpeedLeft, double lfSpeedRight);
//
float fPotToGain(float fADValue);

//evitement code
UInt16 vCircuit_evitement(UInt8 u8Milieux,UInt8 u8Lines, Int16 u16BordGaucheDiff, Int16 u16BordDroiteDiff, void* pSC);
double PID_evitement (UInt8 u8Consigne, double lfMesure, float u16KP);
UInt32 DetectionLigne_evitement(UInt8* aUItableau, UInt8 UIseuil, UInt16 uiRange, Int16* IEcartGauche, Int16* IEcartDroite);


float fPotToSpeed(float fADValue);
#endif /* FONCTIONS_H_ */

ginput_header

/*
 ------------------------------------------------------------
 Copyright 2003-20xx Haute �cole ARC Ing�ni�rie, Switzerland.
 All rights reserved.
 ------------------------------------------------------------
 Nom du fichier : 	gInput.h
 Author and date :	Alaric Clot Lorenzin 2020

 But : Gestionnaire d'entr�e

 Modifications
 Date		Faite	Ctrl		Description
 ------------------------------------------------------------
 */

#ifndef __GINPUT__
#define  __GINPUT__
#include "global.h"
//-----------------------------------------------------------------------------
// Configuration du gestionnaire
//-----------------------------------------------------------------------------
void gInput_Setup(void);

//-----------------------------------------------------------------------------
// Ex�cution du gestionnaire
//-----------------------------------------------------------------------------
void gInput_Execute(void);

#endif /*  __GINPUT__ */

main

main code with 3 mode, test, race and obstacle

/*
------------------------------------------------------------
Copyright 2003-201x Haute ecole ARC Ingenierie, Switzerland.
All rights reserved.
------------------------------------------------------------
File name :	main.c
Author and date :	Alaric Clot Lorenzin 2021

Description in the header (.h)

-----------------------------------------------------------------------------
History:
-----------------------------------------------------------------------------

$History: $

-----------------------------------------------------------------------------
 */

#include "fonctions.h"
#include "global.h"
#define K_MAIN_INTERVAL	(100 / kPit1Period)

#define NBDEPOINT (144)
#define AJUSTEMENT (10)


// Mesure de vitesse et de sens
static float sSpeedMotLeft;
static float sSpeedMotRight;
#define kSpeedTabSize 100
static UInt16 sSpeedIndex;
static float sSpeedTab[kSpeedTabSize][2];


// Mesure de l'acc�l�rom�tre et du magn�tom�tre
//static SRAWDATAEnum sAccel;				// en g
//static SRAWDATAEnum sMagneto;			// en micro teslas
//static UInt8 sAccelMagnetoStatus;
//static float sYaw;		// en degr�s
//static float sRoll;		// en degr�s
//static float sPitch;	// en degr�s

// Vitesse angulaire du gyro en degr�/s
//static float sAngVel_X;
//static float sAngVel_Y;
//static float sAngVel_Z;

// Mesure des courant moteurs et de la tension batterie
static float sIMotLeft;
static float sIMotRight;
static float sUBatt;
static bool  sFaultLeft;
static bool  sFaultRight;

// Distance mesur�e par le LIDAR en mm
//static UInt8 sDistFront;



//-------------------------------------------------------------------------
// Programme principal
//-------------------------------------------------------------------------
int main(void)
{
	static Int16 sDly;
	Int32 aWakeIntMain;
	/*Int8 aCharTab[50];
 	UInt8 aImageTab[13];*/
	float aDuty=0;
	// Tableau contenant l'image (et diverses infos) de la camera digitale
	//UInt8 aImageTab[200];
	// Sensor value
	SRAWDATAEnum aAccel;	// en g
	SRAWDATAEnum aMagneto;	// en micro teslas
	float aYaw=0;		// en degres
	float aRoll=0;	// en degres
	float aPitch=0;	// en degres*/
	// Mesure de vitesse et de sens
	float aSpeedMotLeft=0;
	float aSpeedMotRight=0;
	// TCP connection frame content
    Frame1RxContent* aRxWifiFrame = gWifi_getFrameRx();
	Frame1TxContent* aTxWifiFrame = gWifi_getFrameTx();

	// Since we use those variable before receiving them this is default values.
	aRxWifiFrame->DutyMotorLeft = 0.0;
	aRxWifiFrame->DutyMotorRight = 0.0;
	aRxWifiFrame->ExpoCamera = 1.0;
	aRxWifiFrame->LedOnOff = 0;
	aRxWifiFrame->PosServo = 0.0;

#if (kWithLidar)
	UInt8 aDistFront;
#endif

	//--------------------------------------------------------------------
	// Setup des peripheriques et de la carte
	//--------------------------------------------------------------------
	// Config de la PLL --> CPU 100MHz, bus et periph 50MH z
	mCpu_Setup();

	// Config et start des interrupteurs et poussoirs
	mSwitch_Setup();
	mSwitch_Open();

	// Config et start des Leds
	mLeds_Setup();
	mLeds_Open();

	// Config et start de l'ADC
	mAd_Setup();
	mAd_Open();

	// Config et start du SPI
	mSpi_Setup();
	mSpi_Open();

	// Config et start des delay non bloquant par PIT
	mDelay_Setup();
	mDelay_Open();

	// Config des timer pour la mesure de vitesse et les sorties PWM pour les servos
	mTimer_Setup();
	mTimer_Open();

	// Setup FXOS8700CQ 
	mAccelMagneto_Setup();
	mAccelMagneto_Open();

	// Setup FXAS21002C
	mGyro_Setup();
	mGyro_Open();

	// Config et start du DAC0 utilise pour piloter le driver LED eclairage
	mDac_Setup();
	mDac_Open();

	// Setup et start de la commandes des PWMs moteur et servo ainsi que la mesure de vitesse
	mTimer_Setup();
	mTimer_Open();

	// Enable des IRQ au niveau du CPU -> Primask
	__enable_irq();

	// UART 3 module configuration et start --> wifi autobaud
	mWifi_Setup();
	mWifi_Open();

	// UART 4 monitoring image
	mRs232_Setup();
	mRs232_Open();

	// Wifi setup
	gMonitoring_Setup();

	// Lidar --> I2C
#if (kWithLidar)
	mVL6180x_Setup();
	mVL6180x_Open();
	mVL6180x_StartRange();
#endif

	// SPI0 --> camera SPI --> reset + idle command
	mSpi_MLX75306_Reset();

	//--------------------------------------------------------------------
	// Init et calibration de l'accelerometer et du magnetometer
	//--------------------------------------------------------------------
#if(kWithAccel)

	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
	// !!! La calibration prend presque 1mn et il faut tourner la voiture
	// dans tout les sens pendant ce temps d'init
	// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

	// Init du FXOS8700CQ
	mAccelMagneto_Init();
	// Calibration de l'offset de l'accelerometer
	mAccelMagneto_Accel_Calibration();
	// Calibration de l'offset du magnetometer
	mAccelMagneto_Mag_Calibration();

	// Init du FXAS21002C
	mGyro_Init();
	// Calibration de l'offset du gyroscope
	mGyro_Calibration();
#endif

	aWakeIntMain = mDelay_GetDelay(kPit1, 10/kPit1Period);//10
	 gOutput_Setup();
	 gInput_Setup();
	//--------------------------------------------------------------------
	// Boucle infinie --> round robin
	//--------------------------------------------------------------------
	for(;;)
		{
			//sDistFront=sAngVel_X;

			//--------------------------------------------------------------------
			// Selon la position de l'interrupteur 1 on passe de l'�tat test (si =0, voir ci-dessous) � l'�tat automatique
			//--------------------------------------------------------------------
			// Mode automatique avec wifi
			if(mSwitch_ReadSwitch(kSw1)==true)
				{
					gInput_Execute();
					gCompute_Execute();
					gOutput_Execute();
					//gMonitoring_ExecuteNoBlock();
				}
			//--------------------------------------------------------------------
			// Mode test
			//--------------------------------------------------------------------
			else if(mSwitch_ReadSwitch(kSw4)==true)
			{
				gInput_Execute();
				gCompute_Execute_evitement();
				gOutput_Execute_evitement();
			}
			else
				{
					vCircuit(sCompute.fSpeedValue, sCompute.u8Flag, sCompute.u8PixelMilieux, 1, sCompute.u8ReturnLines);
					mTimer_SetMotorDuty(0.0 ,0.0);
					mTimer_SetServoDuty(0,0);//output sur servo
					// Lecture de l'acc�l�rom�tre et du magn�tom�tre
					// Angle en degr�s et acc�l�ration en g
#if(kWithAccel)
					aRet=mAccelMagneto_ReadData(&sAccel,&sMagneto,&sYaw,&sRoll,&sPitch);
					aRet=mGyro_ReadData_mDPS(&sAngVel_X,&sAngVel_Y,&sAngVel_Z);
#endif

#if (kWithLidar)

					//--------------------------------------------------------------------
					// Lecture de la distance mesur�e par le LIDAR (mm)
					//--------------------------------------------------------------------
					if(mVL6180x_GetRange(&sDistFront)==true)
						{
//							sprintf(aCharTab,"%d",sDistFront);
//							mRs232_Uart4WriteString(aCharTab);
//							mRs232_Uart4WriteChar('\r');
//							mRs232_Uart4WriteChar('\n');
						}

#endif
					//-----------------------------------------------------------------------------
					// Lecture de la vitesse de rotation des moteurs
					// Moteur A = moteur gauche (tr/mn)--> valeur n�gative = en arri�re, valeur pos=en avant
					// Moteur B = moteur droite (tr/mn)
					//-----------------------------------------------------------------------------

					mTimer_GetSpeed(&sSpeedMotLeft,&sSpeedMotRight);
					sSpeedTab[sSpeedIndex][0]=sSpeedMotLeft;
					sSpeedTab[sSpeedIndex][1]=sSpeedMotRight;
					sSpeedIndex++;
					sSpeedIndex%=kSpeedTabSize;

					//allumer leds
					mDac_SetDac0Output(1);
					// Selon la position des interrupteurs (interrupteur 2 et 3) on teste les poussoirs, le servo, les moteurs DC et la camera
					if(mDelay_IsDelayDone(kPit1,sDly)==true)
						{
							mDelay_ReStart(kPit1,sDly,300/kPit1Period);

							// Test des LEDS
							mLeds_Toggle(kMaskLed1+kMaskLed2+kMaskLed3+kMaskLed4);

							// Tests des servo
							// Les 2 boutons poussoirs permettent de bouger dans un sens et dans l'autre

						// Lecture du courant du moteur gauche et droite et de la tension batterie
						sIMotLeft=mAd_Read(kIHBridgeLeft);
						sIMotRight=mAd_Read(kIHBridgeRight);
						sUBatt=mAd_Read(kUBatt);
						sFaultLeft=mTimer_GetFaultMoteurLeft();
						sFaultRight=mTimer_GetFaultMoteurRight();
						}
				}
		}
}

gcompute_header

/*
 * gCompute.h
 *
 *  Created on: 2020
 *      Author: Alaric Clot Lorenzin
 */

#ifndef __GCOMPUTE__
#define __GCOMPUTE__
#include "global.h"
//-----------------------------------------------------------------------------
// Configuration du gestionnaire
//-----------------------------------------------------------------------------
void gCompute_Setup(void);

//-----------------------------------------------------------------------------
// Ex�cution du gestionnaire
//-----------------------------------------------------------------------------
void gCompute_Execute(void);
void gCompute_Execute_evitement(void);

#endif /* __GCOMPUTE__ */

Shop

Project Hub

Video

Blog

Shop home

3D printing

Single Board Computers

Development kits

Sensors

Internet of things

Robotics

Accessories

Product of the Week

Electromaker Educator

The Electromaker Show

The Electromaker Podcast

Arcar5 Project P3

About the project

Project info

Items used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Code

ginput

gcompute

goutput_header

goutput

global structs

fonctions

global structs header

function_header

ginput_header

main

gcompute_header

Credits

Julien_ALARIC

Leave your feedback...

	C programmed round robin software on ARM M4
	LabView programm for camera monitoring
	Wifi monitoring in Java

Shop

Project Hub

Video

Blog

Shop home

3D printing

Single Board Computers

Development kits

Sensors

Internet of things

Robotics

Accessories

Product of the Week

Electromaker Educator

The Electromaker Show

The Electromaker Podcast

Arcar5 Project P3

About the project

Project info

Items used in this project

Hardware components

Software apps and online services

Hand tools and fabrication machines

Story

Code

ginput

gcompute

goutput_header

goutput

global structs

fonctions

global structs header

function_header

ginput_header

main

gcompute_header

Credits

Julien_ALARIC

Leave your feedback...

Related products