Team DIANA / Mbed 2 deprecated IPKF_Test_nucleo

test_IPKF

Dependencies:   mbed

source/pathPlanning.c@0:fb6e494a7656, 2016-04-20 (annotated)

Committer:
LudovicoDani
Date:
Wed Apr 20 10:03:58 2016 +0000
Revision:
0:fb6e494a7656
IPKF_Test_nucleo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LudovicoDani 0:fb6e494a7656 1 #include "pathPlanning.h"
LudovicoDani 0:fb6e494a7656 2
LudovicoDani 0:fb6e494a7656 3 void GeneratePath(matrix* p0, matrix* pf, matrix* path)
LudovicoDani 0:fb6e494a7656 4 {
LudovicoDani 0:fb6e494a7656 5 float delta_x = (pf->element[0][0] - p0->element[0][0]) / path->col;
LudovicoDani 0:fb6e494a7656 6 float delta_y = (pf->element[1][0] - p0->element[1][0]) / path->col;
LudovicoDani 0:fb6e494a7656 7 float delta_z = (pf->element[2][0] - p0->element[2][0]) / path->col;
LudovicoDani 0:fb6e494a7656 8 float delta_theta_x = (pf->element[3][0] - p0->element[3][0]) / path->col;
LudovicoDani 0:fb6e494a7656 9 float delta_theta_y = (pf->element[4][0] - p0->element[4][0]) / path->col;
LudovicoDani 0:fb6e494a7656 10 float delta_theta_z = (pf->element[5][0] - p0->element[5][0]) / path->col;
LudovicoDani 0:fb6e494a7656 11
LudovicoDani 0:fb6e494a7656 12 for(int i = 0; i < path->col; i++)
LudovicoDani 0:fb6e494a7656 13 {
LudovicoDani 0:fb6e494a7656 14 path->element[0][i] = p0->element[0][0] + i*delta_x;
LudovicoDani 0:fb6e494a7656 15 path->element[1][i] = p0->element[1][0] + i*delta_y;
LudovicoDani 0:fb6e494a7656 16 path->element[2][i] = p0->element[2][0] + i*delta_z;
LudovicoDani 0:fb6e494a7656 17 path->element[3][i] = p0->element[3][0] + i*delta_theta_x;
LudovicoDani 0:fb6e494a7656 18 path->element[4][i] = p0->element[4][0] + i*delta_theta_y;
LudovicoDani 0:fb6e494a7656 19 path->element[5][i] = p0->element[5][0] + i*delta_theta_z;
LudovicoDani 0:fb6e494a7656 20
LudovicoDani 0:fb6e494a7656 21 }
LudovicoDani 0:fb6e494a7656 22
LudovicoDani 0:fb6e494a7656 23 return;
LudovicoDani 0:fb6e494a7656 24 }