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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy