-
Notifications
You must be signed in to change notification settings - Fork 1
/
Stratus deus.c
141 lines (128 loc) · 3.72 KB
/
Stratus deus.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
#pragma config(Motor, port2, frontLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port3, backLeft, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port4, frontRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port5, backRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port6, intake, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port7, lifter, tmotorVex393_MC29, openLoop)
#pragma config(Motor, port8, pooper, tmotorVex393_MC29, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)
#include "Vex_Competition_Includes.c" //Main competition background code...do not modify!
/////////////////////////////////////////////////////////////////////////////////////////
//
// Pre-Autonomous Functions
//
// You may want to perform some actions before the competition starts. Do them in the
// following function.
//
/////////////////////////////////////////////////////////////////////////////////////////
void pre_auton()
{
// Set bStopTasksBetweenModes to false if you want to keep user created tasks running between
// Autonomous and Tele-Op modes. You will need to manage all user created tasks if set to false.
bStopTasksBetweenModes = true;
// All activities that occur before the competition starts
// Example: clearing encoders, setting servo positions, ...
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// Autonomous Task
//
// This task is used to control your robot during the autonomous phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
void intaked (float x)
{
motor [intake] = x;
motor [lifter] = x;
}
void driver (int x)
{
motor[frontLeft] = x;
motor[backLeft] = x;
motor[frontRight] = -x;
motor[backRight] = -x;
}
void stopper ()
{
intaked(0);
driver(0);
}
task autonomous()
{
driver(127);
wait1Msec(8000);
driver(0);
intaked(-127);
wait1Msec(4000);
stopper();
}
/////////////////////////////////////////////////////////////////////////////////////////
//
// User Control Task
//
// This task is used to control your robot during the user control phase of a VEX Competition.
// You must modify the code to add your own robot specific commands here.
//
/////////////////////////////////////////////////////////////////////////////////////////
void setIntake (int x, int y)
{
motor[intake] = motor[lifter] = x-y;
}
task usercontrol()
{
// User control code here, inside the loop
while (true)
{
//All the floats
float xL = vexRT[Ch4];
float yL = vexRT[Ch3];
float xR = vexRT[Ch1];
float yR = vexRT[Ch2];
int deadBand = 5;
int intakeForward = vexRT[Btn6U];
int intakeBackwards = vexRT[Btn6D];
setIntake(intakeForward*75, intakeBackwards*75);
//Drive Code
motor[frontLeft] = xR + yR;
motor[backRight] = xR - yR;
motor[backLeft] = xL + yL;
motor[frontRight] = xL - yL ;
//Deadband
if(xL > deadBand || xL < -deadBand)
{
xL = xL;
}
else
{
xL = 0;
}
if(yL > deadBand || yL < -deadBand)
{
yL = yL;
}
else
{yL = 0;
}
if(xR > deadBand || xR < -deadBand)
{
xR = xR;
}
else
{xR = 0;
}
if(yR > deadBand || yR < -deadBand)
{
yR = yR;
}
else
{
yR = 0;
}
}
}