/*
 * odometry.c
 *
 *  Created on: 5. 4. 2017
 *      Author: Steave
 */

#include "stm32f4xx.h"
#include "stm32f4_discovery.h"
#include "main.h"
#include "odometry.h"
#include "math.h"


void InitPosStruct(struct position *pos)
{
	pos->wheelConstR=(M_PI*WHEEL_DR*0.9966)/TICKS_PER_REV_R;
	pos->wheelConstL=(M_PI*WHEEL_DL*0.9966)/TICKS_PER_REV_L;
	pos->angelDif = 0;
	pos->x = 0;
	pos->y = 0;
	pos->angel = 0;
}


void TikToPosition(struct position *pos)
{
	pos->pathLeft = pos->ticksLeft*pos->wheelConstL;
	pos->pathRight = pos->ticksRight*pos->wheelConstR;
	pos->pathCenter = (pos->pathRight+pos->pathLeft)/2;
	pos->angelDif = (pos->pathLeft-pos->pathRight)/(SHAFT);
	pos->angelDifHalf = pos->angelDif/2;
	pos->arg = pos->angel	 + pos->angelDifHalf;
	if (pos->angelDif != 0)
	{
		pos->chordLenght = (pos->pathCenter/pos->angelDifHalf)*sin(pos->angelDifHalf);
		pos->x = pos->x + pos->chordLenght*cos(pos->arg);
		pos->y = pos->y - pos->chordLenght*sin(pos->arg);
		pos->angel = pos->angel+pos->angelDif;
	}
	else
	{
		pos->x = pos->x + pos->pathCenter*cos(pos->angelOld);
		pos->y = pos->y + pos->pathCenter*sin(pos->angelOld);
	}

	if (pos->angel > 2*M_PI || pos->angel < -2*M_PI)
		pos->angel = fmod(pos->angel,2*M_PI);
	if (pos->angel < 0)
		pos->angel += 2*M_PI;

	pos->angelDeg = pos->angel*180/M_PI;

}


