-
Notifications
You must be signed in to change notification settings - Fork 0
/
TouchLabyrinthFunctions.c
52 lines (47 loc) · 1.23 KB
/
TouchLabyrinthFunctions.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
#pragma config(Sensor, S1, touchSensor, sensorEV3_Touch)
#pragma config(Motor, motorB, left, tmotorEV3_Large, PIDControl, encoder)
#pragma config(Motor, motorC, right, tmotorEV3_Large, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
void goForwardUntilTouch() {
//Moves forward until the wall is touched:
while (SensorValue(touchSensor) != 1) {
setMotorSpeed(left, 50);
setMotorSpeed(right, 50);
}
}
void goBackward(int x) {
//Moves backward:
setMotorSpeed(left, -50);
setMotorSpeed(right, -50);
sleep(x);
}
void turn(bool isRight) {
if (isRight == true) {
//Pin turn right:
setMotorSpeed(left, 50);
sleep(380);
return;
}
//Pin turn left:
setMotorSpeed(right, 50);
sleep(380);
}
//Goes forward at a specific power level and duration:
void goForwardCustom(int powerLevel, float ms) {
setMotorSpeed(left, powerLevel);
setMotorSpeed(right, powerLevel);
sleep(ms);
}
task main()
{
goForwardUntilTouch();
goBackward(1600);
turn(false);
goForwardUntilTouch();
goBackward(300);
turn(true);
goForwardUntilTouch();
goBackward(300);
turn(true);
goForwardCustom(50, 1500);
}