forked from Andover-Robotics/ARC-Code
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathAutonomousProgram
180 lines (175 loc) · 7.91 KB
/
AutonomousProgram
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
#pragma config(Hubs, S1, HTMotor, HTServo, HTMotor, none)
#pragma config(Sensor, S2, , sensorI2CMuxController)
#pragma config(Sensor, S3, IRSensor, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, L, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C1_2, R, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S1_C3_1, LinearSlide, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C3_2, ArmMotor, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo, srvo_S1_C2_1, Hook, tServoStandard)
#pragma config(Servo, srvo_S1_C2_2, ArmJoint, tServoStandard)
#pragma config(Servo, srvo_S1_C2_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C2_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C2_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C2_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard and later edited by Chris Zhao, his Majesty !!*//
/*
FTC 2014-15
Cascade Effect
Team 4410 ARC Lightning
Autonomous Code
Christopher Zhao
version .2, 12/5/2014
*/
#include "JoystickDriver.c"
float ipt = 8.25; //inches driven per 1 thousand ticks
int octagonSide = 20; //octagon around center goal has side length 20 inches
int octagonAngle = 135; //hexagon has interior angles of 120 degrees
int linearTicks = 2903; //number of ticks to raise linear slide
int armTicks = 12; //number of ticks to raise arm 1 degree
bool IRDetection = false; //denotes whether or not IR has been detected
void clear(){ //clears all encoders
nMotorEncoder[L] = 0;
nMotorEncoder[R] = 0;
nMotorEncoder[LinearSlide] = 0;
}
void driveStraight(int in){ //function which drives straight distance in
while(nMotorEncoder[L] < (in/ipt) * 1000 //while the encoders read less than (desired inches*1000 ticks/ipt)
&& in >= 0){ //on the condition that the number of inches desired is greater/equal to zero
motor[L] = 80; //run left motor at 80% power
motor[R] = 80; //run right motor at 80% power
}
while(nMotorEncoder[L] > (in/ipt) * 1000 //while the encoders read greater than (desired inches*1000 ticks/ipt)
&& in < 0){ //on the condition that the number of inches desired is less than 0)
motor[L] = -80; //run left motor in reverse at 80% power
motor[R] = -80; //run right motor in reverse at 80% power
}
motor[L] = 0; //Stops left motor
motor[R] = 0; //Stops right motor
wait10Msec(100); //waits .1 seconds
clear(); //clears encoders
}
void turn(int deg){ //function for turning # of degrees, deg
while(nMotorEncoder[R] < (deg*15.87) //while the encoder does not read the desired value of (deg/360)*(15pi in/ipt)*1000ticks
&& deg > 0){ //on the condition that the angle desired is positive
motor[R] = 70; //runs right motor at 70% power
motor[L] = -70; //runs left motor at -70% power
}
while(nMotorEncoder[L] < -(deg*15.87) //while the encoder does not read the desired value of (deg/360)*(15pi in/ipt)*1000ticks
&& deg < 0){ //on the condition that the angle desired is positive
motor[R] = -70; //runs right motor at -70% power
motor[L] = 70; //runs left motor at 70% power
}
motor[R]=0;
motor[L]=0;
clear();
}
void raiseLinearSlide(float per){
while(nMotorEncoder[LinearSlide] < per * linearTicks
&& per > 0){
motor[LinearSlide] = 80;
}
while(nMotorEncoder[LinearSlide] > per * linearTicks
&& per < 0){
motor[LinearSlide] = -80;
}
motor[LinearSlide] = 0;
clear();
}
void raiseArm(int deg){
while(nMotorEncoder[ArmMotor] < deg * armTicks
&& deg > 0){
motor[ArmMotor] = 80;
servo[ArmJoint] = nMotorEncoder[ArmMotor] / 4;
}
int cur = nMotorEncoder[ArmMotor];
while(nMotorEncoder[ArmMotor] > cur - (deg * armTicks)
&& deg < 0){
motor[ArmMotor] = -80;
servo[ArmJoint] = nMotorEncoder[ArmMotor] / 4;
}
motor[ArmMotor] = 0;
}
dumpBottle(){
servo[ArmJoint] += 180;
wait10Msec(200);
servo[ArmJoint] += -180;
wait10Msec(200);
}
/*void gui(){
bool taskBool[4]; //Determines whether or not a certain task gets enacted
string taskList[] =
{"Ramp", "IR", "Regular Tube", "Kickstand"}; //List of different tasks
int i = 0; //denotes how many tasks have been decided upon by the user
while(i < 3){ //while number of tasks decided on is less than total number of tasks
nxtDisplayCenteredTextLine(3, "%s", taskList[i]); //Display the name of task being decided on
wait10Msec(100);
if(nNxtButtonPressed == 1){ //If the left arrow is pressed
nxtDisplayCenteredTextLine(5, "Yes"); //Display "true" on NXT
taskBool[i] = true; //Set the decision to true
}
else{}
if(nNxtButtonPressed == 2){ //If the right arrow is pressed
nxtDisplayCenteredTextLine(5, "No"); //Display "false" on the NXT
taskBool[i] = false; //Set the decision to false
}
else{}
if(nNxtButtonPressed == 3){ //If the center button is pressed, confirm the decision
eraseDisplay(); //clear the display
i = i + 1; //Move on to next decision
}
else{}
}
}*/
void tRaiseArm(){
for(int i = 0; i < 7; i++){
motor[ArmMotor] = 80;
servo[ArmJoint] += -25;
wait10Msec(100);
}
motor[ArmMotor] = 0;
}
void tLowerArm(){
for(int i = 0; i < 28; i++){
motor[ArmMotor] = -80;
servo[ArmJoint] += 25;
wait1Msec(250);
}
motor[ArmMotor] = 0;
}
void initiallize(){
ServoChangeRate[ArmJoint] = 2;
ServoChangeRate[Hook] = 2;
clear();
waitForStart();
}
void IRDrive(int in){ //drives robot a set distance or until IR is detected, does not drive when IR already found
while(nMotorEncoder[L] < (in/ipt)*1000 //while the encoder is less than 1000 * desired inches/inches per thousand tick
&& IRDetection == false //and also the IR beacon has not been found
&& SensorValue[IRS] != 5){ //and also the sensor value is not equal to 5 (straight ahead)
motor[L] = 80; //Left motor drive forward
motor[R] = 80; //Right motor drive forward
}
motor[L] = 0; //stop Left motor
motor[R] = 0; //stop Right motor
if(SensorValue[IRS] == 5){ //if the IR beacon is straight ahead
IRDetection = true; //set the IR beacon to found
}
if(SensorValue[IRS] != 5){
turn(octagonAngle);
}
}
task main(){
initiallize();
tRaiseArm();
driveStraight(68);
raiseLinearSlide(1);
servo[ArmJoint]=255;
tLowerArm();
raiseLinearSlide(-0.9);
//Julian start copying here
driveStraight(-12);
turn(90);
driveStraight(36);
turn(90);
}