-
Notifications
You must be signed in to change notification settings - Fork 0
/
sweepdefblue.c
53 lines (46 loc) · 1.57 KB
/
sweepdefblue.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
#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S3, IRSensor, sensorHiTechnicIRSeeker1200)
#pragma config(Sensor, S4, IRSensor2, sensorHiTechnicIRSeeker1200)
#pragma config(Motor, mtr_S1_C1_1, leftWheels, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, rightWheels, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_1, motorARM1, tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor, mtr_S1_C2_2, motorARM2, tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Servo, srvo_S1_C3_1, clawservo1, tServoStandard)
#pragma config(Servo, srvo_S1_C3_2, clawservo2, tServoStandard)
#pragma config(Servo, srvo_S1_C3_3, sweeper, tServoNone)
#pragma config(Servo, srvo_S1_C3_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C3_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C3_6, servo6, tServoNone)
#include "util.h"
task main()
{
waitForStart();
servo[sweeper] = 169;
servo[clawservo1] = 240;
servo[clawservo2] = 240;
wait1Msec(1000);
forward(.6);
wait1Msec(600);
stopMovement();
servo[sweeper] = 0;
wait1Msec(1000);
rotateLeft();
wait1Msec(400);
forward();
wait1Msec(1900);
rotateRight();
wait1Msec(700);
forward();
wait1Msec(950);
stopMovement();
forward(.5);
wait1Msec(500);
while (true)
{
forward(-0.5);
wait1Msec(1000);
forward(0.65);
wait1Msec(1000);
}
}