-
Notifications
You must be signed in to change notification settings - Fork 0
/
pickTest.c
executable file
·61 lines (53 loc) · 2.75 KB
/
pickTest.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
#pragma config(Hubs, S1, MatrxRbtcs, none, none, none)
#pragma config(Hubs, S4, MatrxRbtcs, none, none, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Sensor, S3, , sensorI2CCustom)
#pragma config(Sensor, S4, , sensorI2CMuxController)
#pragma config(Motor, mtr_Matrix_S1_1, motor1, tmotorMatrix, openLoop, encoder)
#pragma config(Motor, mtr_Matrix_S1_2, motor2, tmotorMatrix, openLoop, encoder)
#pragma config(Motor, mtr_Matrix_S1_3, motor3, tmotorMatrix, openLoop, encoder)
#pragma config(Motor, mtr_Matrix_S1_4, motor4, tmotorMatrix, openLoop, encoder)
#pragma config(Motor, mtr_Matrix_S4_1, motor5, tmotorMatrix, openLoop)
#pragma config(Motor, mtr_Matrix_S4_2, motor6, tmotorMatrix, openLoop)
#pragma config(Motor, mtr_Matrix_S4_3, motor7, tmotorMatrix, PIDControl)
#pragma config(Motor, mtr_Matrix_S4_4, motor8, tmotorMatrix, PIDControl, reversed)
#pragma config(Servo, srvo_Matrix_S1_1, servo1, tServoNone)
#pragma config(Servo, srvo_Matrix_S1_2, servo2, tServoNone)
#pragma config(Servo, srvo_Matrix_S1_3, servo3, tServoNone)
#pragma config(Servo, srvo_Matrix_S1_4, servo4, tServoNone)
#pragma config(Servo, srvo_Matrix_S4_1, servo5, tServoNone)
#pragma config(Servo, srvo_Matrix_S4_2, servo6, tServoNone)
#pragma config(Servo, srvo_Matrix_S4_3, servo7, tServoNone)
#pragma config(Servo, srvo_Matrix_S4_4, servo8, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
// motor 6 is the right auxilary motor, motor5 is the left auxilary motor
// include drivers
#include "JoystickDriver.c" // Include file to handle the Bluetooth messages
#include "OmniWheelDriver.h" // Include driver for the sensor multiplexer
#include "FTCtools.h" // Include some usefull scripts
#include "hitechnic-compass.h" // Include compass sensor file
#include "hitechnic-irseeker-v2.h" // Include driver for the IR Seeker V2
#include "lego-light.h" // Include driver for the light sensors
#include "lego-touch.h" // Include driver for the touch sensors
int rMtr = motorB; //motor8 is right motor, motor7 is left motor
int lMtr = motorC;
int mode = -1;
task main()
{
while(true) {
eraseDisplay();
nxtDisplayString(0, "Btn: %d", nNxtButtonPressed);
switch(nNxtButtonPressed) {
case 3: {
motor[rMtr]=motor[lMtr]= 0;
} break;
case 1: {
motor[rMtr]=motor[lMtr]= 100;
} break;
case 2: {
motor[rMtr]=motor[lMtr]=-100;
} break;
}
wait1Msec(10);
}
}