This repository has been archived by the owner on Jan 30, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
controller mode.c
86 lines (71 loc) · 2.38 KB
/
controller mode.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
#pragma config(Hubs, S1, HTMotor, HTMotor, HTServo, none)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Motor, motorB, , tmotorNXT, openLoop)
#pragma config(Motor, motorC, , tmotorNXT, openLoop)
#pragma config(Motor, mtr_S1_C1_1, green, tmotorTetrix, openLoop, reversed, driveRight)
#pragma config(Motor, mtr_S1_C1_2, motorE, tmotorTetrix, openLoop, driveLeft)
#pragma config(Motor, mtr_S1_C2_1, blue, tmotorTetrix, openLoop)
#pragma config(Motor, mtr_S1_C2_2, red, tmotorTetrix, openLoop)
#pragma config(Servo, srvo_S1_C3_1, servo1, tServoStandard)
#pragma config(Servo, srvo_S1_C3_2, servo2, tServoNone)
#pragma config(Servo, srvo_S1_C3_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C3_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C3_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C3_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c" //Joystick is included in this program
void moveServo (int position)
{
servo[servo1]= position ;
}
task main()
{
int threshold = 15; //to avoid unnecessary movement
while(true) //infinite loop
{
getJoystickSettings(joystick); //retrieves data from the joystick
if(abs(joystick.joy1_y1) > threshold)
{
motor[green] = joystick.joy1_y1 /2; //y1 controller moves motorD
}
else
{
motor[green] = 0;
}
if(abs(joystick.joy1_y2) > threshold)
{
motor[red] = joystick.joy1_y2 /2; //y2 controller moves motorE
}
else
{
motor[red] = 0;
}
if(joy2Btn(5) == 1 || joy1Btn(5) == 1) //If Joy2-Button5 || Joy1-Button5 is pressed:
{
moveServo (127) ;
}
if(joy2Btn(6) == 1 || joy1Btn(6) == 1) //If Joy2-Button6 || Joy1-Button6 is pressed:
{
moveServo (0);
}
if(joy2Btn(7) == 1 || joy1Btn(7) == 1) //If Joy2-Button7 || Joy1-Button7 is pressed:
{
motor[motorA] = -60;
}
else if(joy2Btn(8) == 1 || joy1Btn(8) == 1) //If Joy2-Button8 || Joy1-Button8 is pressed:
{
motor[motorA] = 60;
}
else
{motor[motorA] = 0;
}
if(abs(joystick.joy2_y2) > threshold)
{
motor[blue] = joystick.joy2_y2/2; //y2 controller moves harvester
}
else
{
motor[blue] = 0;
}
}
}