This repository has been archived by the owner on Jun 29, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathSensormux-Test.c
103 lines (79 loc) · 2.74 KB
/
Sensormux-Test.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
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, none)
#pragma config(Hubs, S2, HTMotor, HTServo, none, none)
#pragma config(Sensor, S3, HTSMUX, sensorI2CCustom)
#pragma config(Sensor, S4, Gyro, sensorI2CHiTechnicGyro)
#pragma config(Motor, mtr_S1_C1_1, LeftF, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C1_2, RightF, tmotorTetrix, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_1, LiftR, tmotorTetrix, PIDControl)
#pragma config(Motor, mtr_S1_C2_2, LiftF, tmotorTetrix, PIDControl, reversed)
#pragma config(Motor, mtr_S1_C3_1, Spinner, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C3_2, motorI, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S2_C1_1, LeftR, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S2_C1_2, RightR, tmotorTetrix, openLoop, reversed)
#pragma config(Servo, srvo_S2_C2_1, Clamp, tServoStandard)
#pragma config(Servo, srvo_S2_C2_2, IRlift, tServoStandard)
#pragma config(Servo, srvo_S2_C2_3, SpinLift, tServoStandard)
#pragma config(Servo, srvo_S2_C2_4, servo4, tServoNone)
#pragma config(Servo, srvo_S2_C2_5, servo5, tServoNone)
#pragma config(Servo, srvo_S2_C2_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
void HTSMUXhalt(tSensors smux) {
ubyte sendMsg[4];
sendMsg[0] = 3;
sendMsg[1] = 0x10;
sendMsg[2] = 0x20;
sendMsg[3] = 0;
sendI2CMsg(smux, sendMsg, 0);
wait1Msec(50);
}
void HTSMUXautodetect(tSensor smux) {
ubyte sendMsg[4];
sendMsg[0] = 3;
sendMsg[1] = 0x10;
sendMsg[2] = 0x20;
sendMsg[3] = 1;
sendI2CMsg(smux, sendMsg, 0);
wait1Msec(500);
}
void HTSMUXrun (tSensor smux) {
ubyte sendMsg[4];
sendMsg[0] = 3;
sendMsg[1] = 0x10;
sendMsg[2] = 0x20;
sendMsg[3] = 2;
sendI2CMsg(smux, sendMsg, 0);
}
#define HTSMUX_CMD_HALT 0x00
#define HTSMUX_CMD_AUTODETECT 0x01
#define HTSMUX_CMD_RUN 0x02
void HTSMUXsendCmd(tSensor smux, byte cmd) {
ubyte sendMsg[4];
sendMsg[0] = 3;
sendMsg[1] = 0x10;
sendMsg[2] = 0x20;
sendMsg[3] = cmd;
sendI2CMsg(smux, sendMsg, 0);
if (cmd == HTSMUX_CMD_AUTODETECT) {
wait1Msec(500);
} else if (cmd == HTSMUX_CMD_HULT) {
wait1Msec(50);
}
task main()
{
int nButtonsMask;
while (true)
nButtonsMask = SensorValue[S3];
if (nButtonsMask & 0x01) // Port 1 on the S.M.P. //
// What to do after Touch Sonsor 01 on port S3 //
{
nxtDisplayTextLine(1, "Button 1 Pressed");
}
else {
nxtDisplayTextLine(1, "Button 1 Waitting");
}
// if (IRlist & 0x02) // Port 2 on the S.M.P. //
// What to do after Touch Sonsor 02 on port S3 //
// {
// }
}
}