forked from Wrenhen/WIRED2015
-
Notifications
You must be signed in to change notification settings - Fork 0
/
WIREDPROGRAM.c
357 lines (338 loc) · 9.69 KB
/
WIREDPROGRAM.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
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
#pragma config(Motor, port2, leftwheel, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, forklift, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, eeud, tmotorServoStandard, openLoop)
#pragma config(Motor, port5, flappywing, tmotorServoStandard, openLoop)
#pragma config(Motor, port6, arm, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, eelr, tmotorServoStandard, openLoop)
#pragma config(Motor, port9, rightwheel, tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#if ( _TARGET == "VirtWorld" )
#pragma debuggerWindows("joystickSimple");
#include "JoystickDriver.c"
#endif
#define MAX_MOTOR_SPEED 127
#define DEADBAND 10
// variables
int speed_divisor ;
int flappy_wing_position ;
int last_speed_button_state ;
int forward_drive ;
int last_forward_drive_btn_state ;
void init_drive_parameters() //sets the speed of the motor
{
//sets the speed of the motor to high
speed_divisor = 1 ;
//sets the initial button states to 0
last_speed_button_state = 0 ;
// checks to see if robot is moving forward
forward_drive = 1 ;
//sets the initial forward drive button state to 0
last_forward_drive_btn_state = 0 ;
}
void change_drive_direction() //reverses the controls
{
// sets button for the forward drive button state
int current_forward_drive_btn_state = vexRT[Btn7D] ;
// if statement checking if button is down
if ( current_forward_drive_btn_state == 1 &&
last_forward_drive_btn_state == 0 )
{
// checking to see if forward drive is in use
if(forward_drive == 0)
{
//sets the forward drive to 1 switches controls
forward_drive = 1 ;
}
else
{
//sets the forward drive to 0 doesn't switch controls
forward_drive = 0 ;
}
}
// sets the last button state to the current one
last_forward_drive_btn_state = current_forward_drive_btn_state ;
}
void change_speed() //changes the motor speed
{
int current_speed_button_state ;
#if ( _TARGET == "VirtWorld" ) //virtual world
current_speed_button_state = joy1Pov(Pov0) ;
#else
current_speed_button_state = vexRT[Btn7U] ;
#endif
if( last_speed_button_state == 0 && current_speed_button_state == 1)
{
if (speed_divisor == 2 ) //changes speed of motor to slow
{
speed_divisor = 1; // changes speed of motor to fast
} else {
speed_divisor = 2; // changes speed of motor to slow
}
}
// sets last speed button to current speed button
last_speed_button_state = current_speed_button_state ;
}
void drive() { // Code for driving the robot
#if ( _TARGET == "VirtWorld" )
getJoystickSettings(joystick);
motor[port10] = joystick.joy1_y2 / speed_divisor ;
motor[port1] = -joystick.joy1_y1 / speed_divisor ;
#else
if (forward_drive == 1)
{
if ( vexRT[Ch3] > DEADBAND || vexRT[Ch3] < -DEADBAND ) {
// Controlling the left wheel using channel 3
motor[leftwheel] = vexRT[Ch3] / speed_divisor ;
} else {
motor[leftwheel] = 0 ;
}
if ( vexRT[Ch2] > DEADBAND || vexRT[Ch2] < -DEADBAND ) {
//Controlling the right wheel using channel 2
motor[rightwheel] = -vexRT[Ch2] / speed_divisor ;
} else {
motor[rightwheel] = 0 ;
}
}
else
{
if ( vexRT[Ch2] > DEADBAND || vexRT[Ch2] < -DEADBAND ) {
// Reverse the left wheel using channel 2
motor[leftwheel] = -vexRT[Ch2] / speed_divisor ;
}
else {
motor[leftwheel] = 0 ;
}
if ( vexRT[Ch3] > DEADBAND || vexRT[Ch3] < -DEADBAND ) {
// Reverse the right wheel using channel 3
motor[rightwheel] = vexRT[Ch3] / speed_divisor ;
}
else {
motor[rightwheel] = 0 ;
}
}
#endif
}
//Code for the collection subsystem
void collection() {
#if ( _TARGET == "VirtWorld")
//If Btn5U is pushed, the forklift should move up
if(joy1Btn(Btn5) == 1)
{
//Set the motor speed to MAX_MOTOR_SPEED
motor[port7] = MAX_MOTOR_SPEED;
}
//If Btn5D is pushed, the forklift should move down
else if (joy1Btn(Btn7) == 1)
{
//Set the motor speed to -MAX_MOTOR_SPEED
motor[port7] = -MAX_MOTOR_SPEED;
}
//If no button is pushed, then nothing should move
else
{
//Set the motor speed to 0
motor[port7] = 0;
}
#else
//If Btn5U is pushed, the forklift should move up
if(vexRT[Btn5U] == 1)
{
//Set the motor speed to MAX_MOTOR_SPEED
motor[forklift] = MAX_MOTOR_SPEED;
}
//If Btn5D is pushed, the forklift should move down
else if (vexRT[Btn5D] == 1)
{
//Set the motor speed to -MAX_MOTOR_SPEED
motor[forklift] = -MAX_MOTOR_SPEED;
}
//If no button is pushed, then nothing should move
else
{
//Set the motor speed to 0
motor[forklift] = 0;
}
#endif
}
void init_flappy_wing(){
flappy_wing_position = 0 ;
motor[flappywing] = flappy_wing_position;
}
// Code for the Flappy wing
void flappy_wing(){
//If Btn7R is pushed, deploy the wing
if(vexRT[Btn7R] == 1)
{
//Set the motor speed to MAX_MOTOR_SPEED
flappy_wing_position = -MAX_MOTOR_SPEED;
}
//If Btn7L is pushed, the wing should move left
else if (vexRT[Btn7L] == 1)
{
//Set the motor speed to -MAX_MOTOR_SPEED
flappy_wing_position = MAX_MOTOR_SPEED ;
}
motor[flappywing] = flappy_wing_position;
}
void arm_joint() {
//If Btn6U is pushed, the arm should move up
if(vexRT[Btn6U] == 1)
{
//Set the motor speed to MAX_MOTOR_SPEED
motor[arm] = MAX_MOTOR_SPEED;
}
//If Btn6D is pushed, the arm should move down
else if (vexRT[Btn6D] == 1)
{
//Set the motor speed to -MAX_MOTOR_SPEED
motor[arm] = -MAX_MOTOR_SPEED;
}
else
{
//Set the motor speed to 0
motor[arm] = 0;
}
}
void end_effector()
{
#if ( _TARGET == "VirtWorld")
//If Btn5U is pushed, the forklift should move up
if(joy1Btn(Btn6) == 1)
{
//Set the motor speed to MAX_MOTOR_SPEED
motor[port6] = MAX_MOTOR_SPEED;
}
//If Btn5D is pushed, the forklift should move down
else if (joy1Btn(Btn8) == 1)
{
//Set the motor speed to -MAX_MOTOR_SPEED
motor[port6] = -MAX_MOTOR_SPEED;
}
else //If no button is pushed, then nothing should move
{
//Set the motor speed to 0
motor[port6] = 0;
}
#else
#if 0
//If Btn8U is pushed, the end effector should move up
if(vexRT[Btn8U] == 1)
{
//Set the motor spped to MAX_MOTOR_SPEED
motor[eeud] = MAX_MOTOR_SPEED;
}
//If Btn8D is pushed, the end effector should move down
else if (vexRT[Btn8D] == 1)
{
//Set the motor speed to -MAX_MOTOR_SPEED
motor[eeud]= -MAX_MOTOR_SPEED;
}
else
{
//Set the motor speed 0
motor[eelr] = 0;
}
//If Btn8R is pushed, the end effector should move right
if(vexRT[Btn8R] == 1)
{
//Set the motor speed to MAX_MOTOR_SPEED
motor[eelr] = MAX_MOTOR_SPEED;
}
//If Btn8L is pushed, then the end effector should move left
else if (vexRT[Btn8L] == 1)
{
//Set the motor speed to -MAX_MOTOR_SPEED
motor[eelr]= -MAX_MOTOR_SPEED;
}
else
{
//Set the motor speed to 0
motor[eelr] = 0;
}
#endif
#endif
}
#if 0
void nudge_left() {
#if ( _TARGET == "VirtWorld")
if ( joy1Pov(Pov6) ) {
motor[port10] = MAX_MOTOR_SPEED ;
sleep(300) ;
motor[port1] = -MAX_MOTOR_SPEED ;
sleep(200) ;
motor[port1] = 0 ;
motor[port10] = -MAX_MOTOR_SPEED;
sleep(320) ;
motor[port1] = MAX_MOTOR_SPEED;
sleep(100) ;
motor[port10] = 0 ;
motor[port1] = 0 ;
}
#else
if(vexRT[Btn8L] == 1) {
motor[rightwheel] = MAX_MOTOR_SPEED ;
sleep(300) ;
motor[leftwheel] = -MAX_MOTOR_SPEED ;
sleep(200) ;
motor[leftwheel] = 0 ;
motor[rightwheel] = -MAX_MOTOR_SPEED;
sleep(320) ;
motor[leftwheel] = MAX_MOTOR_SPEED;
sleep(100) ;
motor[rightwheel] = 0 ;
motor[leftwheel] = 0 ;
}
#endif
}
void nudge_right() {
#if ( _TARGET == "VirtWorld")
if ( joy1Pov(Pov2) ) {
motor[port1] = -MAX_MOTOR_SPEED ;
sleep(300) ;
motor[port10] = MAX_MOTOR_SPEED ;
sleep(200) ;
motor[port10] = 0 ;
motor[port1] = MAX_MOTOR_SPEED;
sleep(320) ;
motor[port10] = -MAX_MOTOR_SPEED ;
sleep(100) ;
motor[port1] = 0 ;
motor[port10] = 0 ;
}
#else
if(vexRT[Btn8R] == 1) {
motor[leftwheel] = -MAX_MOTOR_SPEED ;
sleep(300) ;
motor[rightwheel] = MAX_MOTOR_SPEED ;
sleep(200) ;
motor[rightwheel] = 0 ;
motor[leftwheel] = MAX_MOTOR_SPEED;
sleep(320) ;
motor[rightwheel] = -MAX_MOTOR_SPEED ;
sleep(100) ;
motor[leftwheel] = 0 ;
motor[rightwheel] = 0 ;
}
#endif
}
#endif
task main ()
{
// call initialization functions
init_flappy_wing() ;
init_drive_parameters() ;
// loop forever
while (1)
{
// call drive routines
change_speed();
change_drive_direction();
drive();
// call collection and flappy wing routines
collection();
flappy_wing();
// call arm routines
arm_joint();
end_effector();
}
}