Pages: 1 [2]   Go Down
Author Topic: Problem: subraction giving very strange results  (Read 1403 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 33
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset


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.

Code:
#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];}


« Last Edit: June 05, 2012, 07:40:26 pm by ned112 » Logged

Sydney, Australia
Offline Offline
Edison Member
*
Karma: 33
Posts: 1268
Big things come in large packages
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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.
Logged

Arduino libraries http://arduinocode.codeplex.com
Parola hardware & library http://parola.codeplex.com

Offline Offline
Newbie
*
Karma: 0
Posts: 33
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
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)
« Last Edit: June 05, 2012, 08:02:54 pm by ned112 » Logged

Sydney, Australia
Offline Offline
Edison Member
*
Karma: 33
Posts: 1268
Big things come in large packages
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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


Logged

Arduino libraries http://arduinocode.codeplex.com
Parola hardware & library http://parola.codeplex.com

Offline Offline
Newbie
*
Karma: 0
Posts: 33
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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??
Code:
//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];
}
}
Logged

Seattle, WA USA
Online Online
Brattain Member
*****
Karma: 610
Posts: 49016
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Quote
No the Linker will strip what the program doesn't use.
Actually, it's the other way around. The linker will only link in what is actually needed. Other stuff in a .o file will not be used.
Logged

Offline Offline
Newbie
*
Karma: 0
Posts: 33
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset


Compiling with Arduino 1.0.1 works. The Mistery of the Compiler! smiley-roll-blue
Logged

Pages: 1 [2]   Go Up
Jump to: