Pages: [1]   Go Down
 Author Topic: [parte 6]Unire il tutto  (Read 1198 times) 0 Members and 1 Guest are viewing this topic.
0
Offline
Shannon Member
Karma: 117
Posts: 10115
:(){:|:&};: TOX id: fcb8e918bef08581e23f6ddf9d4dba77697c25b217bf372736ed959a95fde36df5b8c5b90fbb
 « on: January 16, 2011, 06:12:45 pm » Bigger Smaller Reset

Ed ecco il risultato finale:
purtroppo non è ancora provato sul campo, infatti anche il valori in pasto al PID sono sicuramente errati
Code:
#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 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
//

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

}
 « Last Edit: January 16, 2011, 06:29:22 pm by lestofante » Logged

my Arduino code: https://github.com/lestofante/arduinoSketch
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

0
Offline
Shannon Member
Karma: 117
Posts: 10115
:(){:|:&};: TOX id: fcb8e918bef08581e23f6ddf9d4dba77697c25b217bf372736ed959a95fde36df5b8c5b90fbb
 « Reply #1 on: January 16, 2011, 06:23:48 pm » Bigger Smaller Reset

da notare
Code:
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:
package orizzonteartificiale;

import com.jme.math.Quaternion;
/**
*
* @author lesto
*/
public class Main {

public static void main(String[] args) {

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:
package orizzonteartificiale;

/**
*
* @author lesto
*/

import gnu.io.CommPort;
import gnu.io.CommPortIdentifier;
import gnu.io.SerialPort;

import java.io.IOException;
import java.io.InputStream;
import java.util.Enumeration;

boolean finito=false;
InputStream in;

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.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();
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;
while (!finito) {
try {
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:
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:
package orizzonteartificiale;

import com.jme.math.Quaternion;
import com.jme.math.Vector3f;

/**
*
* @author mauro
*/
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");
}
}
 Logged

my Arduino code: https://github.com/lestofante/arduinoSketch
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

0
Offline
Shannon Member
Karma: 117
Posts: 10115
:(){:|:&};: TOX id: fcb8e918bef08581e23f6ddf9d4dba77697c25b217bf372736ed959a95fde36df5b8c5b90fbb
 « Reply #2 on: January 20, 2011, 05:22:58 pm » Bigger Smaller Reset

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

my Arduino code: https://github.com/lestofante/arduinoSketch
sei nuovo? non sai da dove partire? leggi qui: http://playground.arduino.cc/Italiano/Newbie

 Pages: [1]   Go Up