-
Notifications
You must be signed in to change notification settings - Fork 0
/
Minigolf.c
79 lines (68 loc) · 2.14 KB
/
Minigolf.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
#pragma config(Sensor, S1, MSDIST, sensorI2CCustomFastSkipStates9V)
#pragma config(Sensor, S2, none, sensorNone)
#pragma config(Sensor, S3, none, sensorNone)
#pragma config(Sensor, S4, HTGYRO, sensorAnalogInactive)
#pragma config(Motor, motorA, hitMotor, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, motorB, rightMotor, tmotorNXT, PIDControl, encoder)
#pragma config(Motor, motorC, leftMotor, tmotorNXT, PIDControl, encoder)
//#pragma config(Sensor, S1, sonar, sensorSONAR) // OLD DRIVER "Distance_read.c"
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
#define _Minigolf
//const tSensors kGyroSensor = S4; // Connect to port 3. Adjust as appropriate.
//#include "Distance_read.c"
#include "DistDriver.c"
#include "GyroDriver.c"
#include "Move_turn.c"
#include "GeradeAusFahren.c"
#include "Ball_Schlagen.c"
#include "Search_Object.c"
#include "Math_func.c"
task main()
{
int tempAngle;
eraseDisplay();
wait1Msec(11);
nxtDisplayStringAt(0, 63, "Minigolf 2013");
nxtDisplayStringAt(0, 48, "GyroSensor:");
nxtDisplayStringAt(0, 40, "SonarSensor:");
//StartTask(GyroDeviceDriver);
StartTask(DistDeviceDriver);
StartTask(getHeading);
while(!_GyrodriverInitFinish) // Gyro initialisierung
{
EndTimeSlice();
}
tempAngle = (int)_fGyroAngle;
wait1Msec(4000);
while(tempAngle != (int)_fGyroAngle)
{
nxtDisplayStringAt(0, 48, "GYRO FAIL, %02d",(int)_fGyroAngle);
nxtDisplayStringAt(0, 40, "Drift after INIT");
}
searchObjekt();
wait1Msec(500);
calc_koordinaten_Ball(_ObjectAngle,_ObjectDistance+45);
wait1Msec(500);
calc_degree_hit();
wait1Msec(500);
calc_koordinaten_Drive();
wait1Msec(500);
turn2Degree(900);
wait1Msec(500);
driveDistance(_distance_X2Drive);
wait1Msec(500);
turn2Degree(0);
wait1Msec(500);
driveDistance(_distance_Y2Drive);
wait1Msec(500);
turn2Degree(_degree_Hit);
wait1Msec(500);
driveDistance(50);
wait1Msec(500);
schlagen(40,60);
while(1)
{
EndTimeSlice();
}
StopAllTasks();
}