-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathFri-bee complete
52 lines (50 loc) · 1.04 KB
/
Fri-bee complete
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
#include <kipr/wombat.h>
int speed = 1500;
int speed_decrease = 0.8;
int servo_up = 400;
int servo_down = 1200;
int main()
{
enable_servos();
set_servo_position(0,servo_down);
create_connect();
set_create_total_angle(0);
wait_for_light(0);
while(analog(0)<3000)
{
create_drive_direct(-100,-100);
msleep(100);
}
create_drive_direct(-150,-150);
msleep(1000);
while(analog(0)<3000)
{
create_drive_direct(-100,-100);
msleep(100);
}
while(analog(0)>3000)
{
create_drive_direct(-100,-100);
msleep(200);
}
create_stop();
while(get_create_total_angle(0)<85)
{
create_drive_direct(-speed,speed);
msleep(10);
}
while(analog(2)<2500)
{
create_drive_direct(-100,-100);
msleep(100);
}
set_servo_position(0,servo_up);
msleep(500);
set_servo_position(0,servo_down);
msleep(500);
create_drive_direct(100,100);
msleep(10000);
disable_servos();
create_disconnect();
return 0;
}