This repository has been archived by the owner on Apr 18, 2024. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 2
/
Autonomous_Maker.c
122 lines (102 loc) · 3.26 KB
/
Autonomous_Maker.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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
#pragma config(Hubs, S1, HTMotor, HTMotor, HTMotor, HTServo)
#pragma config(Sensor, S1, , sensorI2CMuxController)
#pragma config(Motor, mtr_S1_C1_1, frontLeftWheel, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C1_2, frontRightWheel, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_1, backRightWheel, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C2_2, backLeftWheel, tmotorNormal, openLoop)
#pragma config(Motor, mtr_S1_C3_1, leftArm, tmotorNormal, openLoop, reversed)
#pragma config(Motor, mtr_S1_C3_2, rightArm, tmotorNormal, PIDControl, encoder)
#pragma config(Servo, srvo_S1_C4_1, leftClaw, tServoStandard)
#pragma config(Servo, srvo_S1_C4_2, rightClaw, tServoStandard)
#pragma config(Servo, srvo_S1_C4_3, servo3, tServoNone)
#pragma config(Servo, srvo_S1_C4_4, servo4, tServoNone)
#pragma config(Servo, srvo_S1_C4_5, servo5, tServoNone)
#pragma config(Servo, srvo_S1_C4_6, servo6, tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#include "JoystickDriver.c"
task main(){
waitForStart();
int state = -1;
ClearTimer(T1);
while(true){
getJoystickSettings(joystick);
//----------Driving----------------
//Turn Right
if(joystick.joy1_TopHat==2){
motor[frontLeftWheel]=100;
motor[frontRightWheel]=0;
motor[backLeftWheel]=100;
motor[backRightWheel]=0;
if(state != 2){
AddToDatalog(2);
AddToDatalog(time1(T1));
state = 2;
ClearTimer(T1);
nxtDisplayTextLine(3, "right");
}
}
//Turn Left
else if(joystick.joy1_TopHat==6){
motor[frontLeftWheel]=0;
motor[frontRightWheel]=100;
motor[backLeftWheel]=0;
motor[backRightWheel]=100;
if(state != 6){
AddToDatalog(6);
AddToDatalog(time1(T1));
state = 6;
ClearTimer(T1);
nxtDisplayTextLine(3, "left");
}
}
//Move Forward
else if(joystick.joy1_TopHat==0){
motor[frontLeftWheel]=100;
motor[frontRightWheel]=100;
motor[backLeftWheel]=100;
motor[backRightWheel]=100;
if(state != 0){
AddToDatalog(0);
AddToDatalog(time1(T1));
state = 0;
ClearTimer(T1);
nxtDisplayTextLine(3, "up");
}
}
//Move Backward
else if(joystick.joy1_TopHat==4){
motor[frontLeftWheel]=-100;
motor[frontRightWheel]=-100;
motor[backLeftWheel]=-100;
motor[backRightWheel]=-100;
if(state != 4){
AddToDatalog(4);
AddToDatalog(time1(T1));
state = 4;
ClearTimer(T1);
nxtDisplayTextLine(3, "down");
}
}
//Stop
else{
motor[frontLeftWheel]=0;
motor[frontRightWheel]=0;
motor[backLeftWheel]=0;
motor[backRightWheel]=0;
if(state != -1){
AddToDatalog(-1);
AddToDatalog(time1(T1));
state = -1;
ClearTimer(T1);
nxtDisplayTextLine(3, "stop");
}
}
//Save
if(joy1Btn(1) == 1){
SaveNxtDatalog();
nxtDisplayTextLine(3, "save");
return;
}
//---------------------------------
}
}