/
peanutbutter.c
56 lines (48 loc) · 1.7 KB
/
peanutbutter.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
#pragma config(Hubs, S1, HTMotor, HTServo, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, motorD, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S1_C2_1, servo1, tServoStandard)
#pragma config(Servo, srvo_S1_C2_2, servo2, tServoStandard)
#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 "JoystickDriver.c" //Include file to "handle" the Bluetooth messages.
typedef struct ins {
int c;
int d;
int mD;
int mE;
int wait;
} ins;
ins seq[4]; //CHANGEME
int seqindex = 0;
int last = 3; //CHANGEME
void dins(int i, int c, int d, int mD, int mE, int wait) {
seq[i].c = c;
seq[i].d = d;
seq[i].mD = mD;
seq[i].mE = mE;
seq[i].wait = wait;
}
task main()
{
dins(0, 150, 100, 0, 0, 100);
dins(1, 150, 100, 25, 25, 350);
//dins(2, 150, 100, 0, 25, 100);
//dins(3, 150, 100, 25, 25, 100);
waitForStart(); // Wait for the beginning of autonomous phase.
while (1) {
servo[servo1] = seq[seqindex].c;
servo[servo2] = seq[seqindex].d;
motor[motorD] = seq[seqindex].mD;
motor[motorE] = -(seq[seqindex].mE);
wait10Msec(seq[seqindex].wait);
++seqindex;
if (seqindex > last) {
break;
}
}
}