-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSecond Attempt at Coding.java
88 lines (74 loc) · 1.87 KB
/
Second Attempt at Coding.java
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
import edu.cmu.ri.createlab.terk.robot.finch.Finch;
public class FollowObject {
static Finch finch = new Finch();
static boolean upsideDown = false;
static int counter = 0;
public static void followObject(){
while (upsideDown == false){
if (finch.isFinchUpsideDown()){
upsideDown = true;
stop();
}
else{
//try{Thread.sleep(1000);}catch(InterruptedException e)(System.out.println(e)); //Timer
if ((finch.isObstacleLeftSide()) && (finch.isObstacleRightSide() == false)){
moveLeft();
finch.setLED(0, 255, 0);
}
else if ((finch.isObstacleRightSide()) && (finch.isObstacleLeftSide() == false)){
moveRight();
finch.setLED(0, 255, 0);
}
else if (finch.isObstacle()==true){
moveStraight();
finch.setLED(0, 255, 0);
}
else{
stop();
}
}
}
}
public static void moveStraight(){
finch.setWheelVelocities(75, 75, 500);
finch.setLED(0, 255, 0);
finch.buzz(500, 1000);
while(finch.isObstacle() == true){
finch.stop();
finch.setLED(255, 0, 0);
counter++;
}
}
public static void moveLeft(){
finch.setWheelVelocities(50, 150, 500);
finch.setLED(0, 255, 0);
finch.buzz(500, 1000);
while(finch.isObstacleLeftSide() == true){
finch.stop();
finch.setLED(255, 0, 0);
counter++;
}
}
public static void moveRight(){
finch.setWheelVelocities(150, 50, 500);
finch.setLED(0, 255, 0);
finch.buzz(500, 1000);
while(finch.isObstacleRightSide() == true){
finch.stop();
finch.setLED(255, 0, 0);
counter++;
}
}
public static void stop(){
finch.setWheelVelocities(0, 0);
finch.setLED(255, 0, 0);
counter++;
}
public static void main(String[] args){
finch.setLED(255,0,0);
if (finch.isObstacle() && finch.isTapped()){
followObject();
}
main(args);
}
}