-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathautonoRedLeft.c
executable file
·32 lines (30 loc) · 1.19 KB
/
autonoRedLeft.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
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, none)
#pragma config(Sensor, S2, gyro, sensorI2CHiTechnicGyro)
#pragma config(Sensor, S3, htproto, sensorI2CCustom9V)
#pragma config(Motor, motorA, rightArmSpin, tmotorNormal, openLoop, encoder)
#pragma config(Motor, motorB, leftArmSpin, tmotorNormal, openLoop, encoder)
#pragma config(Motor, mtr_S1_C1_1, rightDrive, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C1_2, armClaw, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C2_1, armWrist, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C2_2, ballCollector, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C3_1, leftDrive, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C3_2, armBase, tmotorNormal, openLoop)
#define GYRO S2
#include "autonoLib.c"
#include "JoystickDriver.c"
task main()
{
initGyro();
waitForStart();
//wait1Msec(5000);
driveTo(10000);
turnDegrees(90);
resetEncoders();
driveTo(-4000, 90);
turnDegrees(60);
resetEncoders();
driveTo(-17000, 60);
resetEncoders();
driveTo(500);
holdSpinners();
}