-
Notifications
You must be signed in to change notification settings - Fork 0
/
AutoRampOnlyLS.c
63 lines (47 loc) · 1.83 KB
/
AutoRampOnlyLS.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
#pragma config(Hubs, S1, HTMotor, HTServo, HTMotor, HTMotor)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S3, GYRO, sensorAnalogInactive)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C3_1, motorF, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C3_2, motorG, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C4_1, motorH, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C4_2, motorI, tmotorTetrix, openLoop)
#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 "TurnAngle.h"
task main()
{
HTGYROstartCal(GYRO);
StartTask(getHeading);
//Drive forword for 5 sec//
motor[motorD] = 100;
motor[motorE] = 100;
wait1Msec(5000);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
//Turn right 90 degrees//
TurnAngle(90, 100);
//Move forwar for 2.5 sec//
motor[motorD] = 100;
motor[motorE] = 100;
wait1Msec(2500);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
//Turn right 90 degrees//
TurnAngle(90, 100);
//Move forward for 2.2 sec//
motor[motorD] = 100;
motor[motorE] = 100;
wait1Msec(2200);
//Stop motors//
motor[motorD] = 0;
motor[motorE] = 0;
}