-
Notifications
You must be signed in to change notification settings - Fork 2
/
Copy pathmain.c
308 lines (260 loc) · 12.2 KB
/
main.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
#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, in8, ballDetect, sensorLineFollower)
#pragma config(Sensor, dgtl1, rampSolenoidA, sensorDigitalOut)
#pragma config(Sensor, dgtl2, rampSolenoidB, sensorDigitalOut)
#pragma config(Sensor, dgtl3, alignSolenoid, sensorDigitalOut)
#pragma config(Sensor, dgtl4, shootSolenoid, sensorDigitalOut)
#pragma config(Sensor, dgtl5, tournamentJumper, sensorDigitalIn)
#pragma config(Sensor, dgtl6, autonJumper, sensorDigitalIn)
#pragma config(Sensor, dgtl12, ShooterReadyLED, sensorLEDtoVCC)
#pragma config(Sensor, I2C_1, encLeftFront10, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_2, encShooterLeft7B, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_3, encLeftBack6, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_4, endBackRight5, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_5, encShooterRight2, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Sensor, I2C_6, encRightFront1, sensorQuadEncoderOnI2CPort, , AutoAssign )
#pragma config(Motor, port1, mFrontRight, tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor, port2, mShooter2, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port3, mShooter3, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port4, mShooter4, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port5, mBackRight, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port6, mBackLeft, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor, port7, mShooter7, tmotorVex393HighSpeed_MC29, openLoop, reversed)
#pragma config(Motor, port8, mShooter8, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port9, mShooter9, tmotorVex393HighSpeed_MC29, openLoop)
#pragma config(Motor, port10, mFrontLeft, tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
//Manually define most robotc-specific variables (allows this file to be edited easily in Visual Studio)
#ifdef _MSC_VER
int SensorValue[];int motor[];int vexRT[];int nLCDButtons;int nImmediateBatteryLevel;int bLCDBacklight;
typedef int bool;
#define true 1
#define false 0
typedef char* string;
typedef void task;
int bumpLeft;int bumpRight;int mShooter2;int mShooter3;int mShooter4;int mShooter7;int mShooter8;int mShooter9;int mFrontLeft;int mFrontRight;int mBackLeft;int mBackRight;int ShooterReadyLED;int encShooterRight2;int rampSolenoidA;int rampSolenoidB;int Ch1;int Ch2;int Ch3;int Ch4;int Btn5U;int Btn6D;int Btn5D;int Btn6U;int Btn7U;int Btn7D;int Btn7L;int Btn7R;int Btn8U;int Btn8D;int Btn8L;int Btn8R;int alignSolenoid;int shootSolenoid;int bIfiAutonomousMode;int bIfiRobotDisabled;int time1[];int T1;int T2;int T4;int autonJumper;int nSysTime;
#endif
#pragma platform(VEX)
//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(15)
#pragma userControlDuration(120)
#define USE_BUTTON_PLACEHOLDER
//#include "../CustomLCD.c"
#ifdef _MSC_VER
#include "C:\Program Files (x86)\Robomatter Inc\ROBOTC Development Environment 4.X\Includes\Vex_Competition_Includes.c" //Main competition background code...do not modify!
#else
#include "Vex_Competition_Includes.c"
#endif
//--------------------Button Mapping--------------------//
//Drive axis
#define joyDriveA Ch2 //First axis for the drive joysticks
#define joyDriveB Ch4 //Second axis for the drive joysticks
#define joyDriveC Ch1 //Third axis for the drive joysticks
#define joyTurnRight Btn6U
#define joyTurnLeft Btn5U
#define joyTurnRightSlow Btn6D
#define joyTurnLeftSlow Btn5D
//Shooter buttons
#define joyShooterZero Btn7L //Set the shooter power to zero (E-STOP)
#define joyShooterIncU Btn7U //Increment shooter power
#define joyShooterIncD Btn7D //Decrement the shooter power
#define joyShooterFull Btn7R //Set the shooter power to the preset target
#define joyShooterSpIU Btn8U //Increment the target speed
#define joyShooterSpID Btn8D //Decrememt the target speed
#define joyShooterSpIR Btn8R //Trigger the shooter solenoid manually
//Pneumatics Buttons
#define joyRampActivate Btn8L //Activates the ramp solenoids
//--------------------Constants--------------------//
const int ballDetectThreshold = 2525; //The threshold for the IR sensor to detect a ball being fired
const int defaultManualSpeed = 480; //What manual power we want it set to by default
const int optimalSpeed = 47.5; //The target speed we want to get the shooter to in order to make a goal
//--------------------Variables--------------------//
int lastSysTime = 0; //Stores the previous system time
float lastSpeedA = 0; //The previously measured speed for shooter encoder A
int shooterMotorRaw = 0; //stores the current set speed for the shooter motors
int lastEncA = 0; //The previous encoder count of shooter encoder A
int currentDistA = 0; //The current encoder count of shooter encoder A
float speedAverages = 0; //The calculated average of both shooter encoders
float manualSetSpeed = 0;//the manually adjusted speed
bool ready = false; //true if the shooter is within a wide margin of the target speed
int lastShootTime = 0; //stores the time inbetween the last recorded shot (ms)
float targetSpeed = optimalSpeed; //Stores the target speed the wheels are trying to achieve
//--------------------Helper Functions-------------//
//Sets all drive motors to provided powers
void setDriveMotors(int fL, int fR, int bL, int bR) {
motor[mFrontLeft] = fL;
motor[mFrontRight] = fR;
motor[mBackLeft] = bL;
motor[mBackRight] = bR;
}
//Sets all shooter motors to the provided power
void setShooterMotors(int power) {
motor[mShooter2] = (power+4)/6;
motor[mShooter3] = (power+2)/6;
motor[mShooter4] = (power+0)/6;
motor[mShooter7] = (power+5)/6;
motor[mShooter8] = (power+3)/6;
motor[mShooter9] = (power+1)/6;
}
//Measures shooter speed, calculates power, and updates LCD
void calculateShooter() {
wait1Msec(50);
lastEncA = currentDistA; //Get the prevoius speed of the shooter
currentDistA = SensorValue[encShooterRight2]; //Get the current speed of the shooter SensorValue[encShooterRight2]
int currSysTime;
//Calculate the motor speed based on the system timer and the motor distance. Average the results, weighing
//heavily on the previous value to smooth out fluctuations
float speed = ((currentDistA - lastEncA) * 50.0 / ((currSysTime = nSysTime) - lastSysTime+1));
speedAverages = speedAverages*0.9+ speed*0.1;
//Save the previous states for next iteration
lastSpeedA = speed;
lastSysTime = currSysTime;
if (speed > 80) { speed = 80; } //Clamp the aspeed to make sure it doesn't go over 80/s
else if (speed < -80) { speed = -80; } // (prevents it from generating erronously high values)
//Calculate error and add in a reversal gain if we are approaching the speed
//(prevents overshooting due to the flywheel behavior of the shooter wheels)
float error = targetSpeed - speedAverages;
if (abs(error) < 0.25) {error = 0;}
else if (abs(error) < 0.5) {error = error*-0.35;}
//Make sure the motors will spin down on their own and not generate a negative power
if (targetSpeed == 0) {error=0;}
//Calculate power based on the error. Clamp the motor output to 127
shooterMotorRaw = manualSetSpeed+ error*(2.9*6.0);
if (shooterMotorRaw <= 0) { shooterMotorRaw = 0; }
//Update the LCD
clearLCDLine(1);
ready = (speedAverages > optimalSpeed - 1 && speedAverages < optimalSpeed + 1);
bLCDBacklight = ready;
string str;
stringFormat(str, "M %-2i/%-3i",manualSetSpeed,shooterMotorRaw);
displayLCDCenteredString(1, str);
clearLCDLine(0);
string str2;
stringFormat(str2, "%2.2f/%2.2f",speedAverages,targetSpeed);
displayLCDCenteredString(0, str2);
}
//Takes manual joystick inputs to control solenoids
void solenoidsManual() {
SensorValue[shootSolenoid] = vexRT[joyShooterSpIR];
if (vexRT[joyRampActivate] ) {
SensorValue[rampSolenoidA] = 1; //Set the state of the ramp
SensorValue[rampSolenoidB] = 1; //Set the state of the ramp
shooterMotorRaw = 0; //Disable the shooter after the ramp has been deployed
setShooterMotors(0);
manualSetSpeed = 0;
targetSpeed = 0;
}
else if (!vexRT[joyRampActivate]) {
SensorValue[rampSolenoidA] = 0; //Set the state of the ramp
SensorValue[rampSolenoidB] = 0; //Set the state of the ramp
}
}
//--------------------Initalization Code--------------------//
void pre_auton() {
bStopTasksBetweenModes = false; //Set false for user tasks to run between mode switches
SensorValue[shootSolenoid] = 0;
SensorValue[rampSolenoidA] = 0;
SensorValue[rampSolenoidB] = 0;
}
//--------------------Autonomous mode--------------------//
task autonomous() {
SensorValue[shootSolenoid] = 0; //Set the initial states of variables
clearTimer(T1);
clearTimer(T3);
int state = 0;
ready = false;
speedAverages = 0;
manualSetSpeed = defaultManualSpeed;
targetSpeed = optimalSpeed-1.5;
//Run only in auton mode and if auton is enabled
while (bIfiAutonomousMode && !bIfiRobotDisabled && !SensorValue[autonJumper]) {
if (ready && state == 0) { //Robot is spinning up, 0 balls shot
SensorValue[shootSolenoid] = 1;
clearTimer(T1);
state = 1;
}else if (state == 1){ //Robot is spun up, >= 1 ball shot
targetSpeed = optimalSpeed-1
SensorValue[shootSolenoid] = 1;
clearTimer(T1);
state = 2;
}else if (state == 2 && time1[T1] > 1000 && ready) { //Robot is spun up, >= 1 ball shot
SensorValue[shootSolenoid] = 0;
state = 3;
clearTimer(T1);
}else if (state == 3 && time1[T1] > 2000) {
//SensorValue[shootSolenoid] = 1;
clearTimer(T1);
state = 1;}
if (SensorValue[ballDetect] <= ballDetectThreshold && time1[T3] > 800) {
lastShootTime = time1[T3];
writeDebugStreamLine("%i",lastShootTime);
clearTimer(T3);
}
calculateShooter(); //Calculate the shooter's speed and the motor speed
setShooterMotors(shooterMotorRaw); //set the shooter motor's speed
}
setShooterMotors(0); //Disable the shooter after auton has ended
SensorValue[shootSolenoid] = 0;
}
//--------------------Manual Control Loop--------------------//
task usercontrol() {
targetSpeed = optimalSpeed;
SensorValue[shootSolenoid] = 0; //Set the shooter to open
manualSetSpeed = defaultManualSpeed;
setShooterMotors(0);
clearTimer(T3);
while (true) {
int x = vexRT[joyDriveA];
int y = vexRT[joyDriveB];
int z = (vexRT[joyTurnRight] - vexRT[joyTurnLeft]) * 127 + (vexRT[joyTurnRightSlow] - vexRT[joyTurnLeftSlow]) * 50;
calculateShooter();
if (time1[T3] < 300) {shooterMotorRaw = 767;} //Set the power briefly to max after shooting a ball
//The global shooterMotorRaw holds the power globally. it is passed to setShooterMotors. setShooterMotors should
//accept the power and set the motors accordingly
setShooterMotors(shooterMotorRaw); //set the shooter motor's power
solenoidsManual(); //Get button innputs for solenoid control
//Basic configuration for 4 meccanum wheel drive
setDriveMotors(x + z + y,
x - z - y,
x + z - y,
x - z + y);
//bring the shooter to a full stop permanently
if (vexRT[joyShooterZero] == 1) {
shooterMotorRaw = 0;
setShooterMotors(0);
manualSetSpeed = 0;
targetSpeed = 0;
//Increment the target
}
else if (vexRT[joyShooterIncU] == 1) {
manualSetSpeed+= 1;
//Decrement the target
}
else if (vexRT[joyShooterIncD] == 1) {
manualSetSpeed-=1;
//Set the shooter speed to the optimal speed
}
else if (vexRT[joyShooterFull] == 1) {
manualSetSpeed = defaultManualSpeed;
//targetSpeed = optimalSpeed;
} //shooter button if statements
//MANAL SPEED INCREMENT
if (vexRT[joyShooterSpIU] == 1) {
targetSpeed+= 0.2;
//Decrement the target
}
else if (vexRT[joyShooterSpID] == 1) {
targetSpeed-=0.2;
//Set the shooter speed to the optimal speed
}
//Record the time since the last ball was shot
if (SensorValue[ballDetect] <= ballDetectThreshold && time1[T3] > 800) {
lastShootTime = time1[T3];
writeDebugStreamLine("%i",lastShootTime);
clearTimer(T3);
}
}
}
void frameUpdate(int frameNumber) {
}