monde.c

Basic PID to drive two motors.

/*
 * Monde.c
 *
 *  Created on: 15 f関r. 2009
 *      Author: Adrien
 */

#include "Monde.h"

void initialiser_monde(monde* m) {

	// Distributeurs palets
	m->distributeurs_palets[0].couleur = ROUGE;
	m->distributeurs_palets[0].nombre_palets = NOMBRE_PALETS_PAR_DISTRIBUTEUR;
	m->distributeurs_palets[0].position.x = 0;
	m->distributeurs_palets[0].position.y = 1300;

	m->distributeurs_palets[1].couleur = ROUGE;
	m->distributeurs_palets[1].nombre_palets = NOMBRE_PALETS_PAR_DISTRIBUTEUR;
	m->distributeurs_palets[1].position.x = 289;
	m->distributeurs_palets[1].position.y = 0;

	m->distributeurs_palets[2].couleur = VERT;
	m->distributeurs_palets[2].nombre_palets = NOMBRE_PALETS_PAR_DISTRIBUTEUR;
	m->distributeurs_palets[2].position.x = MONDE_LONGUEUR - 289;
	m->distributeurs_palets[2].position.y = 0;

	m->distributeurs_palets[3].couleur = VERT;
	m->distributeurs_palets[3].nombre_palets = NOMBRE_PALETS_PAR_DISTRIBUTEUR;
	m->distributeurs_palets[3].position.x = MONDE_LONGUEUR;
	m->distributeurs_palets[3].position.y = 1300;

	//distributeurs poutres
	m->distributeurs_poutres[0].couleur = ROUGE;
	m->distributeurs_poutres[0].nombre = NOMBRE_POUTRES_PAR_DISTRIBUTEUR;
	m->distributeurs_poutres[0].position.x = 900;
	m->distributeurs_poutres[0].position.y = MONDE_LARGEUR;

	m->distributeurs_poutres[1].couleur = ROUGE;
	m->distributeurs_poutres[1].nombre = NOMBRE_POUTRES_PAR_DISTRIBUTEUR;
	m->distributeurs_poutres[1].position.x = 1300;
	m->distributeurs_poutres[1].position.y = MONDE_LARGEUR;

	m->distributeurs_poutres[2].couleur = VERT;
	m->distributeurs_poutres[2].nombre = NOMBRE_POUTRES_PAR_DISTRIBUTEUR;
	m->distributeurs_poutres[2].position.x = 1700;
	m->distributeurs_poutres[2].position.y = MONDE_LARGEUR;

	m->distributeurs_poutres[3].couleur = VERT;
	m->distributeurs_poutres[3].nombre = NOMBRE_POUTRES_PAR_DISTRIBUTEUR;
	m->distributeurs_poutres[3].position.x = 2100;
	m->distributeurs_poutres[3].position.y = MONDE_LARGEUR;

	// Zone construction ronde
	m->zones_construction[0].type = MONDE_ZONE_CONSTRUCTION_RONDE;
	m->zones_construction[0].niveau = 2;
	m->zones_construction[0].centre.x = 1500;
	m->zones_construction[0].centre.y = 1050;
	m->zones_construction[0].rayon = 150;
	m->zones_construction[0].coin_inf_droit.x = 1650;
	m->zones_construction[0].coin_inf_droit.y = 900;
	m->zones_construction[0].coin_sup_gauche.x = 1350;
	m->zones_construction[0].coin_sup_gauche.y = 1200;

	//zone construction carre
	m->zones_construction[1].type = MONDE_ZONE_CONSTRUCTION_RECTANGULAIRE;
	m->zones_construction[1].niveau = 0;
	m->zones_construction[1].coin_inf_droit.x = MONDE_LONGUEUR - 578;
	m->zones_construction[1].coin_inf_droit.y = 0;
	m->zones_construction[1].coin_sup_gauche.x = MONDE_LONGUEUR - 1178;
	m->zones_construction[1].coin_sup_gauche.y = 100;

	m->zones_construction[2].type = MONDE_ZONE_CONSTRUCTION_RECTANGULAIRE;
	m->zones_construction[2].niveau = 1;
	m->zones_construction[2].coin_inf_droit.x = MONDE_LONGUEUR - 1178;
	m->zones_construction[2].coin_inf_droit.y = 0;
	m->zones_construction[2].coin_sup_gauche.x = 1178;
	m->zones_construction[2].coin_sup_gauche.y = 100;

	m->zones_construction[3].type = MONDE_ZONE_CONSTRUCTION_RECTANGULAIRE;
	m->zones_construction[3].niveau = 0;
	m->zones_construction[3].coin_inf_droit.x = 1178;
	m->zones_construction[3].coin_inf_droit.y = 0;
	m->zones_construction[3].coin_sup_gauche.x = 578;
	m->zones_construction[3].coin_sup_gauche.y = 100;
	// etc ...
	initialiser_zone(m);

}

void initialiser_zone(monde *m) {
	int i,j,k;
	int xsup,ysup,xinf,yinf;
	zone_construction z;
	// Initialisation du tableau de zone
	for (i=0;i<ZONE_LONGUEUR;i++) {
		for (j=0;j<ZONE_LARGEUR;j++) {
			if (i!=0&&i!=ZONE_LONGUEUR-1&&j!=0&&j!=ZONE_LARGEUR-1) {
				m->zone[i][j] = ZONE_NON_CALCULEE;
			} else {
				m->zone[i][j] = ZONE_BORD;
			}
		}
	}

	// Remplissage du tableau de zone avec les obstacles fixes
	for (i=0;i<NOMBRE_ZONE_CONSTRUCTION;i++) {

		z = m->zones_construction[i];
		xinf = (z.coin_sup_gauche.x -1)/ ZONE_LONGUEUR_CASE;
		xsup = (z.coin_inf_droit.x +1)/ ZONE_LONGUEUR_CASE;
		ysup = (z.coin_sup_gauche.y +1) / ZONE_LARGEUR_CASE;
		yinf = (z.coin_inf_droit.y -1) / ZONE_LARGEUR_CASE;


		for (j=xinf;j<=xsup;j++) {
			for (k=yinf;k<=ysup;k++) {
				m->zone[j][k] = ZONE_OBSTACLE_FIXE;
			}
		}
	}
}