Skip to content

Commit

Permalink
New program with time graph
Browse files Browse the repository at this point in the history
  • Loading branch information
Matías con tilde committed Nov 14, 2017
1 parent 7253252 commit c3ef3e6
Show file tree
Hide file tree
Showing 4 changed files with 388 additions and 0 deletions.
207 changes: 207 additions & 0 deletions Processing/PIDgraphs/Drone.pde
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,207 @@
class Drone {
final int MIN_BOX_THRESHOLD = 50;
final int MAX_BOX_THRESHOLD = 100;
final int BOX_INCREMENT = 10;
final int MAX_TIME_LOST = 2000;

PVector currentDronePos, previousDronePos, predictionObserved;
boolean foundAtBeginning;
float lastSeen;

Knct kinect;
PID[] pids;

Drone(PApplet pa) {
currentDronePos = new PVector();
previousDronePos = new PVector();
predictionObserved = new PVector();

foundAtBeginning = false;

lastSeen = millis();

kinect = new Knct(pa);

pids = new PID[3];
//pids[0] = new PID(-0.85, 0.5, 0); // throttle
//pids[1] = new PID(0.7, 0, 0); // alante atras
//pids[2] = new PID(0.7, 0, 0); // izq der
pids[0] = new PID(pidKs[0][0], pidKs[0][1], pidKs[0][2]); // throttle
//pids[0].setMinMaxOut(50, 127);
pids[1] = new PID(pidKs[1][0], pidKs[1][1], pidKs[1][2]); // alante atras
pids[2] = new PID(pidKs[2][0], pidKs[2][1], pidKs[2][2]); // izq der
}

void update(PVector setPoint) {
if (GRAPHS) {
for (int i = 0; i < history.length; i++) {
history[i][frameCount % history[i].length] = 0;
}
}

if (!foundAtBeginning) {
currentDronePos = findAtBeginning(FIND_FIRST_THRESHOLD);

if (currentDronePos != null) {
//println("-----------------------");
foundAtBeginning = true;
predictionObserved = currentDronePos.copy();
previousDronePos = currentDronePos.copy();
}
} else {
previousDronePos = currentDronePos.copy();

// For future prediction with PID
// PVector prediction = PVector.add(predObserved, predIdeal);
// prediction.mult(0.5);

currentDronePos = searchDrone(predictionObserved);

if (millis() - lastSeen > MAX_TIME_LOST) {
foundAtBeginning = false;
}

if (GRAPHS) {
for (int i = 0; i < history.length; i++) {
history[i][frameCount % history[i].length] = Knct.realToKinect(currentDronePos).array()[i];
}
}

predictionObserved = PVector.add(currentDronePos, PVector.sub(currentDronePos, previousDronePos));

if (serial.available() > 0) {
byte[] vals = new byte[4];

vals[0] = (byte) map(pids[0].compute(currentDronePos.y, setPoint.y), -127, 127, -90, 127);
vals[1] = (byte) pids[1].compute(currentDronePos.z, setPoint.z);
vals[2] = (byte) pids[2].compute(currentDronePos.x, setPoint.x);
vals[3] = 0;

serial.clear();
serial.write(vals) ;
printArray(vals);
}

// For visualizing the prediction
// println(predictionObserved, currentDronePos, previousDronePos);
// line(currentDronePos.x, currentDronePos.y, predictionObserved.x * -100, predictionObserved.y * -100);
// noStroke();
// fill(0, 0, 255);
// ellipse(previousDronePos.x, previousDronePos.y, 8, 8);
// fill(0, 255, 0);
// ellipse(currentDronePos.x, currentDronePos.y, 8, 8);
// fill(255, 0, 0);
// ellipse(predictionObserved.x, predictionObserved.y, 8, 8);
}
}

PVector findAtBeginning(int threshold) {
int[] depth = kinect.getDepth();
PImage blobsImage = createImage(Knct.width, Knct.height, RGB);

BlobDetection blobs = new BlobDetection(Knct.width, Knct.height);
blobs.setThreshold(0.25);
blobs.setBlobMaxNumber(1);

blobsImage.loadPixels();

int averageDepth = 0, counter = 0;
for (int i = 0; i < depth.length; i++) {
if (depth[i] < threshold && depth[i] > 0) {
blobsImage.pixels[i] = color(0);
averageDepth += depth[i];
counter++;
} else {
blobsImage.pixels[i] = color(255);
}
}
if (counter > 0) averageDepth /= counter;
else averageDepth = FIND_FIRST_THRESHOLD;

blobsImage.updatePixels();
fastblur(blobsImage, 2);

blobs.computeBlobs(blobsImage.pixels);

// set(0, 0, kinect.getImage());
set(0, 0, blobsImage);
drawBlobsAndEdges(blobs, true, false);

if (blobs.getBlobNb() >= 1) {
Blob b = blobs.getBlob(0);
return Knct.kinectToReal(new PVector(b.x * Knct.width, b.y * Knct.height, averageDepth));
}
return null;
}

PVector searchDrone(PVector prediction) {
prediction = Knct.realToKinect(prediction);
int[] depth = kinect.getDepth();
PImage blobsImage = createImage(Knct.width, Knct.height, RGB);

BlobDetection blobs = new BlobDetection(blobsImage.width, blobsImage.height);
blobs.setThreshold(0.25);
blobs.setBlobMaxNumber(1);

for (int threshold = MIN_BOX_THRESHOLD; threshold < MAX_BOX_THRESHOLD; threshold += BOX_INCREMENT) {
PVector boxMin = new PVector(
max(prediction.x - threshold, 0),
max(prediction.y - threshold, 0),
prediction.z - threshold
// max(prediction.z - threshold, 0)
);
PVector boxMax = new PVector(
min(prediction.x + threshold, Knct.width),
min(prediction.y + threshold, Knct.height),
prediction.z + threshold
// min(prediction.z + threshold, 7000)
);

//PVector boxMin = new PVector(max(prediction.x - threshold, 0), max(prediction.y - threshold, 0), FIND_FIRST_THRESHOLD - threshold);
//PVector boxMax = new PVector(min(prediction.x + threshold, Knct.width), min(prediction.y + threshold, Knct.height), FIND_FIRST_THRESHOLD + threshold);

stroke(255, 0, 0);
strokeWeight(4);
noFill();
//rect(boxMin.x, boxMin.y, boxMax.x, boxMax.y);

blobsImage.loadPixels();
for (int i = 0; i < blobsImage.pixels.length; i++) blobsImage.pixels[i] = color(255);

int averageDepth = 0, counter = 0;
for (int y = int(boxMin.y); y < boxMax.y; y++) {
for (int x = int(boxMin.x); x < boxMax.x; x++) {
int i = x + y * blobsImage.width;
if (depth[i] > boxMin.z && depth[i] < boxMax.z) {
blobsImage.pixels[i] = color(0);
averageDepth += depth[i];
counter++;
} else {
blobsImage.pixels[i] = color(255);
}
}
}
if (counter > 0) averageDepth /= counter;
else averageDepth = FIND_FIRST_THRESHOLD;

blobsImage.updatePixels();
fastblur(blobsImage, 2);

blobs.computeBlobs(blobsImage.pixels);

set(0, 0, kinect.getImage());
// set(0, 0, blobsImage);
drawBlobsAndEdges(blobs, true, false);

if (blobs.getBlobNb() >= 1) {
Blob b = blobs.getBlob(0);

lastSeen = millis();
return Knct.kinectToReal(new PVector(b.x * Knct.width, b.y * Knct.height, averageDepth));
}

println(threshold);
}
return currentDronePos;
}
}
7 changes: 7 additions & 0 deletions Processing/PIDgraphs/Functions.pde
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,7 @@
import blobDetection.*;

//void fastblur(PImage img,int radius){if(radius<1)return;int w=img.width,h=img.height,wm=w-1,hm=h-1,wh=w*h,div=radius+radius+1,r[]=new int[wh],g[]=new int[wh],b[]=new int[wh],rsum,gsum,bsum,x,y,i,p,p1,p2,yp,yi,yw,vmin[]=new int[max(w,h)],vmax[]=new int[max(w,h)],dv[]=new int[256*div];int[]pix=img.pixels;for(i=0;i<256*div;i++)dv[i]=(i/div);yw=yi=0;for(y=0;y<h;y++){rsum=gsum=bsum=0;for(i=-radius;i<=radius;i++){p=pix[yi+min(wm,max(i,0))];rsum+=(p&0xff0000)>>16;gsum+=(p&0x00ff00)>>8;bsum+=p&0x0000ff;}for(x=0;x<w;x++){r[yi]=dv[rsum];g[yi]=dv[gsum];b[yi]=dv[bsum];if(y==0){vmin[x]=min(x+radius+1,wm);vmax[x]=max(x-radius,0);}p1=pix[yw+vmin[x]];p2=pix[yw+vmax[x]];rsum+=((p1&0xff0000)-(p2&0xff0000))>>16;gsum+=((p1&0x00ff00)-(p2&0x00ff00))>>8;bsum+=(p1&0x0000ff)-(p2&0x0000ff);yi++;}yw+=w;}for(x=0;x<w;x++){rsum=gsum=bsum=0;yp=-radius*w;for(i=-radius;i<=radius;i++){yi=max(0,yp)+x;rsum+=r[yi];gsum+=g[yi];bsum+=b[yi];yp+=w;}yi=x;for(y=0;y<h;y++){pix[yi]=0xff000000|(dv[rsum]<<16)|(dv[gsum]<<8)|dv[bsum];if(x==0){vmin[y]=min(y+radius+1,hm)*w;vmax[y]=max(y-radius,0)*w;}p1=x+vmin[y];p2=x+vmax[y];rsum+=r[p1]-r[p2];gsum+=g[p1]-g[p2];bsum+=b[p1]-b[p2];yi+=w;}}}
void fastblur(PImage img,int radius){if(radius<1)return;int w=img.width,h=img.height,wm=w-1,hm=h-1,wh=w*h,div=radius+radius+1,r[]=new int[wh],g[]=new int[wh],b[]=new int[wh],rsum,gsum,bsum,x,y,i,p,p1,p2,yp,yi,yw,vmin[]=new int[max(w,h)],vmax[]=new int[max(w,h)],dv[]=new int[256*div];int[]pix=img.pixels;for(i=0;i<256*div;i++)dv[i]=(i/div);yw=yi=0;for(y=0;y<h;y++){rsum=gsum=bsum=0;for(i=-radius;i<=radius;i++){p=pix[yi+min(wm,max(i,0))];rsum+=(p&0xff0000)>>16;gsum+=(p&0x00ff00)>>8;bsum+=p&0x0000ff;}for(x=0;x<w;x++){r[yi]=dv[rsum];g[yi]=dv[gsum];b[yi]=dv[bsum];if(y==0){vmin[x]=min(x+radius+1,wm);vmax[x]=max(x-radius,0);}p1=pix[yw+vmin[x]];p2=pix[yw+vmax[x]];rsum+=((p1&0xff0000)-(p2&0xff0000))>>16;gsum+=((p1&0x00ff00)-(p2&0x00ff00))>>8;bsum+=(p1&0x0000ff)-(p2&0x0000ff);yi++;}yw+=w;}for(x=0;x<w;x++){rsum=gsum=bsum=0;yp=-radius*w;for(i=-radius;i<=radius;i++){yi=max(0,yp)+x;rsum+=r[yi];gsum+=g[yi];bsum+=b[yi];yp+=w;}yi=x;for(y=0;y<h;y++){pix[yi]=0xff000000|(dv[rsum]<<16)|(dv[gsum]<<8)|dv[bsum];if(x==0){vmin[y]=min(y+radius+1,hm)*w;vmax[y]=max(y-radius,0)*w;}p1=x+vmin[y];p2=x+vmax[y];rsum+=r[p1]-r[p2];gsum+=g[p1]-g[p2];bsum+=b[p1]-b[p2];yi+=w;}}}

//void drawBlobsAndEdges(BlobDetection bD,boolean dB,boolean dE){noFill();Blob b;EdgeVertex eA,eB;for(int n=0;n<bD.getBlobNb();n++){b=bD.getBlob(n);if(b!=null){if(dE){strokeWeight(2);stroke(0,255,0);for(int m=0;m<b.getEdgeNb();m++){eA=b.getEdgeVertexA(m);eB=b.getEdgeVertexB(m);if(eA!=null&&eB!=null)line(eA.x*Knct.width,eA.y*Knct.height,eB.x*Knct.width,eB.y*Knct.height);}}if(dB){rect(b.xMin*Knct.width,b.yMin*Knct.height,b.w*Knct.width,b.h*Knct.height);}}}}
void drawBlobsAndEdges(BlobDetection bD,boolean dB,boolean dE){noFill();Blob b;EdgeVertex eA,eB;for(int n=0;n<bD.getBlobNb();n++){b=bD.getBlob(n);if(b!=null){if(dE){strokeWeight(2);stroke(0,255,0);for(int m=0;m<b.getEdgeNb();m++){eA=b.getEdgeVertexA(m);eB=b.getEdgeVertexB(m);if(eA!=null&&eB!=null)line(eA.x*Knct.width,eA.y*Knct.height,eB.x*Knct.width,eB.y*Knct.height);}}if(dB){rect(b.xMin*Knct.width,b.yMin*Knct.height,b.w*Knct.width,b.h*Knct.height);}}}}
46 changes: 46 additions & 0 deletions Processing/PIDgraphs/PID.pde
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,46 @@
class PID {
float kP, kI, kD;
float minOutput, maxOutput;
float prevError, integral;
float lastTime;

PID(float kP_, float kI_, float kD_) {
setKs(kP_, kI_, kD_);

minOutput = -127;
maxOutput = 127;

prevError = 0;
integral = 0;

lastTime = millis();
}

void setKs(float kP_, float kI_, float kD_) {
kP = kP_;
kI = kI_;
kD = kD_;
}

void setMinMaxOut(float min, float max) {
minOutput = min;
maxOutput = max;
}

float compute(float input, float setPoint) {
float now = millis();
float deltaTime = now - lastTime;

float error = setPoint - input;

integral += error * kI;
//integral = constrain(integral, ?, ?);

float output = error * kP + integral * kI + ((error - prevError) / deltaTime) * kD;

prevError = error;
lastTime = now;

return constrain(output, minOutput, maxOutput);
}
}
128 changes: 128 additions & 0 deletions Processing/PIDgraphs/PIDgraphs.pde
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,128 @@
import processing.serial.*;

final int FIND_FIRST_THRESHOLD = 1500;
final boolean GRAPHS = true;

Drone drone;
Serial serial;

float[][] history;

float[][] pidKs = {{-15, 0, 0},
/* */ {0.5, 0, 0},
/* */ {-1, 0, 0}};

void setup() {
//Unncomment the next line if GRAPHS == true
size(1024, 424, FX2D);
//Unncomment the next line if GRAPHS == false
//size(512, 424, P2D);

drone = new Drone(this);
serial = new Serial(this, Serial.list()[1]);

if (GRAPHS) history = new float[3][width - Knct.width];
}

void draw() {
background(0);
surface.setTitle(getClass().getName() + " [size " + width + "/" +height + "] [frame " + frameCount + "] [frameRate " +frameRate + "]");

drone.update(new PVector(0, 0, 1400));

//drone.foundAtBeginning = false;

if (GRAPHS) {
noStroke();
fill(255);
rect(512, 0, width, height);
noFill();

strokeWeight(3);
int[] mapMin = {0, 0, 500};
int[] mapMax = {Knct.width, Knct.height, 4500};
for (int j = 0; j < history.length; j++) {
stroke(j==0?255:0, j==1?255:0, j==2?255:0);
beginShape();
for (int i = 0; i < history[j].length; i++) {
vertex(Knct.width + i, map(history[j][(frameCount + history[j].length - i) % history[j].length], mapMin[j], mapMax[j], 0, height));
}
endShape();
}
}
}

void keyPressed() {
float pidInc = 0.5;

switch (key) {
case ' ':
drone.foundAtBeginning = false;
break;
case 'Q':
pidKs[0][0] += pidInc;
break;
case 'A':
pidKs[0][0] -= pidInc;
break;
case 'W':
pidKs[0][1] += pidInc;
break;
case 'S':
pidKs[0][1] -= pidInc;
break;
case 'E':
pidKs[0][2] += pidInc;
break;
case 'D':
pidKs[0][2] -= pidInc;
break;
case 'R':
pidKs[1][0] += pidInc;
break;
case 'F':
pidKs[1][0] -= pidInc;
break;
case 'T':
pidKs[1][1] += pidInc;
break;
case 'G':
pidKs[1][1] -= pidInc;
break;
case 'Y':
pidKs[1][2] += pidInc;
break;
case 'H':
pidKs[1][2] -= pidInc;
break;
case 'U':
pidKs[2][0] += pidInc;
break;
case 'J':
pidKs[2][0] -= pidInc;
break;
case 'I':
pidKs[2][1] += pidInc;
break;
case 'K':
pidKs[2][1] -= pidInc;
break;
case 'O':
pidKs[2][2] += pidInc;
break;
case 'L':
pidKs[2][2] -= pidInc;
break;
default:
println(key);
break;
}

drone.pids[0].setKs(pidKs[0][0], pidKs[0][1], pidKs[0][2]);
drone.pids[1].setKs(pidKs[1][0], pidKs[1][1], pidKs[1][2]);
drone.pids[2].setKs(pidKs[2][0], pidKs[2][1], pidKs[2][2]);

println(pidKs[0][0], pidKs[1][0], pidKs[2][0]);
println(pidKs[0][1], pidKs[1][1], pidKs[2][1]);
println(pidKs[0][2], pidKs[1][2], pidKs[2][2]);
}

0 comments on commit c3ef3e6

Please sign in to comment.