-
Notifications
You must be signed in to change notification settings - Fork 0
/
2AutoIRonly.c
70 lines (51 loc) · 1.79 KB
/
2AutoIRonly.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
#pragma config(Hubs, S1, HTMotor, HTServo, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S3, GYRO, sensorAnalogInactive)
#pragma config(Sensor, S4, IR, sensorI2CCustom)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorTetrix, openLoop, reversed)
#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 "beaconDetection.h"
#include "TurnAngle.h"
#include <JoystickDriver.c>
task main()
{
//Wait for the start command//
waitForStart();
//Start getHeading.h//
HTGYROstartCal(GYRO);
StartTask(getHeading);
//Move forward until it sees the beacon or until 5 sec is up//
motor[motorD] = -50;
motor[motorE] = -50;
time1[T3] = 0;
while (atBeacon() == false && time1[T3] < 5000) {
wait1Msec(2);
}
//Turn 90 to the right//
TurnAngle(90, false);
///////////////////////
//Move forward for 1 sec//
motor[motorD] = 80;
motor[motorE] = 80;
wait1Msec(1000);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
//Move the block belt for 2.5 sec//
motor[motorC] = -100;
wait1Msec(2500);
motor[motorC] = 0;
//Move backward for 1 sec//
motor[motorD] = -50;
motor[motorE] = -50;
wait1Msec(1000);
motor[motorD] = 0;
motor[motorE] = 0;
}