void updateSpeeds(void) {
for(unsigned char i = 0; i < 4; ++i) {
Wheels[i]->Buffer[Wheels[i]->Time] = Wheels[i]->Counter;
Wheels[i]->Counter = 0;
if(Wheels[i]->Time == (BUFSIZE -1)) {
Wheels[i]->Speed = avgSpeed(Wheels[i]);
Wheels[i]->Acceleration = updateAcc(Wheels[i]);
Wheels[i]->Time = 0;
}
else {
Wheels[i]->Time++;
}
}
}
//no real calibration only set the actual position as default
void calibrateAcc(void)
{
int16_t tmp;
updateAcc();
for(uint8_t i=0; i<3; i++)
{
achse[i].offset = 0;
}
tmp = getAchseAcc(X_ACHSE);
achse[X_ACHSE].offset = tmp * (-1);
tmp = getAchseAcc(Y_ACHSE);
achse[Y_ACHSE].offset = tmp * (-1);
tmp = getAchseAcc(Z_ACHSE);
achse[Z_ACHSE].offset = (tmp - 341) * (-1);
}
void rungeKutta4( FLOAT **rm, FLOAT **vm, FLOAT **rM, FLOAT **vM, FLOAT *M, FLOAT dt, int n_masses, int n_Masses ){
// Crea los vectores de aceleracion, posicion y velodidad temporales
FLOAT **newRm;
FLOAT **newRM;
// Crea el vector k de constantes intermedias del método RungeKutta
FLOAT ***krm;
FLOAT ***krM;
FLOAT ***kvm;
FLOAT ***kvM;
// Aparta memoria
newRm = allocate2d( n_masses, n_dim );
newRM = allocate2d( n_Masses, n_dim );
krm = allocate3d( n_masses, n_dim, 4 );
krM = allocate3d( n_Masses, n_dim, 4 );
kvm = allocate3d( n_masses, n_dim, 4 );
kvM = allocate3d( n_Masses, n_dim, 4 );
//********************************************************************
//--------------------------------------------------------------------
// Cuerpo del RungeKutta
//--------------------------------------------------------------------
//********************************************************************
int i;
/*
* Rungekutta 1st step
*/
for( i = 0; i < n_Masses; i++ ){
// RungeKutta 1st step for n_Masses
krM[0][0][i] = vM[0][i];
krM[0][1][i] = vM[1][i];
}
for( i = 0; i < n_masses; i++ ){
// RungeKutta 1st step for n_masses
krm[0][0][i] = vm[0][i];
krm[0][1][i] = vm[1][i];
}
// Actualiza las aceleraciones para el primer paso de RungeKutta
updateAcc( rm, rM, kvm[0], kvM[0], M, n_masses, n_Masses );
/*
* Rungekutta 2nd step
*/
for( i = 0; i < n_Masses; i++ ){
// RungeKutta 2nd step for n_Masses
newRM[0][i] = rM[0][i] + krM[0][0][i]*dt/2 ;
newRM[1][i] = rM[1][i] + krM[0][1][i]*dt/2 ;
krM[1][0][i] = vM[0][i] + kvM[0][0][i]*dt/2 ;
krM[1][1][i] = vM[1][i] + kvM[0][1][i]*dt/2 ;
}
for( i = 0; i < n_masses; i++ ){
// RungeKutta 2nd step for n_masses
newRm[0][i] = rm[0][i] + krm[0][0][i]*dt/2 ;
newRm[1][i] = rm[1][i] + krm[0][1][i]*dt/2 ;
krm[1][0][i] = vm[0][i] + kvm[0][0][i]*dt/2 ;
krm[1][1][i] = vm[1][i] + kvm[0][1][i]*dt/2 ;
}
// Actualiza las aceleraciones para el segundo paso de RungeKutta
updateAcc( newRm, newRM, kvm[1], kvM[1], M, n_masses, n_Masses );
/*
* Rungekutta 3rd step
*/
for( i = 0; i < n_Masses; i++ ){
// RungeKutta 3rd step for n_Masses
newRM[0][i] = rM[0][i] + krM[1][0][i]*dt/2 ;
newRM[1][i] = rM[1][i] + krM[1][1][i]*dt/2 ;
krM[2][0][i] = vM[0][i] + kvM[1][0][i]*dt/2 ;
krM[2][1][i] = vM[1][i] + kvM[1][1][i]*dt/2 ;
}
for( i = 0; i < n_masses; i++ ){
// RungeKutta 3rd step for n_masses
newRm[0][i] = rm[0][i] + krm[1][0][i]*dt/2 ;
newRm[1][i] = rm[1][i] + krm[1][1][i]*dt/2 ;
krm[2][0][i] = vm[0][i] + kvm[1][0][i]*dt/2 ;
krm[2][1][i] = vm[1][i] + kvm[1][1][i]*dt/2 ;
}
// Actualiza las aceleraciones para el segundo paso de RungeKutta
updateAcc( newRm, newRM, kvm[2], kvM[2], M, n_masses, n_Masses );
/*
* Rungekutta 4th step
*/
for( i = 0; i < n_Masses; i++ ){
// RungeKutta 4th step for n_Masses
newRM[0][i] = rM[0][i] + krM[2][0][i]*dt ;
newRM[1][i] = rM[1][i] + krM[2][1][i]*dt ;
krM[3][0][i] = vM[0][i] + kvM[2][0][i]*dt ;
krM[3][1][i] = vM[1][i] + kvM[2][1][i]*dt ;
}
for( i = 0; i < n_masses; i++ ){
// RungeKutta 4th step for n_masses
newRm[0][i] = rm[0][i] + krm[2][0][i]*dt ;
newRm[1][i] = rm[1][i] + krm[2][1][i]*dt ;
krm[3][0][i] = vm[0][i] + kvm[2][0][i]*dt ;
krm[3][1][i] = vm[1][i] + kvm[2][1][i]*dt ;
}
// Actualiza las aceleraciones para el segundo paso de RungeKutta
updateAcc( newRm, newRM, kvm[3], kvM[3], M, n_masses, n_Masses );
//-----------------------------------------------------------------
//*****************************************************************
// Promedia los k y actualiza los vectores de entrada en i+1
for( i = 0; i < n_Masses; i++ ){
// Promedia los ks de n_Masses
// Actualiza los vectores de entrada
FLOAT kmean = (1/6.0)*(krM[0][0][i] + 2*krM[1][0][i] + 2*krM[2][0][i] + krM[3][0][i]);
rM[0][i] = rM[0][i] + kmean*dt;
kmean = (1/6.0)*(krM[0][1][i] + 2*krM[1][1][i] + 2*krM[2][1][i] + krM[3][1][i]);
rM[1][i] = rM[1][i] + kmean*dt;
kmean = (1/6.0)*(kvM[0][0][i] + 2*kvM[1][0][i] + 2*kvM[2][0][i] + kvM[3][0][i]);
vM[0][i] = vM[0][i] + kmean*dt;
kmean = (1/6.0)*(kvM[0][1][i] + 2*kvM[1][1][i] + 2*kvM[2][1][i] + kvM[3][1][i]);
vM[1][i] = vM[1][i] + kmean*dt;
}
for( i = 0; i < n_masses; i++ ){
// Promedia los ks de n_masses
// Actualiza los vectores de entrada
FLOAT kmean = (1/6.0)*(krm[0][0][i] + 2*krm[1][0][i] + 2*krm[2][0][i] + krm[3][0][i]);
rm[0][i] = rm[0][i] + kmean*dt;
kmean = (1/6.0)*(krm[0][1][i] + 2*krm[1][1][i] + 2*krm[2][1][i] + krm[3][1][i]);
rm[1][i] = rm[1][i] + kmean*dt;
kmean = (1/6.0)*(kvm[0][0][i] + 2*kvm[1][0][i] + 2*kvm[2][0][i] + kvm[3][0][i]);
vm[0][i] = vm[0][i] + kmean*dt;
kmean = (1/6.0)*(kvm[0][1][i] + 2*kvm[1][1][i] + 2*kvm[2][1][i] + kvm[3][1][i]);
vm[1][i] = vm[1][i] + kmean*dt;
}
// Libera memoria
int q,w;
for(q = 0; q < 4; q++){
for( w = 0; w < 2; w++){
free(krm[q][w]);
free(kvm[q][w]);
}
free(krm[q]);
free(kvm[q]);
}
for( w = 0; w < 2; w++){
free(newRM[w]);
}
for( w = 0; w < 2; w++){
free(newRm[w]);
}
for(q = 0; q < 4; q++){
for( w = 0; w < 2; w++){
free(krM[q][w]);
free(kvM[q][w]);
}
free(krM[q]);
free(kvM[q]);
}
free(krm);
free(krM);
free(kvm);
free(kvM);
}
