Created
December 8, 2015 07:33
-
-
Save Happsson/6826d761a7a7744b5ac9 to your computer and use it in GitHub Desktop.
Kalman filter quadcopter
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/** | |
* STM32F303 | |
* RX-> PD8 | |
* TX-> PD9 | |
* Baud 9600 (standard) | |
* 0.1 | |
*/ | |
import java.lang.*; | |
import javax.swing.*; | |
import processing.serial.*; | |
import java.nio.*; | |
int[] data1, data2; | |
boolean en = false; | |
boolean go = true; | |
byte[] inByte; | |
byte[] floater = new byte[4]; | |
byte[] inByteFilter; | |
byte[] floaterFilter = new byte[4]; | |
float valueXFilter; | |
float lastXFilter; | |
float valueX; | |
float lastX; | |
int x_axis=0; | |
Serial myPort; | |
boolean draws = false; | |
//import controlP5.*; | |
//ControlP5 cp5; | |
//Textlabel myTextlabelA; | |
void setup() { | |
size(1000, 600, P3D); //Storleken på bilden. P3D krävs för att rita i 3D. | |
smooth(); | |
fill(0); | |
colorMode(RGB, 1); | |
myPort = new Serial(this, "COM4", 115200); | |
myPort.bufferUntil(0x01); | |
//surface.setResizable(true); //Makes the window resizable | |
frameRate(24); | |
thread("reader"); | |
//surface.setTitle("Sensorer"); //Renames the first window | |
//cp5 = new ControlP5(this); | |
doRedraw(); | |
} | |
void draw() { | |
if(draws) drawPlate(); | |
else drawGraph(); | |
} | |
void drawGraph(){ | |
if(x_axis == 1000){ | |
doRedraw(); | |
} | |
//while((System.currentTimeMillis() - time) % 10 != 0); | |
pushMatrix(); | |
translate(0, 300); | |
strokeWeight(2); | |
stroke(0.8, 0.30, 0.30); | |
line(++x_axis-1, lastX*10, x_axis, valueX*10); | |
stroke(0.30, 0.8, 0.30); | |
//averageX = filter(); | |
line(x_axis-1, lastXFilter*10, x_axis, valueXFilter*10); | |
lastX = valueX; | |
lastXFilter = valueXFilter; | |
popMatrix(); | |
} | |
void drawPlate(){ | |
background(0.388,0.502,0.616,0.1); | |
pushMatrix(); | |
noStroke(); | |
translate(width/2, height/2, -30); //Placera objektet till höger i bilden | |
//Rotera objektet baserat på inkommande data. | |
rotateX(radians(valueX - 90)); | |
rotateY(radians(valueXFilter)); | |
scale(120); | |
beginShape(QUADS); | |
//Rita ut kvadraten med lite sköna färgskiftningar. | |
fill(0.133, 0.271, 0.404); vertex(-1, -1, 0); | |
fill(0.133, 0.271, 0.404); vertex( 1, -1, 0); | |
fill(0.247, 0.384, 0.514); vertex( 1, 1, 0); | |
fill(0.247, 0.384, 0.514); vertex(-1, 1, 0); | |
endShape(); | |
popMatrix(); | |
} | |
void keyReleased(){ | |
switch(key){ | |
case(' '): | |
draws = !draws; | |
doRedraw(); | |
break; | |
case('p'): | |
saveFrame("line-######.png"); | |
} | |
} | |
void reader(){ | |
while(true){ | |
while (myPort.available() > 0) { | |
inByte= myPort.readBytesUntil(0x00); | |
inByteFilter= myPort.readBytesUntil(0x01); | |
if (inByte != null && inByteFilter != null) { | |
if (inByte.length ==5 && inByteFilter.length == 5) { | |
for (int i = 0; i<4; i++) { | |
floater[i] = (byte)(inByte[i]-2); | |
floaterFilter[i] = (byte)(inByteFilter[i]-2); | |
} | |
valueX = ByteBuffer.wrap(floater).order(ByteOrder.LITTLE_ENDIAN).getFloat(); | |
valueXFilter = ByteBuffer.wrap(floaterFilter).order(ByteOrder.LITTLE_ENDIAN).getFloat(); | |
// print("RAW: "+valueX+" Filter: "+valueXFilter); | |
// println(""); | |
} | |
} | |
} | |
delay(1); | |
} | |
} | |
void doRedraw() { | |
fill(0); | |
rect(0, 0, width, 1010); | |
lastX = 0; | |
x_axis = 0; | |
pushMatrix(); | |
strokeWeight(1); | |
stroke(0, 255, 0); | |
translate(0, 300); | |
for(int i = -100; i <= 100; i=i+10){ | |
if(i == 0){ | |
stroke(200,200,200); | |
}else if(i % 50 == 0){ | |
stroke(0,200,100); | |
}else{ | |
stroke(0,200,0); | |
} | |
line(0,i,1000,i); | |
} | |
popMatrix(); | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment