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
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143

#use "xbccamlib.ic"
#use defines.ic


void killswitch()
{       // kills the bot when either 89 seconds are up
    // or someone hits A and B at the same time.
    /* INSERT CODE: function call to botball routines */
    for(;;) {
        if(check_button(1) == 1 && check_button(2) == 1)
          {
            disable_servos();
            /* INSERT KILL FUNCTION [die()??] */
        }
    }
}

// #################### END IN-PROGRESS ################



// FUNCTIONS BEYOND  THIS POINT ARE CONSIDERED FINISHED.
// #####################################################

void raise_claw()
{
    set_servo_position(CLAW_SERVO, CLAW_POS_UP);
    sleep(CLAW_ACT_SLEEP);
}
void drop_claw()
{
    set_servo_position(CLAW_SERVO, CLAW_POS_DOWN);
    sleep(CLAW_ACT_SLEEP);
}

void claw_grab() // actually just grabs something with the claw.
{
    raise_claw(); // swings the ball claw up
    act_claw(0); //opens claw
    drop_claw(); // drops the claw. DUH!
    act_claw(1); // closes the claw
    raise_claw(); // picks up anything in the claw.
}

void demo() // actually just grabs something with the claw.
{
    forwards(1.0); // forwards backwards left right
    backwards(1.0); // these guys just move in each of those
    left(1.0); // directions for the number of seconds (the arg).
    right(1.0);
    
    claw_grab();
}

void act_claw(int dir)
{ // moves the claw based on the number passed to the function.
    // act_claw(0); opens the claw, act_claw(1); closes it.
    if(dir==1){ motor(CLAW_MOTOR, -100);
        sleep(CLAW_SLEEP);
        motor(CLAW_MOTOR, 0);
    }   
    if(dir==0){ motor(CLAW_MOTOR, 100);
        sleep(CLAW_SLEEP);
        motor(CLAW_MOTOR, 0);
    }
    sleep(0.5);
}

void forwards(float len)
{
    printf("F");    
    motor(PORT_MOTOR, 100); motor(STBD_MOTOR, 100);
    sleep(len); bk(PORT_MOTOR); bk(STBD_MOTOR);      
    ao();
}
void left(float len)
{
    printf("L");
    motor(PORT_MOTOR, 100); motor(STBD_MOTOR, -100);
    sleep(len); bk(PORT_MOTOR); bk(STBD_MOTOR);
    ao();
}
void backwards(float len)
{
    printf("B");
    motor(PORT_MOTOR, -100); motor(STBD_MOTOR, -100);
    sleep(len); bk(PORT_MOTOR); bk(STBD_MOTOR);      
    ao();
}
void right(float len)
{
    printf("R");
    motor(PORT_MOTOR, -100); motor(STBD_MOTOR, 100);
    sleep(len); bk(PORT_MOTOR); bk(STBD_MOTOR);
    ao();
}

void get_blue_ball()
// tracks and moves towards the blue ball.
// when it gets to the ball (as detected by ET)
// it will halt (HAMMERZEIT) and drop the claw.a
{
    //    while(get_et_dist() >= BLUE_BALL_DIST_TO_DROP_CLAW)
    while(1) // temp!!
      { 
        if(track_confidence(CHANNEL,INDEX) > TRACK_THRESH){
            while(cur_blob_pos < CAM_CENTER - 25) left(TRACK_ADJUST_AMT);
            while(cur_blob_pos > CAM_CENTER + 25) right(TRACK_ADJUST_AMT);
            while(cur_blob_pos>=CAM_CENTER-25&&cur_blob_pos<=CAM_CENTER+25) forwards(FWD_AMT);
        }
        
        sleep(0.1);
        
    }
    claw_grab();
}

int get_et_dist()
{
    return (255-analog(IR_SENS));
    //  distance
}

void update_camdata()
{
    init_camera();
    while(1){
        if(track_is_new_data_available()==1)
          {
            track_update();
            cur_blob_pos = track_x(CHANNEL,INDEX);
            printf("cur_blob_pos = %d\n", cur_blob_pos); // diag data
            sleep(0.25);
        }
    }
}


void initialize_servo_positions()
{
    set_servo_position(CLAW_SERVO,CLAW_POS_UP);
}