Skip to content

P
Proken-IoT-2018B
Commit 7fbc69ea authored by Kotaro Terada's avatar Kotaro Terada
Browse files
Options

Work (2018.11.14)

parent 3f6647e4
Hide whitespace changes
Inline Side-by-side
Showing with 56 additions and 33 deletions
ARDroneForP5_Sample.pde
View file @ 7fbc69ea
...@@ -3,14 +3,16 @@ import java.util.Timer; ...@@ -3,14 +3,16 @@ import java.util.Timer;
ARDroneForP5 ardrone; ARDroneForP5 ardrone;
//OpenCV opencv;
boolean stop = false; boolean stop = false;
char state = 'n'; char state = 'n';
int start_time = 0; int start_time = 0;
float yaw_stop = 0;
void setup() { void setup() {
size(640, 360); size(640, 360);
stop = false;
ardrone=new ARDroneForP5("192.168.1.1"); ardrone = new ARDroneForP5("192.168.1.1");
// connect to the AR.Drone // connect to the AR.Drone
ardrone.connect(); ardrone.connect();
// for getting sensor information // for getting sensor information
...@@ -42,25 +44,32 @@ void draw() { ...@@ -42,25 +44,32 @@ void draw() {
float[] velocity = ardrone.getVelocity(); float[] velocity = ardrone.getVelocity();
int battery = ardrone.getBatteryPercentage(); int battery = ardrone.getBatteryPercentage();
String attitude = "pitch:" + pitch + "\nroll:" + roll + "\nyaw:" + yaw + "\naltitude:" + altitude; String attitude = "pitch:" + pitch + "\nroll:" + roll + "\nyaw:" + yaw + "\naltitude:" + altitude + "\nyaw_stop:" + yaw_stop;
text(attitude, 20, 85); text(attitude, 20, 85);
String vel = "vx:" + velocity[0] + "\nvy:" + velocity[1]; String vel = "vx:" + velocity[0] + "\nvy:" + velocity[1];
text(vel, 20, 140); text(vel, 20, 160);
String bat = "battery:" + battery + " %"; String bat = "battery:" + battery + " %";
text(bat, 20, 170); text(bat, 20, 190);
String is_stop = "stop:" + stop;
text(is_stop, 20, 210);
float vx = velocity[0], vy = velocity[1]; float vx = velocity[0], vy = velocity[1];
if(stop){ if(stop){
int lr = (int)vy / 15; int lr = (int) vy / 15;
int fb = (int)vx / 15; int fb = (int) vx / 15;
ardrone.parallel(-lr, fb); int spin = (int) (yaw - yaw_stop);
float yaw_diff = yaw_stop - yaw;
//if(yaw_diff){
//}
ardrone.move3D(-fb, lr, 0, spin);
} }
int ms = millis(); int ms = millis();
String time = "time:" + ms; String time = "time:" + ms;
text(time, 20, 210); text(time, 20, 230);
if(state=='w'){ if(state == 'w'){
square(ms); square(ms);
} }
} }
...@@ -71,41 +80,39 @@ void keyPressed() { ...@@ -71,41 +80,39 @@ void keyPressed() {
stop = false; stop = false;
if (key == CODED) { if (key == CODED) {
if (keyCode == UP) { if (keyCode == UP) {
state='f';
ardrone.forward(); // go forward ardrone.forward(); // go forward
} }
else if (keyCode == DOWN) { else if (keyCode == DOWN) {
state='b';
ardrone.backward(); // go backward ardrone.backward(); // go backward
} }
else if (keyCode == LEFT) { else if (keyCode == LEFT) {
state='l';
ardrone.goLeft(); // go left ardrone.goLeft(); // go left
} }
else if (keyCode == RIGHT) { else if (keyCode == RIGHT) {
state='r';
ardrone.goRight(); // go right ardrone.goRight(); // go right
} }
else if (keyCode == SHIFT) { else if (keyCode == SHIFT) {
ardrone.takeOff(); // take off, AR.Drone cannot move while landing ardrone.takeOff(); // take off, AR.Drone cannot move while landing
delay(2500);
stop = true; stop = true;
yaw_stop = ardrone.getYaw();
} }
else if (keyCode == CONTROL) { else if (keyCode == CONTROL) {
ardrone.landing(); // landing
state = 'n'; state = 'n';
ardrone.landing(); stop = false;
// landing
} }
} }
else { else {
if (key =='w'){ if (key =='w'){
state='w'; state = 'w';
start_time = millis(); start_time = millis();
} }
else if (key == 's') { else if (key == 's') { // hovering
ardrone.stop();
state = 's'; state = 's';
stop = true; stop = true;
ardrone.stop(); yaw_stop = ardrone.getYaw();
} }
else if (key == 'r') { else if (key == 'r') {
ardrone.spinRight(); // spin right ardrone.spinRight(); // spin right
...@@ -135,29 +142,45 @@ void keyPressed() { ...@@ -135,29 +142,45 @@ void keyPressed() {
ardrone.toggleCamera(); // set next camera setting ardrone.toggleCamera(); // set next camera setting
} }
else if (key == 'e'){ else if (key == 'e'){
ardrone.parallel(-5,5); ardrone.parallel(5, -5);
} }
} }
} }
void square(int ms){ void square(int ms){
int time = ms - start_time; int time = ms - start_time;
int speed = 10; int speed = 5;
if(time <1000){ if(time < 1000){ //hitsuyou??
stop=true; yaw_stop = ardrone.getYaw();
}else if(time < 4000){ stop = true;
stop=false; } else if(time < 4000){ //0~4s go forward for 4s
stop = false;
ardrone.forward(speed); ardrone.forward(speed);
}else if(time < 8000){ } else if(time < 7000){ //4~7s stop for 3s
//yaw_stop = ardrone.getYaw();
stop = true;
} else if (time < 11000){ //7~11s go right for 4s
stop = false;
ardrone.goRight(speed+2);
} else if(time < 14000){ //11~14s stop for 3s
//yaw_stop = ardrone.getYaw();
stop = true; stop = true;
} else if (time < 11000){ } else if(time < 18000){ //14~18s go backward for 4s
stop=false; stop = false;
ardrone.goRight(speed); ardrone.backward(speed+2);
} else if(time < 16000){ } else if(time < 21000){ //18~21s stop for 3s
//yaw_stop = ardrone.getYaw();
stop = true;
} else if(time < 25000){ //21~25s go left for 4s
stop = false;
ardrone.goLeft(speed+2);
} else if(time < 28000){ //25~28s stop for 3s
//yaw_stop = ardrone.getYaw();
stop = true; stop = true;
} else { } else { //28s~ landing
stop = false; stop = false;
state = 'n';
ardrone.landing(); ardrone.landing();
} }
} }
\ No newline at end of file
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment