-
Notifications
You must be signed in to change notification settings - Fork 0
/
servotester3.c
executable file
·64 lines (47 loc) · 1.59 KB
/
servotester3.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
#pragma config(Hubs, S1, HTServo, HTMotor, none, none)
#pragma config(Sensor, S2, lightR, sensorLightActive)
#pragma config(Motor, mtr_S1_C2_1, motorD, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C2_2, motorE, tmotorNormal, openLoop)
#pragma config(Servo, srvo_S1_C1_6, servo6, tServoStandard)
#pragma config(Servo, srvo_S1_C1_5, servo5, tServoStandard)
#pragma config(Servo, srvo_S1_C1_4, servo4, tServoStandard)
#pragma config(Servo, srvo_S1_C1_3, servo3, tServoStandard)
#pragma config(Servo, srvo_S1_C1_2, servo2, tServoStandard)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
task main()
{
nMotorEncoder[motorC] = 0;
while (nMotorEncoder[motorC] > -90)
{
motor[motorE] = -30;
}
motor[motorE] = 0;
servo[servo2] = 255;
servo[servo3] = 50;
servo[servo4] = 255;
servo[servo5] = 128;
servo[servo6] = 0;
wait1Msec(10000);
servo[servo2] = 100;
servo[servo3] = 100;
servo[servo4] = 100;
servo[servo5] = 190;
servo[servo6] = 100;
wait1Msec(10000);
servo[servo2] = 255;
servo[servo3] = 50;
servo[servo4] = 255;
servo[servo5] = 128;
servo[servo6] = 0;
wait1Msec(10000);
//servo[servo2] = 0;
// servo[servo2] = 0;
//servo[servo2] = 0;
// servo[servo2] = 0;
nMotorEncoder[motorC] = 0;
while (nMotorEncoder[motorC] < 90)
{
motor[motorE] = 30;
}
motor[motorE] = 0;
}