/***********************************************************************//**
 * @file		kinematic.h
 * @brief		Contains functions for inverse kinematic calculations
 * @version		1.0
 * @date		26. February. 2014
 * @author		Michal Semrad
 **************************************************************************/

#ifndef KINEMATIC_H
#define KINEMATIC_H

#include "pwm.h"
#include "LPC17xx.h"
#include "aux_math_lib.h"
#include "arm_math.h"

#include "SerialCOM.h"

#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "queue.h"

#define FRAC2DEG(x) (x*180/0x4000)
#define DEG2FRAC(x) (x*0x4000/180)

#define LENGTHARM0 70
#define LENGTHARM1 18
#define LENGTHARM2 145
#define LENGTHARM3 186
#define LENGTHARM4 100
#define LENGTHCRITICAL 116

int InverseKinematic();
void ConvertCoordinatesKto3(int* x3,int* y3,int* z3,int* angle_q0,int* angle_q0_previous);
int CheckCoordinates(int x3,int y3,int z3, int* l);

int q0(int angle_q0);
int q2(int l,int* angle_gamma,int* angle_q2);
int q1(int x3,int y3,int z3,int l,int angle_gamma,int* angle_q1);
int q3(int angle_beta_deg,int angle_q1,int angle_q2);
int q4(int angle_alpha_deg);
int q5(int grip_mm);

void DirectKinematic();
void AnglesToPWM();


#endif /* KINEMATIC_H */
