Problem: subraction giving very strange results

system · June 5, 2012, 8:26pm

Guys I'm having a strange problem with my code.
Basically I'm performing the same operation in a loop, but after the first time I got wrong values back.
The strange part is the vector subtraction Kreference[]-Kstate[], this gives strange results.
In the first run the result is correct but as you can see from the log from the serial port after the first run the results are wrong.
Basically Kreference-Kstate returns four 0. while Kreference is {0,-0.57,0,0.57}. Kstate is {0,0,0,0}.
All the vector are initialized as float.

void loop(){
  Serial.println("Press one button to start moving");
  beta = state[2];//maybe unuseful
  rotationmatrix(beta);
  math.MatrixMult((float*)K,(float*) state, 4, 6, 1,(float*) Kstate);
  Serial.println("state[:]"); 
  Serial.println(state[0]);
  Serial.println(state[1]);
  Serial.println(state[2]);
  Serial.println(state[3]);
  Serial.println(state[4]);
  Serial.println(state[5]);
  Serial.println("Kstate[:]");
  Serial.println(Kstate[0]);
  Serial.println(Kstate[1]);
  Serial.println(Kstate[2]);
  Serial.println(Kstate[3]);
  math.MatrixMult((float*)K,(float*) reference, 4, 6, 1,(float*) Kreference);
  Serial.println("Kreference[:]");  
  Serial.println(Kreference[0]);
  Serial.println(Kreference[1]);
  Serial.println(Kreference[2]);
  Serial.println(Kreference[3]);
  Serial.println("Kreference[:]-Kstate[:]");  
  Serial.println(Kreference[0]-Kstate[0]);
  Serial.println(Kreference[1]-Kstate[1]);
  Serial.println(Kreference[2]-Kstate[2]);
  Serial.println(Kreference[3]-Kstate[3]);
  u[0]=(Kreference[0]-Kstate[0])*100;
  u[1]=(Kreference[1]-Kstate[1])*100;
  u[2]=(Kreference[2]-Kstate[2])*100;
  u[3]=(Kreference[3]-Kstate[3])*100;
  Serial.println("u[:]"); 
  Serial.println(u[0]);
  Serial.println(u[1]);
  Serial.println(u[2]);
  Serial.println(u[3]);
  math.MatrixMult((float*)Rotation,(float*)u, 4, 4, 1,(float*)Input);
  PwmDirection();
  analogWrite(PWMA, torquePWM[0]); 
  digitalWrite(DIRA, Direction[0]);
  analogWrite(PWMB, torquePWM[1]);
  digitalWrite(DIRB, Direction[1]);
  analogWrite(PWMC, torquePWM[2]);
  digitalWrite(DIRC, Direction[2]);
  analogWrite(PWMD, torquePWM[3]);
  digitalWrite(DIRD, Direction[3]);
  Speed();
  delay(400);}

the log:

Press one button to start moving
state[:]
0.00
0.00
0.00
0.00
0.00
0.00
Kstate[:]
0.00
0.00
0.00
0.00
Kreference[:]
0.00
-0.57
0.00
0.57
Kreference[:]-Kstate[:]
0.00
-0.57
0.00
0.57
u[:]
0.00
-56.57
0.00
56.57
Press one button to start moving
state[:]
0.00
0.00
0.00
0.00
0.00
0.00
Kstate[:]
0.00
0.00
0.00
0.00
Kreference[:]
0.00
-0.57
0.00
0.57
Kreference[:]-Kstate[:]
0.00
0.00
0.00
0.00
u[:]
0.00
0.00
0.00
0.00

pylon · June 5, 2012, 8:39pm

Show us the complete code. If the vectors are defined as float arrays why do you have to cast them to (float *)? You see, we have to see all the code to help you.

udoklein · June 5, 2012, 8:40pm

Also float arrays will consume a lot of memory. Maybe you are already out of memory.

system · June 5, 2012, 8:55pm

How can I check if I'm out of memory? The compiled code is 12.702 bytes.

Here is the complete code:

#include <MatrixMath.h>
#include <superSerial.h>
#include <Wire.h>
#include <digitalWriteFast.h>

#define NO_PORTB_PINCHANGES //to indicate that port b will not be used for pin change interrupts
#include <PinChangeInt.h>   //using version 1.6beta

MatrixMath math;
 

//Define pin
#define PINA1 A0
#define PINA2 A1
#define DIRA 13
#define PWMA 11

#define PINB1 A2
#define PINB2 A3
#define DIRB 12
#define PWMB 10

#define PINC1 2
#define PINC2 3
#define DIRC 8
#define PWMC 9

#define PIND1 4
#define PIND2 5
#define DIRD 7
#define PWMD 6

////LQR controller////

//Global variable
double Rotation[4][4];
double InputLimit=0.5; 

float beta;
float state[6]={0};
float Kstate[4];
float reference[6]={0.8,0,0,0,0,0};
float Kreference[4];
float u[4];
float Input[4];

//LQR control vector
int torquePWM[4];
int Direction[4];

//Kinematic Dynamic vector
float vlocal[3]={0,0,0}; //[u,v,theta_dot]
float Vglobal[3]={0,0,0};
float dX;
float dY;
float dth;
unsigned long startTime, lastTime, deltaT;

//Encoder Positions
double countA = 0;
double countB = 0;
double countC = 0;
double countD = 0;

//Reference Velocity
float Vref=1; //m/s

//Wheels speed
double speedwheel[4];

//Conditions
int NewPath=1;
int Step=0;

//coming from MatLab LQR toolbox, precalculated we can even have various K for different type of navigation i.e: with different weight on Q and R//
int CoeffA;
int CoeffB;
int CoeffC;
int CoeffD;
float K[4][6]={{0, 0.7071, 0.2236, 0, 0.8468, 0.2493},
  {-0.7071, 0, 0.2236, -0.8468, 0, 0.2493},
  {0, -0.7071, 0.2236, 0, -0.8468, 0.2493},
  {0.7071, 0.0000, 0.2236, 0.8468, 0, 0.2493}};

float qTOv[3][4]={{     0,   -0.5000,         0,    0.5000},
                  {0.5000,   -0.0000,   -0.5000,    0.0000},
                  {1.6129,    1.6129,    1.6129,    1.6129}};
float vTOV[3][3]={{1,0,0},{0,1,0},{0,0,1}};

//PIN A                     
void funcA1(){
  PCintPort::pinState!=digitalReadFast(PINA2)?countA++:countA--;
}
void funcA2(){
  PCintPort::pinState!=digitalReadFast(PINA1)?countA--:countA++;
}
//PIN B
void funcB1(){
  PCintPort::pinState!=digitalReadFast(PINB2)?countB++:countB--;
}
void funcB2(){
  PCintPort::pinState!=digitalReadFast(PINB1)?countB--:countB++;
}
//PIN C
void funcC1(){
  PCintPort::pinState!=digitalReadFast(PINC2)?countC++:countC--;
}
void funcC2(){
  PCintPort::pinState!=digitalReadFast(PINC1)?countC--:countC++;
}
//PIN D
void funcD1(){
  PCintPort::pinState!=digitalReadFast(PIND2)?countD++:countD--;
}
void funcD2(){
  PCintPort::pinState!=digitalReadFast(PIND1)?countD--:countD++;
}

                     
void setup(){
Serial.begin(115200);
/*WHAT DOES HIGH LOW INPUT MEAN??*/
pinMode(PINA1, INPUT); digitalWrite(PINA1, HIGH);
PCintPort::attachInterrupt(PINA1, &funcA1, CHANGE);  
pinMode(PINA2, INPUT); digitalWrite(PINA2, HIGH);
PCintPort::attachInterrupt(PINA2, &funcA2, CHANGE);
pinMode(DIRA,OUTPUT); digitalWrite(DIRA,LOW);
pinMode(PWMA,OUTPUT); digitalWrite(PWMA,LOW);
  
pinMode(PINB1, INPUT); digitalWrite(PINB1, HIGH);
PCintPort::attachInterrupt(PINB1, &funcB1, CHANGE);  
pinMode(PINB2, INPUT); digitalWrite(PINB2, HIGH);
PCintPort::attachInterrupt(PINB2, &funcB2, CHANGE);
pinMode(DIRB,OUTPUT); digitalWrite(DIRB,LOW);
pinMode(PWMB,OUTPUT); digitalWrite(PWMB,LOW);
  
pinMode(PINC1, INPUT); digitalWrite(PINC1, HIGH);
PCintPort::attachInterrupt(PINC1, &funcC1, CHANGE);  
pinMode(PINC2, INPUT); digitalWrite(PINC2, HIGH);
PCintPort::attachInterrupt(PINC2, &funcC2, CHANGE);
pinMode(DIRC,OUTPUT); digitalWrite(DIRC,LOW);
pinMode(PWMC,OUTPUT); digitalWrite(PWMC,LOW);
  
pinMode(PIND1, INPUT); digitalWrite(PIND1, HIGH);
PCintPort::attachInterrupt(PIND1, &funcD1, CHANGE);  
pinMode(PIND2, INPUT); digitalWrite(PIND2, HIGH);
PCintPort::attachInterrupt(PIND2, &funcD2, CHANGE);
pinMode(DIRD,OUTPUT); digitalWrite(DIRD,LOW);
pinMode(PWMD,OUTPUT); digitalWrite(PWMD,LOW);

//used to compute speed//
startTime = millis();
lastTime = millis();
}

void loop(){
  Serial.println("Press one button to start moving");
    beta = state[2];//maybe unuseful
    rotationmatrix(beta);
  math.MatrixMult((float*)K,(float*) state, 4, 6, 1,(float*) Kstate);//why was not declared?? It's a function!
  Serial.println("state[:]"); 
  Serial.println(state[0]);
  Serial.println(state[1]);
  Serial.println(state[2]);
  Serial.println(state[3]);
  Serial.println(state[4]);
  Serial.println(state[5]);
  Serial.println("Kstate[:]");
  Serial.println(Kstate[0]);
  Serial.println(Kstate[1]);
  Serial.println(Kstate[2]);
  Serial.println(Kstate[3]);
  math.MatrixMult((float*)K,(float*) reference, 4, 6, 1,(float*) Kreference);
  Serial.println("Kreference[:]");  
  Serial.println(Kreference[0]);
  Serial.println(Kreference[1]);
  Serial.println(Kreference[2]);
  Serial.println(Kreference[3]);
  Serial.println("Kreference[:]-Kstate[:]");  
  Serial.println(Kreference[0]-Kstate[0]);
  Serial.println(Kreference[1]-Kstate[1]);
  Serial.println(Kreference[2]-Kstate[2]);
  Serial.println(Kreference[3]-Kstate[3]);
  u[0]=(Kreference[0]-Kstate[0])*100;
  u[1]=(Kreference[1]-Kstate[1])*100;
  u[2]=(Kreference[2]-Kstate[2])*100;
  u[3]=(Kreference[3]-Kstate[3])*100;
  Serial.println("u[:]"); 
  Serial.println(u[0]);
  Serial.println(u[1]);
  Serial.println(u[2]);
  Serial.println(u[3]);
  math.MatrixMult((float*)Rotation,(float*)u, 4, 4, 1,(float*)Input);
  PwmDirection();
  analogWrite(PWMA, torquePWM[0]); 
  digitalWrite(DIRA, Direction[0]);
  analogWrite(PWMB, torquePWM[1]);
  digitalWrite(DIRB, Direction[1]);
  analogWrite(PWMC, torquePWM[2]);
  digitalWrite(DIRC, Direction[2]);
  analogWrite(PWMD, torquePWM[3]);
  digitalWrite(DIRD, Direction[3]);
  Speed();
  delay(400);}

void rotationmatrix(float angle){
double coth,sith;
coth=cos(angle);
sith=sin(angle);
Rotation[0][0]=(pow(coth,2)/2 + coth/2 + pow(sith,2)/2)/(pow(coth,2) + pow(sith,2));
Rotation[0][1]=sith/(2*(pow(coth,2) + pow(sith,2)));
Rotation[0][2]=(pow(coth,2)/2 - coth/2 + pow(sith,2)/2)/(pow(coth,2) + pow(sith,2));
Rotation[0][3]=-sith/(2*(pow(coth,2) + pow(sith,2)));
Rotation[1][0]=Rotation[0][3];
Rotation[1][1]=Rotation[0][0];
Rotation[1][2]=Rotation[0][1];
Rotation[1][3]=Rotation[0][2];
Rotation[2][0]=Rotation[0][2];
Rotation[2][1]=Rotation[0][3];
Rotation[2][2]=Rotation[0][0];
Rotation[2][3]=Rotation[0][1];
Rotation[3][0]=Rotation[0][1];
Rotation[3][1]=Rotation[0][2];
Rotation[3][2]=Rotation[0][3];
Rotation[3][3]=Rotation[0][0];}

void PwmDirection(){
  int i;
  for(i=0;i<=3;i=i++){
     if(abs(Input[i])>InputLimit){
     Input[i]=InputLimit;
     //Serial.println("PwmDirection ifabs");
     }
     if(Input[i]>0){
     Direction[1]=HIGH;}
     else{//ma si può fare questa struttura if if else??
     Direction[i]=LOW;}
  torquePWM[i]=abs((Input[i]/InputLimit)*255);
}}

void Speed(){
startTime=millis();
deltaT=(startTime-lastTime)/1000;//SHOULD BE CONVERTED IN SEC//
CoeffA=10000;
CoeffB=10000;
CoeffC=10000;
CoeffD=10000;
speedwheel[0]=countA/(deltaT*CoeffA);//m/s in theory
speedwheel[1]=countB/(deltaT*CoeffB);//m/s in theory
speedwheel[2]=countC/(deltaT*CoeffC);//m/s in theory
speedwheel[3]=countD/(deltaT*CoeffD);//m/s in theory
countA = 0;
countB = 0;
countC = 0;
countD = 0;
lastTime=millis();
math.MatrixMult((float*)qTOv,(float*)speedwheel, 3, 4, 1,(float*)vlocal);
RotationMatrix();
math.MatrixMult((float*)vTOV,(float*)vlocal, 3, 3, 1,(float*)Vglobal); //consider using a 2x2 matrix to save computational time
dX=((Vglobal[0]+state[3])/2)*deltaT; //RK2 integration method//
dY=((Vglobal[1]+state[4])/2)*deltaT; //RK2 integration method//
dth=((Vglobal[2]+state[5])/2)*deltaT;//RK2 integration method//
state[0]=state[0]+dX;
state[1]=state[1]+dY;
state[2]=state[2]+dth;
state[3]=Vglobal[0];
state[4]=Vglobal[1];
state[5]=Vglobal[2];
}

void RotationMatrix(){
vTOV[0][0]=cos(state[2]);
vTOV[0][1]=-sin(state[2]);
vTOV[1][0]=-vTOV[0][1];
vTOV[1][1]=vTOV[0][0];}

system · June 5, 2012, 9:00pm

pylon:
Show us the complete code. If the vectors are defined as float arrays why do you have to cast them to (float *)? You see, we have to see all the code to help you.

Cause otherwise It doesn't compile. The mathMatrixMult function are part of a library I didn't write.
Since I'm using only MatrixMult should I add the function along the code insted of adding the whole library??

lloyddean · June 5, 2012, 9:56pm

No the Linker will strip what the program doesn't use.

marco_c · June 5, 2012, 9:58pm

The compiled code is 12.702 bytes

The code and the data space are very different. The code sits in flash RAM but the data sits in 'normal' (static) RAM. There is a lot less static RAM that there is flash RAM - how much depends on your AVR processor. You can find out in the datasheets for the processor.

The code at Arduino Playground - HomePage may also help.

system · June 5, 2012, 10:01pm

marco_c:

The compiled code is 12.702 bytes

The code and the data space are very different. The code sits in flash RAM but the data sits in 'normal' (static) RAM. There is a lot less static RAM that there is flash RAM - how much depends on your AVR processor. You can find out in the datasheets for the processor.

The code at Arduino Playground - HomePage may also help.

Ok, so is there a way to check if I'm running out of memory?
I know malloc should do something similar but there's nothing for Arduino I think...
any clue?

marco_c · June 5, 2012, 10:04pm

Yes. Have you looked at the code in the link?

Generally using functions like malloc() on embedded processors is a bad idea. Use static arrays and manage you memory usage really closely.

system · June 5, 2012, 10:16pm

marco_c:
Yes. Have you looked at the code in the link?

Generally using functions like malloc() on embedded processors is a bad idea. Use static arrays and manage you memory usage really closely.

Ok, I run this function Serial.println(freeMemory()); and I got always the same value: 957. I ran it just before and after the crytical part giving me error. Is it ok to have always the same result?
I have some variables that are constant and won't be changed should I move them to the flash memory using PROGMEM?

cmiyc · June 5, 2012, 10:29pm

ned112:
Ok, so is there a way to check if I'm running out of memory?
I know malloc should do something similar but there's nothing for Arduino I think...
any clue?

In an embedded system with no memory controller, the only absolute method is to keep track of memory usage yourself. You can start by counting up how many bytes your global variables use. In your case, you're at about 477 bytes. So 25% of the RAM is just from global variables.

A couple of things to realize:

There's no difference between a float and double on the arduino.
All floating point math is done in software and is very slow.
There is a difference between the constants "0" and "0.0". The first is an integer, the second is a float.
e.g. this line:
u[0]=(Kreference[0]-Kstate[0])*100;
would be better stated:
u[0]=(Kreference[0]-Kstate[0])*100.0;

system · June 5, 2012, 10:36pm

ned112:
Ok, so is there a way to check if I'm running out of memory?
I know malloc should do something similar but there's nothing for Arduino I think...
any clue?

In an embedded system with no memory controller, the only absolute method is to keep track of memory usage yourself. You can start by counting up how many bytes your global variables use. In your case, you're at about 477 bytes. So 25% of the RAM is just from global variables.

A couple of things to realize:

There's no difference between a float and double on the arduino.

All floating point math is done in software and is very slow.

There is a difference between the constants "0" and "0.0". The first is an integer, the second is a float.
e.g. this line:
u[0]=(Kreference[0]-Kstate[0])*100;
would be better stated:
u[0]=(Kreference[0]-Kstate[0])*100.0;

Ok, thanks, for double and float I know there are no differencies, but the code has been changed and rewritten a lot of time so some of these are just survived the cleaning.
Thanks for point 3, in any case that 100 will disappear I put it there just to check the 0 value that was appering wasn't simply a value to small to be displayed.

For point 2 I know it's slow but how can I solve it?

system · June 5, 2012, 10:57pm

Ok, I run the code without the Speed() function, and It works, the freememory function report something more the 1100bytes.
But it's so strange... and even very problematic cause I'll need more space than the one I'm using now (I'm supposed to run even a bezier curve function with this... so at leat other two float [21] and 1 float [21][4]!

system · June 5, 2012, 11:54pm

And even stranger, if I run the code, every time i close and open again the SerialMonitor i got a shot of right calculation. Then all the others coming are wrong. If i close serial monitor and open it again I got another right calcultaion and then again all the other go wrong.

system · June 6, 2012, 12:03am

pylon:
Show us the complete code.

This.

system · June 6, 2012, 12:35am

PeterH:

pylon:
Show us the complete code.

This.

Yeah I forgot.
This one is a little modified in respect to the one posted before. I tried to use less global variables to save some space, but the problem is still there.

#include <MatrixMath.h>
#include <superSerial.h>
#include <Wire.h>
#include <digitalWriteFast.h>
#include <MemoryFree.h>

#define NO_PORTB_PINCHANGES //to indicate that port b will not be used for pin change interrupts
#include <PinChangeInt.h>   //using version 1.6beta

MatrixMath math;



//Define pin
#define PINA1 A0
#define PINA2 A1
#define DIRA 13
#define PWMA 11

#define PINB1 A2
#define PINB2 A3
#define DIRB 12
#define PWMB 10

#define PINC1 2
#define PINC2 3
#define DIRC 8
#define PWMC 9

#define PIND1 4
#define PIND2 5
#define DIRD 7
#define PWMD 6


////LQR controller////

//Global variable
float Rotation[4][4];
float InputLimit=0.5;

float state[6]={0};
float reference[6]={0.8,0,0,0,0,0};

float Input[4];

//LQR control vector
int torquePWM[4];
int Direction[4];



unsigned long lastTime=0;

//Encoder Positions
double countA = 0;
double countB = 0;
double countC = 0;
double countD = 0;

//Reference Velocity
//int Vref=1; //m/s

//Wheels speed


//Conditions
int NewPath=1;
int Step=0;

//coming from MatLab LQR toolbox, precalculated we can even have various K for different type of navigation i.e: with different weight on Q and R//

float K[4][6]={{0, 0.7071, 0.2236, 0, 0.8468, 0.2493},
  {-0.7071, 0, 0.2236, -0.8468, 0, 0.2493},
  {0, -0.7071, 0.2236, 0, -0.8468, 0.2493},
  {0.7071, 0.0000, 0.2236, 0.8468, 0, 0.2493}};

float qTOv[3][4]={{     0,   -0.5000,         0,    0.5000},
                  {0.5000,   -0.0000,   -0.5000,    0.0000},
                  {1.6129,    1.6129,    1.6129,    1.6129}};
float vTOV[3][3]={{1,0,0},{0,1,0},{0,0,1}};

//PIN A                     
void funcA1(){
  PCintPort::pinState!=digitalReadFast(PINA2)?countA++:countA--;
}
void funcA2(){
  PCintPort::pinState!=digitalReadFast(PINA1)?countA--:countA++;
}
//PIN B
void funcB1(){
  PCintPort::pinState!=digitalReadFast(PINB2)?countB++:countB--;
}
void funcB2(){
  PCintPort::pinState!=digitalReadFast(PINB1)?countB--:countB++;
}
//PIN C
void funcC1(){
  PCintPort::pinState!=digitalReadFast(PINC2)?countC++:countC--;
}
void funcC2(){
  PCintPort::pinState!=digitalReadFast(PINC1)?countC--:countC++;
}
//PIN D
void funcD1(){
  PCintPort::pinState!=digitalReadFast(PIND2)?countD++:countD--;
}
void funcD2(){
  PCintPort::pinState!=digitalReadFast(PIND1)?countD--:countD++;
}

                     
void setup(){
Serial.begin(115200);
/*WHAT DOES HIGH LOW INPUT MEAN??*/
pinMode(PINA1, INPUT); digitalWrite(PINA1, HIGH);
PCintPort::attachInterrupt(PINA1, &funcA1, CHANGE);  
pinMode(PINA2, INPUT); digitalWrite(PINA2, HIGH);
PCintPort::attachInterrupt(PINA2, &funcA2, CHANGE);
pinMode(DIRA,OUTPUT); digitalWrite(DIRA,LOW);
pinMode(PWMA,OUTPUT); digitalWrite(PWMA,LOW);
  
pinMode(PINB1, INPUT); digitalWrite(PINB1, HIGH);
PCintPort::attachInterrupt(PINB1, &funcB1, CHANGE);  
pinMode(PINB2, INPUT); digitalWrite(PINB2, HIGH);
PCintPort::attachInterrupt(PINB2, &funcB2, CHANGE);
pinMode(DIRB,OUTPUT); digitalWrite(DIRB,LOW);
pinMode(PWMB,OUTPUT); digitalWrite(PWMB,LOW);
  
pinMode(PINC1, INPUT); digitalWrite(PINC1, HIGH);
PCintPort::attachInterrupt(PINC1, &funcC1, CHANGE);  
pinMode(PINC2, INPUT); digitalWrite(PINC2, HIGH);
PCintPort::attachInterrupt(PINC2, &funcC2, CHANGE);
pinMode(DIRC,OUTPUT); digitalWrite(DIRC,LOW);
pinMode(PWMC,OUTPUT); digitalWrite(PWMC,LOW);
  
pinMode(PIND1, INPUT); digitalWrite(PIND1, HIGH);
PCintPort::attachInterrupt(PIND1, &funcD1, CHANGE);  
pinMode(PIND2, INPUT); digitalWrite(PIND2, HIGH);
PCintPort::attachInterrupt(PIND2, &funcD2, CHANGE);
pinMode(DIRD,OUTPUT); digitalWrite(DIRD,LOW);
pinMode(PWMD,OUTPUT); digitalWrite(PWMD,LOW);
}

void loop(){
  float Kreference[4]={0,0,0,0};
  float u[4]={0,0,0,0};
  float Kstate[4]={0,0,0,0};
  Serial.println("Press one button to start moving");
  rotationmatrix(state[2]);
  math.MatrixMult((float*)K,(float*)state, 4, 6, 1,(float*)Kstate);//why was not declared?? It's a function!
  Serial.print("freeMemory()=");
  Serial.println(freeMemory());
  math.MatrixMult((float*)K,(float*) reference, 4, 6, 1,(float*) Kreference);
  Serial.print("freeMemory()=");
  Serial.println(freeMemory());
  u[0]=(Kreference[0]-Kstate[0]);
  u[1]=(Kreference[1]-Kstate[1]);
  u[2]=(Kreference[2]-Kstate[2]);
  u[3]=(Kreference[3]-Kstate[3]);
  Serial.println("u[:]"); 
  Serial.println(u[0]);
  Serial.println(u[1]);
  Serial.println(u[2]);
  Serial.println(u[3]);
  math.MatrixMult((float*)Rotation,(float*)u, 4, 4, 1,(float*)Input);
  PwmDirection();
  analogWrite(PWMA, torquePWM[0]); 
  digitalWrite(DIRA, Direction[0]);
  analogWrite(PWMB, torquePWM[1]);
  digitalWrite(DIRB, Direction[1]);
  analogWrite(PWMC, torquePWM[2]);
  digitalWrite(DIRC, Direction[2]);
  analogWrite(PWMD, torquePWM[3]);
  digitalWrite(DIRD, Direction[3]);
  Speed();
  delay(400);}

void rotationmatrix(float angle){ 
double coth,sith;
coth=cos(angle);
sith=sin(angle);
Rotation[0][0]=(pow(coth,2)/2 + coth/2 + pow(sith,2)/2)/(pow(coth,2) + pow(sith,2));
Rotation[0][1]=sith/(2*(pow(coth,2) + pow(sith,2)));
Rotation[0][2]=(pow(coth,2)/2 - coth/2 + pow(sith,2)/2)/(pow(coth,2) + pow(sith,2));
Rotation[0][3]=-sith/(2*(pow(coth,2) + pow(sith,2)));
Rotation[1][0]=Rotation[0][3];
Rotation[1][1]=Rotation[0][0];
Rotation[1][2]=Rotation[0][1];
Rotation[1][3]=Rotation[0][2];
Rotation[2][0]=Rotation[0][2];
Rotation[2][1]=Rotation[0][3];
Rotation[2][2]=Rotation[0][0];
Rotation[2][3]=Rotation[0][1];
Rotation[3][0]=Rotation[0][1];
Rotation[3][1]=Rotation[0][2];
Rotation[3][2]=Rotation[0][3];
Rotation[3][3]=Rotation[0][0];}

void PwmDirection(){
  int i;
  for(i=0;i<=3;i=i++){
     if(abs(Input[i])>InputLimit){
     Input[i]=InputLimit;
     //Serial.println("PwmDirection ifabs");
     }
     if(Input[i]>0){
     Direction[1]=HIGH;}
     else{//ma si può fare questa struttura if if else??
     Direction[i]=LOW;}
  torquePWM[i]=abs((Input[i]/InputLimit)*255);
}}

void Speed(){
unsigned long startTime=0, deltaT=0;
float vlocal[3]={0,0,0};
float Vglobal[3]={0,0,0};
float dX=0, dY=0, dth=0;
float speedwheel[4]={0,0,0,0};
startTime=millis();
deltaT=(startTime-lastTime)/1000;//SHOULD BE CONVERTED IN SEC//
int CoeffA=10000;
int CoeffB=10000;
int CoeffC=10000;
int CoeffD=10000;
speedwheel[0]=countA/(deltaT*CoeffA);//m/s in theory
speedwheel[1]=countB/(deltaT*CoeffB);//m/s in theory
speedwheel[2]=countC/(deltaT*CoeffC);//m/s in theory
speedwheel[3]=countD/(deltaT*CoeffD);//m/s in theory
countA = 0;
countB = 0;
countC = 0;
countD = 0;
lastTime=millis();
math.MatrixMult((float*)qTOv,(float*)speedwheel, 3, 4, 1,(float*)vlocal);
RotationMatrix();
math.MatrixMult((float*)vTOV,(float*)vlocal, 3, 3, 1,(float*)Vglobal); //consider using a 2x2 matrix to save computational time
dX=((Vglobal[0]+state[3])/2)*deltaT; //RK2 integration method//
dY=((Vglobal[1]+state[4])/2)*deltaT; //RK2 integration method//
dth=((Vglobal[2]+state[5])/2)*deltaT;//RK2 integration method//
state[0]=state[0]+dX;
state[1]=state[1]+dY;
state[2]=state[2]+dth;
state[3]=Vglobal[0];
state[4]=Vglobal[1];
state[5]=Vglobal[2];
}

void RotationMatrix(){
vTOV[0][0]=cos(state[2]);
vTOV[0][1]=-sin(state[2]);
vTOV[1][0]=-vTOV[0][1];
vTOV[1][1]=vTOV[0][0];}

marco_c · June 6, 2012, 12:42am

And even stranger, if I run the code, every time i close and open again the SerialMonitor i got a shot of right calculation

The CPU is reset when you run the Serial monitor. If you put something in the setup function you will see that that prints every time you close and open the serial monitor window.

system · June 6, 2012, 12:57am

marco_c:

And even stranger, if I run the code, every time i close and open again the SerialMonitor i got a shot of right calculation

The CPU is reset when you run the Serial monitor. If you put something in the setup function you will see that that prints every time you close and open the serial monitor window.

Ok, so this explain the strange behavior...
Still i can't get on how the speed function is making my program going so bad.
I even realized that the problem happens in every subtraction. I checked and when I do startTime-lastTime I always have 0 as results... even if the two are non-zero... (i checked through SerialMonitor)

marco_c · June 6, 2012, 1:15am

Some things I would look at:

put some print statements in the matrix multiplication function to see if things look right there
put up the matrix multiplication function so we can see it

system · June 6, 2012, 1:23am

marco_c:
Some things I would look at:

put some print statements in the matrix multiplication function to see if things look right there

put up the matrix multiplication function so we can see it

I checked the results of the multiplication are right... I usually print the output and compare with matlab computation. The routine is also a standard library available on playground. So it should be fine.
Should I try a different compiler??

//Matrix Multiplication Routine
// C = A*B
void MatrixMath::MatrixMult(float* A, float* B, int m, int p, int n, float* C)
{
	// A = input matrix (m x p)
	// B = input matrix (p x n)
	// m = number of rows in A
	// p = number of columns in A = number of rows in B
	// n = number of columns in B
	// C = output matrix = A*B (m x n)
	int i, j, k;
	for (i=0;i<m;i++)
		for(j=0;j<n;j++)
		{
			C[n*i+j]=0;
			for (k=0;k<p;k++)
				C[n*i+j]= C[n*i+j]+A[p*i+k]*B[n*k+j];
		}
}

Topic		Replies	Views
Simple math strange behavior while subtracting Programming	11	1321	May 5, 2021
unwanted variable change Programming	13	2236	May 5, 2021
[Solved]calculation issue Programming	9	1051	May 5, 2021
Serial monitor printing out wrong value General Guidance	10	760	July 31, 2022
Subtraction function verifys OK but does not work on serial monitor Programming	5	943	May 5, 2021

Problem: subraction giving very strange results

Related topics