Go Down

Topic: [parte 6]Unire il tutto (Read 1 time) previous topic - next topic

lesto

Jan 17, 2011, 12:12 am Last 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 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;
 }

}
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

lesto

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

}


ReadSerial.java:
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 ;D
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);
   }
}


GraphicsThread.java
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");
   }
}
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

lesto

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;
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

Go Up