-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathri3d.c
119 lines (106 loc) · 4.66 KB
/
ri3d.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
#pragma config(Sensor, dgtl1, shoot, sensorTouch)
#pragma config(Sensor, dgtl2, lift, sensorTouch)
#pragma config(Motor, port1, leftWheel2, tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor, port2, leftWheel1, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, rightWheel3, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port4, rightLift2, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, leftLift1, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port6, rightLift1, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, leftLift2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port8, rightWheel2, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port9, rightWheel1, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port10, leftWheel3, tmotorVex393_HBridge, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(0)
#pragma userControlDuration(60)
#pragma systemFile // eliminates warning for "unreferenced" functions
#include "Vex_Competition_Includes.c" //Main competition background code...do not modify!
#include "JonLib/Drivebase.h"
/*/////////////////////////////////////////////////////////////////////////////////////
/////____________/\\\\\____/\\\\\\\\\_____ /////
///// ________/\\\\////___/\\\///////\\\___ /////
///// _____/\\\///_______\///______\//\\\__ /////
///// ___/\\\\\\\\\\\______________/\\\/___ /////
///// __/\\\\///////\\\_________/\\\//_____ /////
///// _\/\\\______\//\\\_____/\\\//________ /////
///// _\//\\\______/\\\____/\\\/___________ /////
///// __\///\\\\\\\\\/____/\\\\\\\\\\\\\\\_ /////
///// ____\/////////_____\///////////////__ /////
/////__/\\\\\\\\\\\\\\\__/\\\\\\\\\\\\\\\_____/\\\\\\\________/\\\\\\\____ /////
///// _\/////////////\\\_\/////////////\\\___/\\\/////\\\____/\\\/////\\\__ /////
///// ____________/\\\/_____________/\\\/___/\\\____\//\\\__/\\\____\//\\\_ /////
///// __________/\\\/_____________/\\\/____\/\\\_____\/\\\_\/\\\_____\/\\\_ /////
///// ________/\\\/_____________/\\\/______\/\\\_____\/\\\_\/\\\_____\/\\\_ /////
///// ______/\\\/_____________/\\\/________\/\\\_____\/\\\_\/\\\_____\/\\\_ /////
///// ____/\\\/_____________/\\\/__________\//\\\____/\\\__\//\\\____/\\\__ /////
///// __/\\\/_____________/\\\/_____________\///\\\\\\\/____\///\\\\\\\/___ /////
///// _\///______________\///_________________\///////________\///////_____/////
///// RI3D Robot /////
///// Skills Code /////
///// Authors: Jonathan Damico, Griffin Tabor /////
///// Since: June 20, 2016 /////
*//////////////////////////////////////////////////////////////////////////////////////
void setLeftWheelSpeed (int speed) {
motor[leftWheel1] = speed;
motor[leftWheel2] = speed;
motor[leftWheel3] = speed;
}
void setRightWheelSpeed (int speed) {
motor[rightWheel1] = speed;
motor[rightWheel2] = speed;
motor[rightWheel3] = speed;
}
void setLiftSpeed (int speed) {
motor[leftLift1] = speed;
motor[leftLift2] = speed;
motor[rightLift1] = speed;
motor[rightLift2] = speed;
}
task intake() {
while(true) {
if(vexRT[Btn5U]) {
if(SensorValue[shoot]) {
setLiftSpeed(-40);
wait1Msec(500);
} else {
while(!SensorValue[shoot]) {
setLiftSpeed(127);
wait1Msec(25);
}
}
} else if(vexRT[Btn5D]) {
setLiftSpeed(127);
wait1Msec(700);
setLiftSpeed(0);
} else if(vexRT[Btn7U]) {
setLiftSpeed(-50);
} else if(vexRT[Btn8U]) {
while(!SensorValue[lift]) {
setLiftSpeed(-127);
wait1Msec(25);
}
} else if(SensorValue[lift]) {
setLiftSpeed(0);
} else {
setLiftSpeed(10);
}
}
}
void init () {
startTask(intake);
}
void pre_auton () {
}
task autonomous () {
init();
}
task usercontrol() {
init();
while (true) {
tankDrive(vexRT(Ch3),vexRT(Ch2), 10);
wait1Msec(25);
}
}