-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathBlock and Ramp
105 lines (97 loc) · 3.1 KB
/
Block and Ramp
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
#pragma config(Hubs, S1, HTMotor, HTMotor, none, none)
#pragma config(Hubs, S3, HTServo, none, none, none)
#pragma config(Hubs, S4, HTMotor, HTMotor, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S2, INF, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S3, , sensorI2CMuxController)
#pragma config(Sensor, S4, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, yellow, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, red, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C2_1, blue, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C2_2, green, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S4_C1_1, arm, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S4_C1_2, whisk, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S4_C2_1, lift, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S4_C2_2, flag, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S3_C1_1, autoflipper, tServoStandard)
#pragma config(Servo, srvo_S3_C1_2, wrist, tServoStandard)
#pragma config(Servo, srvo_S3_C1_3, servo3, tServoNone)
#pragma config(Servo, srvo_S3_C1_4, servo4, tServoNone)
#pragma config(Servo, srvo_S3_C1_5, servo5, tServoNone)
#pragma config(Servo, srvo_S3_C1_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
void driveToInf(){
nMotorEncoder[yellow] = 0;
while(SensorValue[INF] != 5){
motor(red) = -60;
motor(yellow) = 60;
motor(green) = -60;
motor(blue) = 60;
}
motor(blue) = 0;
motor(green) = 0;
motor(yellow) = 0;
motor(red) = 0;
servo[autoflipper] = 65;
wait1Msec(300000);
}
void turnRight(int right){
nMotorEncoder[yellow] = 0;
while((nMotorEncoder[yellow] * 360) > right *7000){
motor(blue) = -50;
motor(green) = -50;
motor(yellow) = -50;
motor(red) = -50;
}
motor(blue) = 0;
motor(green) = 0;
motor(yellow) = 0;
motor(red) = 0;
}
void turnLeft(int left){
nMotorEncoder[yellow] = 0;
while((nMotorEncoder[yellow] * 360) < -left * 7000){
motor(blue) = 50;
motor(green) = 50;
motor(yellow) = 50;
motor(red) = 50;
}
motor(blue) = 0;
motor(green) = 0;
motor(yellow) = 0;
motor(red) = 0;
}
void returnto(){
time1[T1] = 0;
while(nMotorEncoder[yellow] > 0 && time1[T1] < 7000){
motor(red) = 60;
motor(yellow) = -60;
motor(green) = 60;
motor(blue) = -60;
}
}
void glide(){
time1[T1] = 0;
while(nMotorEncoder[yellow] > 0 && time1[T1] < 5000){
motor(red) = 60;
motor(yellow) = 60;
motor(green) = -60;
motor(blue) = -60;
}
}
void ramp(){
time1[T1] = 0;
while(nMotorEncoder[yellow] > 0 && time1[T1] < 7000){
motor(red) = -60;
motor(yellow) = 60;
motor(green) = -60;
motor(blue) = 60;
}
}
task main ()
{
driveToInf();
returnto();
glide();
ramp();
}