forked from josephchan91/Robockey
/
playFxns.c
169 lines (168 loc) · 5 KB
/
playFxns.c
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
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
void playStriker(){
// GET DIR
if(check(PINB,5)){
direction=true; //SET THE ROBOT DIRECTION TO SHOOT
} else {
direction=false;
}
// GET POS
getPos();
// SEND LOCATION TO HELPER
//buffer[1]=myPos[X];
//buffer[2]=myPos[Y];
//buffer[3]=direction;
//m_rf_send(TXADDRESS, buffer, PACKET_LENGTH);
// READ ADC VALS
int adc1 = ADC0_read();
int adc2 = ADC1_read();
// m_usb_tx_string("adc1, adc2 = (");
// m_usb_tx_int(adc1);
// m_usb_tx_string(", ");
// m_usb_tx_int(adc2);
// m_usb_tx_string(")\n");
int adcDiff = adc2 - adc1;
// TIMER CHECKS
//m_usb_tx_string("myLastPos(x,y): (");
//m_usb_tx_int(myLastPos[X]);
//m_usb_tx_string(", ");
//m_usb_tx_int(myLastPos[Y]);
//m_usb_tx_string(") TIMER:");
//m_usb_tx_int(timer);
//m_usb_tx_string("\n");
if(timer>5 && timer<10){
backward();
m_wait(250);
turnLeft();
m_wait(250);
backward();
m_wait(250);
timer = 0;
} else if(hasPuck()){// ROBOT SWITCH PUSHED.. PUCK IS SEEN
m_green(OFF);
m_wait(100);
unsigned int xTarg;
unsigned int yTarg = 60;
if(direction){ // Face goal A
xTarg = 240;
} else { // Face goal B
xTarg = 0;
}
// ROTATE TO FACE GOAL
if(!facing(xTarg,yTarg)) return;
if(direction){ // Try to go to A
if(myPos[X]>160){ // Close enough, fire!
m_green(ON);
fire();
} else {
m_green(OFF); // Too far, keep going forward
forward();
}
} else { // Try to go to B
if(myPos[X]<80){ // Close enough, fire!
m_green(ON);
fire();
} else { // Too far, keep going forward
m_green(OFF);
forward();
}
}
} else if(adc1>960||adc2>960){ // PUCK SEEN, hone in..
if(adcDiff>10){
spinLeft();
} else if(adcDiff<-10){
spinRight();
} else {
forward();
}
} else{ //PUCK NOT SEEN, keep spinning..
m_green(OFF);
spinRight();
}
}
void playHelper(){
// GET DIR
if(check(PINB,5)){
direction=true; //SET THE ROBOT DIRECTION TO SHOOT
} else {
direction=false;
}
// direction = directionPacket;
// READ ADC VALS
int adc1 = ADC0_read();
int adc2 = ADC1_read();
//m_usb_tx_string("adc1, adc2 = (");
//m_usb_tx_int(adc1);
//m_usb_tx_string(", ");
//m_usb_tx_int(adc2);
//m_usb_tx_string(")\n");
int adcDiff = adc2 - adc1;
if(direction) m_usb_tx_string("direction true\n");
else m_usb_tx_string("direction false\n");
if(timer>=1){
turnLeft();
m_wait(1000);
backward();
m_wait(500);
timer = 0;
} else if(hasPuck()){// ROBOT SWITCH PUSHED.. PUCK IS SEEN
m_green(OFF);
m_wait(100);
unsigned int xTarg;
unsigned int yTarg = 60;
if(direction){ // Face goal A
xTarg = 240;
} else { // Face goal B
xTarg = 0;
}
// ROTATE TO FACE GOAL
if(!facing(xTarg,yTarg)) return;
if(direction){ // Try to go to A
if(myPos[X]>160){ // Close enough, fire!
m_green(ON);
fire();
} else {
m_green(OFF); // Too far, keep going forward
forward();
}
} else { // Try to go to B
if(myPos[X]<80){ // Close enough, fire!
m_green(ON);
fire();
} else { // Too far, keep going forward
m_green(OFF);
forward();
}
}
} else if(adc1>960||adc2>960){ // PUCK SEEN, hone in..
if(adcDiff>10){
spinLeft();
} else if(adcDiff<-10){
spinRight();
} else {
forward();
}
} else{ // PUCK NOT SEEN, keep spinning..
m_green(OFF);
spinRight();
}
}
void playGoalie(){
// READ ADC VALS
int adcBack = ADC0_read();
int adcLowerRight = ADC1_read();
int adcMid = ADC4_read();
int adcTopRight = ADC5_read();
int adcFront = ADC6_read();
m_usb_tx_string("adcBack, adcLowerRight, adcMid, adcTopRight, adcFront = (");
m_usb_tx_int(adcBack);
m_usb_tx_string(", ");
m_usb_tx_int(adcLowerRight);
m_usb_tx_string(", ");
m_usb_tx_int(adcMid);
m_usb_tx_string(", ");
m_usb_tx_int(adcTopRight);
m_usb_tx_string(", ");
m_usb_tx_int(adcFront);
m_usb_tx_string(")\n");
forward();
}