Submitted version of the thesis - Airlab, the Artificial Intelligence ...

B.1. Microprocessor Code 111
#include "math.h"
#include "Misc.h"
/* Include of other module interface headers ---------------------------------*/
/* Local includes ------------------------------------------------------------*/
/* Private typedef -----------------------------------------------------------*/
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
/* Private function prototypes -----------------------------------------------*/
void Triskar(s16* destination, s8 omega, s16* vt);
void Direct_Motion(s16* destination, s8 angle, s16* speed);
/* Private functions ---------------------------------------------------------*/
void Triskar(s16* destination, s8 omega, s16* vt) {
s16 frontal_speed = destination[0];
s16 lateral_speed = destination[1];
double alpha = 0;
s16 velocity[2];
velocity[0] = frontal_speed * cos(-alpha)
- lateral_speed * sin(-alpha);
velocity[1] = frontal_speed * sin(-alpha)
+ lateral_speed * cos(-alpha);
#ifdef NO_DEBUG
USB_WriteString("Calculating Motion \n");
Print_Int(velocity[0],"frontal");
Print_Int(velocity[1],"lateral");
#endif
u8 R_robot = 250;
s16 v_F = velocity[0];
s16 v_L = velocity[1];
double d_cosA = 0.8660;
double d_sinA = 0.5000;
u8 cosA = d_cosA * 100;
u8 sinA = d_sinA * 100;
s16 v[3] = { v_F, v_L, (omega * R_robot) / 100 };
#ifdef NO_DEBUG
//print velocity vector
Print_Int(v[0],"v0");
Print_Int(v[1],"v1");
Print_Int(v[2],"v2");
#endif
u8 k, l;
s16 MF[3][3] = { { -cosA, sinA, -100 }, { cosA, sinA,
-100 }, { 0, -100, -100 } };