forked from Andover-Robotics/ARC-Code
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy path11_5_14
156 lines (150 loc) · 6.92 KB
/
11_5_14
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
#pragma config(Hubs, S2, HTMotor, HTServo, HTMotor, none)
#pragma config(Sensor, S2, , sensorI2CMuxController)
#pragma config(Sensor, S3, IRSensor, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4, ColorSensor, sensorI2CHiTechnicColor)
#pragma config(Motor, mtr_S2_C1_1, L, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S2_C1_2, R, tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S2_C3_1, LinearSlide, tmotorTetrix, openLoop, encoder)
#pragma config(Motor, mtr_S2_C3_2, ArmMotor, tmotorTetrix, openLoop, encoder)
#pragma config(Servo, srvo_S2_C2_1, ArmExtend, tServoContinuousRotation)
#pragma config(Servo, srvo_S2_C2_2, ArmJoint, tServoStandard)
#pragma config(Servo, srvo_S2_C2_3, servo3, tServoNone)
#pragma config(Servo, srvo_S2_C2_4, servo4, tServoNone)
#pragma config(Servo, srvo_S2_C2_5, servo5, tServoNone)
#pragma config(Servo, srvo_S2_C2_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*
FTC 2014-15
Cascade Effect
Team 4410 ARC Lightning
Autonomous Code
Christopher Zhao
version .03, 10/27/2014
*/
#include "JoystickDriver.c" //Needed for waitForStart
//#include "hitechnic-colour-v2.h" //Color Sensor Drivers, unsure if will be used
int rampLength; //Denotes Ramp Length in inches
float wheel = 6.5*PI; //Denotes Wheel Circumference in inches
int driveTicks = 1440/wheel; //Denotes number of ticks needed to drive 1 inch
float circumference = 66 * PI; //Denotes the outer circumference that the robot will drive along in in
void clear(){ //Clears all encoders
nMotorEncoder[L] = 0; //Clears left motor
nMotorEncoder[R] = 0; //Clears right motor
nMotorEncoder[LinearSlide] = 0; //Clears linear slide motor
nMotorEncoder[ArmMotor] = 0; //Clears arm motor
}
void driveStraight(int in){ //Drives straight for desired inches (denoted by int in)
while(nMotorEncoder[L] <
driveTicks * in){ //while the encoder is less than (ticks/in) * desired inches
motor[L] = 80; //Left motor at 80 power
motor[R] = 80; //Right motor at 80 power
}
clear(); //clear all motors
wait1Msec(500); //waits for half a second
}
void turn(int deg){ //turns the robot at a specified angle
if(deg > 0){ //if turning at a positive angle
while(nMotorEncoder[R]<
((deg/360)*(driveTicks*1.5*PI))){ //while the encoder reads having not driven enough to turn this amount of degrees
motor[R] = 65; //Right motor at power 65
motor[L] = -65; //Left motor at power -65
}
}
if(deg < 0){ //if turning at a negative angle
while(nMotorEncoder[R] >
((deg/360)*(driveTicks*1.5*PI))){ //while the encoder is greater than the negative number of ticks needed to be driven to turn such an angle
motor[R] = -65; //Right motor at power -65
motor[L] = 65; //Left motor ar power 65
}
}
}
void circle(bool dir){ //Drives the robot in a circle or until it finds IR signal.
//Direction (left (true) or right (false)) based on boolean dir
if(dir == true){ //If path should opt left
while(nMotorEncoder[L] <
(driveTicks*circumference) &&
SensorValue[IRSensor] != 5){ //While the encoder hasn't circumnavigated the center goal and the IR Sensor hasn't detected anything
int r = 0; //int r is the amount added to the power of the right motor to fix the ratio
motor[L] = 88; //sets the power of the left motor to 88
motor[R] = 40 + r; //sets the power of the right motor to 40 + r
if(nMotorEncoder[L]/nMotorEncoder[R] < 2.2){ //if the ratio between the encoders is less than 2.2
r+=-1; //add -1 to power of right motor
}
if(nMotorEncoder[L]/nMotorEncoder[R] > 2.2){ //if the ratio between the encoders is greater than 2.2
r+=1; //add 1 to power of right motor
}
}
}
if(dir == false){ //if path should opt right
while(nMotorEncoder[R] <
(driveTicks*circumference) || //While the encoder hasn't circumnavigated the center goal and the IR Sensor hasn't detected anything
SensorValue[IRSensor] != 5){
int l = 0; //int l is the amount added to the power of the left motor to fix the ratio
motor[L] = 40 + l; //sets the power of the left motor to 40 + l
motor[R] = 88; //sets the power of the right motor to 88
if(nMotorEncoder[R]/nMotorEncoder[L] < 2.2){
l+=-1;
}
if(nMotorEncoder[R]/nMotorEncoder[L] > 2.2){
l+=1;
}
}
}
clear();
}
task main(){
bool taskBool[4]; //Determines whether or not a certain task gets enacted
string taskList[] =
{"Ramp", "IR", "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
if(nNxtButtonPressed == 1){ //If the left arrow is pressed
nxtDisplayCenteredTextLine(5, "True"); // Display "true" on NXT
taskBool[i] = true; // Set the decision to true
}
else{}
if(nNxtButtonPressed == 2){ //If the right arrow is pressed
nxtDisplayCenteredTextLine(5, "False"); // 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{}
}
nxtDisplayCenteredTextLine(3, "Ready"); //Once all decisions are made, set robot to ready
waitForStart(); //Wait for SamoFCS to send ready signal
switch(taskBool[0]){
case true:
driveStraight(rampLength);
break;
case false:
break;
}
wait10Msec(50);
switch(taskBool[1]){
case true:
circle(taskBool[0]);
break;
case false:
break;
}
wait10Msec(50);
switch(taskBool[2]){
case true:
break;
case false:
break;
}
wait10Msec(50);
switch(taskBool[3]){
case true:
break;
case false:
break;
}
wait10Msec(50);
}