/
newgostright.c
66 lines (58 loc) · 2.5 KB
/
newgostright.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
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, HTServo)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S3, GYRO, sensorI2CHiTechnicGyro)
#pragma config(Sensor, S4, HTPB, sensorI2CCustom9V)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C2_1, motorF, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_2, motorG, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C3_1, motorH, tmotorNormal, openLoop, reversed, encoder)
#pragma config(Motor, mtr_S1_C3_2, motorI, tmotorNormal, openLoop, encoder)
#pragma config(Servo, srvo_S1_C4_1, servo1, tServoStandard)
#pragma config(Servo, srvo_S1_C4_2, servo2, tServoNone)
#pragma config(Servo, srvo_S1_C4_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C4_4, servo4, tServoStandard)
#pragma config(Servo, srvo_S1_C4_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C4_6, servo6, tServoNone)
#pragma config(SrvoPosition, Position01, 234, 128, 249, 141, 58, 128, 128, 128)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*
#pragma config(Hubs, S1, HTMotor, none, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S3, GYRO, sensorI2CHiTechnicGyro)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*/
#include "common.h"
#include "HTPB-driver.h"
#include "HTGYRO-driver.h"
#include "HTAC-driver.h"
//#include "drivers/HTSMUX-driver.h"
//#include "drivers/HTGYRO-driver.h"
#include "HTEOPD-driver.h"
#include "JoystickDriver.c"
void initialize()
{
HTGYROstartCal(S3);
servo[servo1] = 0;
//servo[servo2] = 10;
servo[servo3] = 250;
servo[servo4] = 139;
servo[servo5] = 75;
servo[servo2] = 0;
}
task main()
{
initialize();
waitForStart();
motor[motorI] = 100; // set mtr_S1_C1_1 to speed 75
motor[motorH] = 100;
wait1Msec(6500);
/*
turn(90, 100);
move(1000, 100);
turn(180, 100);
move(2000, 100);
turn(-100, 100);
*/
}