-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSECRET CODE.c
151 lines (125 loc) · 4.36 KB
/
SECRET CODE.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
#pragma config(Hubs, S1, HTMotor, HTServo, none, none)
#pragma config(Sensor, S2, magnet, sensorHiTechnicMagnetic)
#pragma config(Sensor, S3, IR, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4, lightSensor, sensorLightActive)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, openLoop)
#pragma config(Servo, srvo_S1_C2_1, servo1, tServoNone)
#pragma config(Servo, srvo_S1_C2_2, servo2, tServoNone)
#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
#include "JoystickDriver.c" //Include file to "handle" the Bluetooth messages.
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// initializeRobot
//
// Prior to the start of autonomous mode, you may want to perform some initialization on your robot.
// Things that might be performed during initialization include:
// 1. Move motors and servos to a preset position.
// 2. Some sensor types take a short while to reach stable values during which time it is best that
// robot is not moving. For example, gyro sensor needs a few seconds to obtain the background
// "bias" value.
//
// In many cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
void initclaw()
{
motor[motorA] = 50;
wait1Msec(1000);
motor[motorA] = 0;
}
int x = 0;
void lightsensed()
{
while(nNxtButtonPressed != 3)
{
nxtDisplayTextLine(1, "Light Value is %d", SensorValue[lightSensor]);
}
x = SensorValue[lightSensor];
}
void initializeRobot()
{
initclaw();
lightsensed();
}
/////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Main Task
//
// The following is the main code for the autonomous robot operation. Customize as appropriate for
// your specific robot.
//
// The types of things you might do during the autonomous phase (for the 2008-9 FTC competition)
// are:
//
// 1. Have the robot follow a line on the game field until it reaches one of the puck storage
// areas.
// 2. Load pucks into the robot from the storage bin.
// 3. Stop the robot and wait for autonomous phase to end.
//
// This simple template does nothing except play a periodic tone every few seconds.
//
// At the end of the autonomous period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////
task main()
{
initializeRobot();
wait1Msec(5000);
//waitForStart(); // Wait for the beginning of autonomous phase.
time1(T1) = 0;
motor[motorD] = 80;
motor[motorE] = 80;
wait1Msec(1500);
motor[motorD]= 0;
motor[motorE]= 0;
wait1Msec(500);
motor[motorD]= 40;
motor[motorE]= -40;
wait1Msec(1400); // turn 90 deg
motor[motorD]= 0;
motor[motorE]= 0;
wait1Msec(500);
motor[motorD]= 80;
motor[motorE]= 80;
wait1Msec(1000);
motor[motorD]= 0;
motor[motorE]= 0;
wait1Msec(500);
motor[motorD]= 40;
motor[motorE]= -40;
wait1Msec(1400); //turn 90 deg
motor[motorD]= 0;
motor[motorE]= 0;
wait1Msec(500);
motor[motorD]= 80;
motor[motorE]= 80;
wait1Msec(1000);
motor[motorD]= 0;
motor[motorE]= 0;
wait1Msec(500);
motor[motorD]= -40;
motor[motorE]= 40;
wait1Msec(1400); //turn 90 deg
motor[motorD]= 0;
motor[motorE]= 0;
wait1Msec(500);
motor[motorD]= 80;
motor[motorE]= 80;
wait1Msec(2500);
motor[motorD]= 0;
motor[motorE]= 0;
wait1Msec(500);
servo[servo3] = 200;
servo[servo5] = 45;
motor[motorD]= 0;
motor[motorE]= 0;
wait1Msec(500);
motor[motorD]= -80;
motor[motorE]= -80;
wait1Msec(6000);
}