[parte 6]Unire il tutto

[lestofante]

Ed ecco il risultato finale:
purtroppo non è ancora provato sul campo, infatti anche il valori in pasto al PID sono sicuramente errati

#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 0

Giroscopio 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 1900
InputPin 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;
  }

}

[lestofante]

da notare

    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

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);
                }
            }
        }
    }

}

ReadSerial.java:

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 ;D

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);
    }
}

GraphicsThread.java

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");
    }
}

[lestofante]

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 è:

  //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;