Go Down

### Topic: [parte 6]Unire il tutto (Read 2493 times)previous topic - next topic

#### lestofante

##### Jan 17, 2011, 12:12 amLast Edit: Jan 17, 2011, 12:29 am by lestofante Reason: 1
Ed ecco il risultato finale:
purtroppo non è ancora provato sul campo, infatti anche il valori in pasto al PID sono sicuramente errati
Code: [Select]
`#include <Wire.h>#include "NunchukFarlocco.h"#include "IMU.h"#include "Motore.h"#include "Giroscopio.h"#include "InputPin.h"#include "PID.h"#define ledPin 13#define gyroXpin 1#define gyroYpin 2#define gyroZpin 3#define motore1PWMPin 3#define motore2PWMPin 9#define motore3PWMPin 10#define motore4PWMPin 11//If max PWM value is 255, and the max input of PID is -PI<input<PI then -100<P<100 because PI*100=314 that is bigger than 255#define PofPID 1#define MIN_RADIO_VALUE 900#define MAX_RADIO_VALUE 1900#define maxReceiverAngle 0.2 //maxReceiverAngle is in radiant#define PITCH_STABLE_ANGLE 0#define ROLL_STABLE_ANGLE 1.55#define YAW_STABLE_ANGLE 0Giroscopio gyro[3];NunchukFarlocco nun;IMU orizzonte;PID pid[4]={PID(PofPID), PID(PofPID), PID(PofPID), PID(PofPID)};Motore motori[] = { Motore(motore1PWMPin), Motore(motore2PWMPin), Motore(motore3PWMPin), Motore(motore4PWMPin) };//channel 0=power, 1=pitch, 2=roll, 3=yaw from 900 to 1900InputPin inputs;unsigned long time;boolean statoLedPin=false;void setup(){  time = micros();  Serial.begin(19200);  pinMode(ledPin, OUTPUT);  digitalWrite(ledPin, HIGH);  //setting gyro pin, automagigally they calibration  gyro[0].setPin(gyroXpin);  gyro[1].setPin(gyroXpin);  gyro[2].setPin(gyroXpin);    digitalWrite(ledPin, LOW);    Serial.println("begin nunchuck");  nun.nunchuck_init();  Serial.println("end nunchuck");}int loopCount=0;unsigned long timeMid=0;unsigned long time2;float lastYaw=0;void loop(){  //Serial.print("loop");  loopCount++;  time2=micros();  if (timeMid==0){    timeMid=time2-time;  }else{    timeMid= (timeMid+(time2-time) )/2;  }  time=time2;    if ( nun.nunchuck_get_data() ){    orizzonte.IMUupdate(gyro[0].leggi(), gyro[1].leggi(), gyro[2].leggi(), nun.x, nun.y, nun.z);    //orizzonte.IMUupdate(0, 0, 0, nun.x, nun.y, nun.z); //FOR DEBUG  }else{    Serial.println("Nunckuck non pronto!!");  }  float roll=atan2(2*(orizzonte.q0*orizzonte.q1+orizzonte.q2*orizzonte.q3), 1-2*(orizzonte.q1*orizzonte.q1+orizzonte.q2*orizzonte.q2));  float pitch=asin( 2*(orizzonte.q0*orizzonte.q2-orizzonte.q1*orizzonte.q3) );  float yaw=atan2(2*(orizzonte.q0*orizzonte.q3+orizzonte.q1*orizzonte.q2), 1-2*(orizzonte.q2*orizzonte.q2+orizzonte.q3*orizzonte.q3));    if (loopCount>200){    Serial.print("Actual angle:");    Serial.print(roll);    Serial.print(" ");    Serial.print(pitch);    Serial.print(" ");    Serial.print(yaw);    Serial.println();  }    //find out actual estimated yaw angular rotation. We can also use directly gyro value with a proportion :-D  yaw-=lastYaw;  lastYaw=yaw;    /* because a quadricopter, engine setup:      0    2   3      1  */  float engineAngle[4];  engineAngle[0]=roll+yaw;  engineAngle[1]=-roll+yaw;  engineAngle[2]=-pitch-yaw;  engineAngle[3]=pitch-yaw;  int i;    /*  //USER INPUT  */    // FAKE INPUT, FOR DEBUG PURPOISE  //  long enginesPower=0;  float desiredAngle[4];  for (i=0; i < 4; i++){    desiredAngle[0]= ROLL_STABLE_ANGLE+YAW_STABLE_ANGLE;    desiredAngle[1]= -ROLL_STABLE_ANGLE+YAW_STABLE_ANGLE;    desiredAngle[2]= -PITCH_STABLE_ANGLE-YAW_STABLE_ANGLE;    desiredAngle[3]= PITCH_STABLE_ANGLE-YAW_STABLE_ANGLE;  }  //  // END FAKE INPUT  //  /*  //  // REAL INPUT  //  enginesPower=map( inputs.getDuration(0), MIN_RADIO_VALUE, MAX_RADIO_VALUE, 0, 200 );    //angle in radiant!!  pitch = map( inputs.getDuration(1), MIN_RADIO_VALUE, MAX_RADIO_VALUE, -maxReceiverAngle, maxReceiverAngle )+PITCH_STABLE_ANGLE;  roll = map( inputs.getDuration(2), MIN_RADIO_VALUE, MAX_RADIO_VALUE, -maxReceiverAngle, maxReceiverAngle )+ROLL_STABLE_ANGLE;  yaw = map( inputs.getDuration(3), MIN_RADIO_VALUE, MAX_RADIO_VALUE, -maxReceiverAngle, maxReceiverAngle )+YAW_STABLE_ANGLE;    desiredAngle[0]=roll+yaw;  desiredAngle[1]=-roll+yaw;  desiredAngle[2]=-pitch-yaw;  desiredAngle[3]=pitch-yaw;  //  // END REAL INPUT  //  */    /*  //END USER INPUT  */    float enginePower[4];    int maxP=0;  for (i=0; i < 4; i++){    enginePower[i]=pid[i].update(engineAngle[i], desiredAngle[i])+enginesPower;    if (enginePower[i]>maxP)      maxP=enginePower[i];  }    if (maxP>255){//if motor power requested is bigger than maximus (255 for PWM)    for (i=0; i < 4; i++){      enginePower[i]=map(enginePower[i], 0, maxP, 0, 255);//scale engine power    }  }  for (i=0; i < 4; i++){    motori[i].update( enginePower[i] );  }    if (loopCount>200){    /*    Serial.print(gyroX);    Serial.print(" ");    Serial.print(gyroY);    Serial.print(" ");    Serial.print(gyroZ);    Serial.println();        Serial.print(nun.x);    Serial.print(" ");    Serial.print(nun.y);    Serial.print(" ");    Serial.print(nun.z);    Serial.println();        Serial.print(gyroMid[0]);    Serial.print(" ");    Serial.print(gyroMid[1]);    Serial.print(" ");    Serial.print(gyroMid[2]);    Serial.println(" ");    */    statoLedPin=!statoLedPin;    if (statoLedPin)      digitalWrite(ledPin, HIGH);    else      digitalWrite(ledPin, LOW);        Serial.print("TIME:");    Serial.println(timeMid);        Serial.print(orizzonte.q0);    Serial.print(" ");    Serial.print(orizzonte.q1);    Serial.print(" ");    Serial.print(orizzonte.q2);    Serial.print(" ");    Serial.print(orizzonte.q3);    Serial.println();        //recalculate absolute yaw, because was angular yaw rotation    //yaw=atan2(2*(orizzonte.q0*orizzonte.q3+orizzonte.q1*orizzonte.q2), 1-2*(orizzonte.q2*orizzonte.q2+orizzonte.q3*orizzonte.q3));        Serial.print(roll);    Serial.print(" ");    Serial.print(pitch);    Serial.print(" ");    Serial.print(yaw);    Serial.println();        Serial.print("Motori:");    Serial.print(enginePower[0]);    Serial.print(" ");    Serial.print(enginePower[1]);    Serial.print(" ");    Serial.print(enginePower[2]);    Serial.print(" ");    Serial.println(enginePower[3]);    loopCount=0;  }}`
Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

#### lestofante

#1
##### Jan 17, 2011, 12:23 am
da notare
Code: [Select]
`    Serial.print(orizzonte.q0);    Serial.print(" ");    Serial.print(orizzonte.q1);    Serial.print(" ");    Serial.print(orizzonte.q2);    Serial.print(" ");    Serial.print(orizzonte.q3);    Serial.println();`

questa parte viene ricevuta da un programma Java che disegna un parallelepipedo 3d per mostrare il comportamento dall'algoritmo IMU.
È necessaria la libreria JME 2 http://www.jmonkeyengine.org/ per la grafica 3d e rxtx per le comunicazioni seriali.
Il codice è molto basico perchè mi serviva solo come test. 4 numeri separati da " " e seguiti da "\n" vengono considerati quaternione di rotazione e applicati. Quindi se dovete scrivere dei valori per sicurezza fateli seguire da una scritta (senza andare a capo) e siete sicuri che non verranno erroneamente interpretati

main.java
Code: [Select]
`package orizzonteartificiale;import com.jme.math.Quaternion;/** * * @author lesto */public class Main {    public static void main(String[] args) {      GraphicsThread g = new GraphicsThread();        new Thread(g).start();        ReadSerial r = new ReadSerial();        String s;        String v[];        float a, b, c, d;        float a1=0, b1=0, c1=0, d1=0;        boolean set = false;        while (true){            s=r.pollCommands();            if (r.pollSize()>100){                System.out.println("PollSize:"+r.pollSize());            }            //System.out.println("Comando ok:"+s);            v=s.split(" ");            if (v.length==4){                //we have a quaternion                try{                    a = Float.parseFloat(v[0]);                    b = Float.parseFloat(v[1]);                    c = Float.parseFloat(v[2]);                    d = Float.parseFloat(v[3]);                    g.setBox( new Quaternion(a, b, c, d) );                    double roll=Math.atan2(2*(a*b+c*d), 1-2*(b*b+c*c));                    double pitch=Math.asin( 2*(a*c-b*d) );                    double yaw=Math.atan2(2*(a*d+b*c), 1-2*(c*c+d*d));                    System.out.println("Angoli:"+roll+" "+pitch+" "+yaw);                }catch(Exception e){                    System.out.println("Errore: "+e);                }            }        }    }}`

Code: [Select]
`package orizzonteartificiale;/** * * @author lesto */import gnu.io.CommPort;import gnu.io.CommPortIdentifier;import gnu.io.SerialPort;import java.io.BufferedReader;import java.io.IOException;import java.io.InputStream;import java.io.InputStreamReader;import java.util.Enumeration;import java.util.concurrent.LinkedBlockingQueue;public class ReadSerial implements Runnable{      LinkedBlockingQueue<String> comandi = new LinkedBlockingQueue<String>();      boolean finito=false;      InputStream in;      public ReadSerial() {            boolean portFound = false;            String defaultPort;            // determine the name of the serial port on several operating systems            String osname = System.getProperty("os.name", "").toLowerCase();            System.out.println(osname);            if (osname.startsWith("windows")) {                  // windows                  defaultPort = "COM1";            } else if (osname.startsWith("linux")) {                  // linux                  defaultPort = "/dev/ttyUSB0";            } else if (osname.startsWith("mac")) {                  // mac                  defaultPort = "????";            } else {                  System.out.println("Sorry, your operating system is not supported");                  return;            }            System.out.println("Set default port to " + defaultPort);            Enumeration<?> portList = CommPortIdentifier.getPortIdentifiers();            CommPortIdentifier portId;            while (portList.hasMoreElements()) {                  portId = (CommPortIdentifier) portList.nextElement();                  if (portId.getPortType() == CommPortIdentifier.PORT_SERIAL) {                        System.out.println("Serial found port: " + defaultPort);                        if (portId.getName().equals(defaultPort)) {                              System.out.println("Found port: " + defaultPort);                              portFound = true;                        }                  }            }            if (!portFound) {                  System.out.println("port " + defaultPort + " not found.");                  System.exit(0);            }            try {                  System.out.println("connessione");                  connect(defaultPort);                  System.out.println("connessione fine");            } catch (Exception e) {                  // TODO Auto-generated catch block                  e.printStackTrace();            }      }      void connect(String portName) throws Exception {            CommPortIdentifier portIdentifier = CommPortIdentifier                        .getPortIdentifier(portName);            if (portIdentifier.isCurrentlyOwned()) {                  System.out.println("Error: Port is currently in use");            } else {                  CommPort commPort = portIdentifier.open(this.getClass().getName(),                              2000);                  if (commPort instanceof SerialPort) {                        SerialPort serialPort = (SerialPort) commPort;                        serialPort.setSerialPortParams(19200, SerialPort.DATABITS_8,                                    SerialPort.STOPBITS_1, SerialPort.PARITY_NONE);                        in = serialPort.getInputStream();                        // OutputStream out = serialPort.getOutputStream();                        Thread t= new Thread(this, "Reader");                        t.start();                  } else {                        System.out.println("Error: Only serial ports are handled.");                  }            }      }      public String pollCommands(){                  try {                        return comandi.take();                  } catch (InterruptedException e) {                        // TODO Auto-generated catch block                        e.printStackTrace();                  }                  return null;      }      @Override      public void run() {            System.out.println("starting:");                String inputstring;                BufferedReader bufferedReader = new BufferedReader(new InputStreamReader(in));            while (!finito) {                  try {                        while (bufferedReader.ready()) {                              inputstring = bufferedReader.readLine();                              if (inputstring!=null){                                    try {                                          comandi.put(inputstring);                                    } catch (InterruptedException e) {                                          // TODO Auto-generated catch block                                          e.printStackTrace();                                    }                                    //System.out.print("input: ");                                    //System.out.println(inputstring);                              }                        }                  } catch (IOException e) {                        // TODO Auto-generated catch block                        e.printStackTrace();                  }            }            System.out.println("End");      }      public void stop(){            finito = true;      }    public int pollSize() {        return comandi.size();    }}`

HelloWorld.java
Code: [Select]
`package orizzonteartificiale;import com.jme.app.SimpleGame;import com.jme.math.Quaternion;import com.jme.scene.shape.Box;import com.jme.math.Vector3f;/** * * @author mauro */public class HelloWorld extends SimpleGame {    Box b=null;    public HelloWorld(){        b = new Box("Mybox", new Vector3f(-2, -5, -5), new Vector3f(2, 5, 5));                setConfigShowMode(ConfigShowMode.AlwaysShow);    }    protected void simpleInitGame() {      rootNode.attachChild(b); // Put it in the scene graph    }    public void setBox(float angle, Vector3f axis){        b.getLocalRotation().fromAngleAxis(angle, axis);        System.out.println("setting rotation:"+angle);    }    public void setBox(Quaternion q){        b.setLocalRotation(q);        System.out.println("setting quaternion:"+q);    }}`

Code: [Select]
`package orizzonteartificiale;import com.jme.math.Quaternion;import com.jme.math.Vector3f;/** * * @author mauro */public class GraphicsThread implements Runnable{    HelloWorld app=null;        public void run() {        app = new HelloWorld(); // Create Object        if (app!=null)            System.out.println("app is ok");        else            System.out.println("app is null creation");        app.start();    }    public void setBox(float angle, Vector3f axis){        if (app!=null)            app.setBox(angle, axis);        else            System.out.println("app is null");    }    public void setBox(Quaternion q){        if (app!=null)            app.setBox(q);        else            System.out.println("app is null");    }}`
Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

#### lestofante

#2
##### Jan 20, 2011, 11:22 pm
ho trovato un grosso bug che fa impazzire i motori, causa logica sbagliata sul calcolo della rotazione dello yaw.
poco dopo orizzonte.IMUupdate(blbablabal)

il codice corretto è:
Code: [Select]
`  //find out actual estimated yaw angular rotation. We can also use directly gyro value with a proportion :-D  float yawR = yaw-lastYaw;  lastYaw=yaw;    if (yawR<minYaw)    minYaw = yawR;    if (yawR>maxYaw)    maxYaw = yawR;  /* because a quadricopter, engine setup:      0    2   3      1  */  float engineAngle[4];  engineAngle[0]=roll+yawR;  engineAngle[1]=-roll+yawR;  engineAngle[2]=-pitch-yawR;  engineAngle[3]=pitch-yawR;`
Guida per principianti http://playground.arduino.cc/Italiano/newbie
Unoffical Telegram group https://t.me/genuino

Go Up