-
Notifications
You must be signed in to change notification settings - Fork 5
/
RedAuto1.c
127 lines (126 loc) · 4.29 KB
/
RedAuto1.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
/*----------------------------------------------------------------------------*/
/* */
/* Module: RedAuto1.c */
/* Author: Jefferson Maldonado */
/* Created: 2 May 2016 */
/* */
/* Copyright (c) VCAT Robotics 2016, All Rights Reserved */
/* */
/*----------------------------------------------------------------------------*/
/* */
/* Licensed under the Apache License, Version 2.0 (the "License"); */
/* you may not use this file except in compliance with the License. */
/* You may obtain a copy of the License at */
/* */
/* http://www.apache.org/licenses/LICENSE-2.0 */
/* */
/*----------------------------------------------------------------------------*/
#include "RedAuto1Failsafe.c"
void RedAuto1(void)
{
int Launch = 0;
Transmission(1);
BallCounter = 4;
ShootingMode = 1;
IntakeShoot(6000);//SHOOT 4 PRELOADS // * FAILSAFE POINT
ShootingMode = 4;
stage = 1;
TurnDegree(-10,40,0); // DO WE NEED FAILSAFE HERE?
delay(200);
Move(2,127);
delay(500);
clearTimer(T3);
while(SensorValue[IR1] == 1) //*FAILSAFE ?WHAT IF WE DONT SEE ANY BALLS
{
IntakeOnAuto();
Move(2,20);
if(time1[T3] > 8000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1Fail1();/////////////////////RED1FAIL1
IntakePower(0);
Break(100,10);
while(MoveDist(-1, 20,0) == 1) // DO WE NEED FAILSAFE HERE?
{
IntakeOnAuto();
}
while(MoveDist(40, 50,0) == 1) //*FAILSAFE ?WHAT IF WE ARE BLOCKED (ROBOT FINISHED ABOUT MID COURT)
{
if(FailSafeEngaged == 1)Red1Fail1();/////////////////////RED1FAIL1
IntakeOnAuto();
}
IntakePower(0);
TurnDegree(-45,127,2000); //*FAILSAFE ?WHAT IF WE ARE BLOCKED
if(FailSafeEngaged == 1)Red1FailInfinite();
delay(200);
SetRPM(2250);//LAUNCHER ON (NEXT SHOOTING RPM)
clearTimer(T3);
while(LineStatus() == 0) //*FAILSAFE AT WHAT POINT DO WE STOP MOVING? WHAT IF WE ARE OF COURSE?
{
Move(-2,30);
if(time1[T3] > 3500)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1FailInfinite();
Break(100,-20);
delay(200);
while(MoveDist(7, 30,0) == 1){} // DO WE NEED FAILSAFE HERE?
clearTimer(T3);
while(LineStatus() == 0) // TURN UNTILL IT SEES LINE (WHAT IF WE NEVER SEE LINE)
{
Move(1,22);
if(time1[T3] > 3000)
{
FailSafeEngaged = 1;
break;
}
}
if(FailSafeEngaged == 1)Red1FailInfinite();
while(LineStatus() == 1 && LineStatusOffset() == 0) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(40);
if(LineStatus() == 1 && LineStatusOffset() == 1) Launch = 1;//WE REACHED THE CROSS(MIDDLE OF FIELD SET FLAG
}
if(LineStatus() == 1 && LineStatusOffset() == 1) Launch = 1;
Break(100,20);
StopDrive();
delay(300);
if(Launch) //IF LAUNCH FLAG WAS SATISFIED EARLIER THEN WE CAN LAUNCH BALLS
{
IntakePower(127); // SHOOT MID COURT
delay(3000);
}
stage = 1;
ShootingMode = 3;
DistanceCalculator(1);
while(SensorValue[IR1] == 1 && LineStatus() == 1 && DistanceCalculator(0) < 55) //*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
}
Break(100,20);
StopDrive();
while(stage == 1) // WHAT IF TWO BALLS ARE STUCK FOREVER? TIMER?
{
IntakeOnAuto();
}
while(LineStatus() == 1 && DistanceCalculator(0) < 55)//*FAILSE WHAT IF WE GET OFF THE LINE ( WHAT IF ROBOT BLOCKS PATH AND WE ARE STILL ON THE LINE)
{
FollowLine(35);
IntakeOnAuto();
}
MoveTime(300,127);
StopDrive();
IntakePower(127);///SHOT LAST BALLS BAR SHOTS
delay(4000);
stage = 1;
///////////
KillALL();
while(1);
}