SpeedTest

#include <stdio.h>
#include <math.h>
#include <time.h>
#include "tick.h"
#include <wiringPi.h>
#include "BrickPi.h"
#include <linux/i2c-dev.h>
#include <fcntl.h>





long i=0;

long posdactual=0;
long posiactual=0;
float posdractual=0.0;
float posdranterior=0.0;
float posiractual=0.0;
float posiranterior=0.0;
float veldr=0.0;
float velir=0.0;

float pul2rad=0.0174533;
int main ()
{
	ClearTick();
	BrickPiSetup();
	BrickPi.Address[0] = 1;
	BrickPi.Address[1] = 2;
	BrickPi.MotorEnable[PORT_A] = 1;	//Enable the motors
	BrickPi.MotorEnable[PORT_D] = 1;
	BrickPiSetupSensors();		//Set up the properties of sensors for the BrickPi
	//printf("BrickPiSetupSensors: %d\n", result);

	BrickPi.EncoderOffset[PORT_A]=BrickPi.Encoder[PORT_A];
	BrickPi.EncoderOffset[PORT_D]=BrickPi.Encoder[PORT_D];
	FILE *fichero;
	fichero = fopen("registro1.txt","a+");


		while(i<100)
		{
			BrickPiUpdateValues();
			//obtenemos la posicion de las ruedas en pulsos
			posdactual=BrickPi.Encoder[PORT_D];
			posiactual=BrickPi.Encoder[PORT_A];

			//calculamos la posicion de las ruedas en radianes
			posdractual=pul2rad*posdactual;
			posiractual=pul2rad*posiactual;

			//calculamos la velocidad de las ruedas en Radianes/S
			veldr=(posdractual-posdranterior)/0.05;
			velir=(posiractual-posiranterior)/0.05;

			//dar control a los motores
			BrickPi.MotorSpeed[PORT_A] = 255;
			BrickPi.MotorSpeed[PORT_D] = 255;

			//almacenamos los datos en el fichero de texto
			fprintf(fichero, " %f",posdractual);
			fprintf(fichero, " %f",posiractual);
			fprintf(fichero, " %f",veldr);
			fprintf(fichero, " %f\n",velir);

			//almacenamos la posicion de las ruedas para poder calcular
			//la velocidad en la iteracion siguiente
			posdranterior=posdractual;
			posiranterior=posiractual;



			//verificamos el periodo de muestreo e incrementamos el valor del contador
			usleep(50000);
			i++;

		}
	fclose;
}