-
Notifications
You must be signed in to change notification settings - Fork 4
/
Copy pathtravel scan test2.txt
140 lines (108 loc) · 5.52 KB
/
travel scan test2.txt
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
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
/*test of move from starting point on satellite initialization circle to center of panel
initialization circle
purpose -- to determine whether scanning for panel during flight makes sense
method -- use ZRSetPositionTarget add force later if needed
assume that satellite Z position is 0 -- in game, there will be z as well
globals needed--
float movTarget[3]; //movement target
float attTarget[3]; //position of attitude target
float panel_state[4]; // to store panel position
float init_att_target[3]; //to store initial attitude target spot
float prior_target_attitude[]; // used to compare with current attitude for error report
float prior_angle_err; // used to check whether attitude is close to pointing to attTarget
float rotation_start_time; // used to sequence rotation of attitude target
*/
// void ZRUser(float * myState, float * otherState, float time) {
// copy the code below
float current_position[3];
float current_attitude[3];
float target_attitude[3]; // a unit vector pointing to the intended target
float distance_to_movTarget;
//float attitude_error[3]; //difference between attitude target set in last step and current attitude reported in degrees
float angle_a;
float angle_b;
float currdegatt[3]; //current attitude in degrees
float targdegatt[3]; //target attitude in degrees
int i;
//record current position & attitude for use in Vfunc
current_position[0] = myState[0];
current_position[1] = myState[1];
current_position[2] = myState[2];
current_attitude[0] = myState[6];
current_attitude[1] = myState[7];
current_attitude[2] = myState[8];
if (time ==0) {
for(i=0;i<4;i++) panel_state[i] = 0; // initialize panel_state global
movTarget[0] = .7*getPanelSide(); // could be another move target
movTarget[1] = 0;
movTarget[2] = 0;
//start by checking an eliptical area ~centered on the panel init circle center
attTarget[0] = .7*getPanelSide();
attTarget[1] = 0;
attTarget[2] = 0;
init_att_target[0] = 0; // used as a flag, changes to to non-zero
}
if (time > 3 && prior_angle_err < 3 && (!init_att_target[0]) ) { // should take about 10 seconds; block runs only once
//time >3 avoids false trigger
// set up next target
// find distance to far circle edge
float distance;
printf(" in center block\n");
distance = sqrt(pow((attTarget[0]-current_position[0]),2)+pow((current_position[1]+0.5),2));
// find angle to circle edge - radians
angle_a = acos((attTarget[0]-current_position[0])/distance);
//subtract 6 degrees in radians
angle_b = angle_a - 3.14159*6/180;
// find y position of target attitude
// NOTE THAT Z IS IGNORED--SHOULD BE FIXED FOR A REAL STARTING POSITION
init_att_target[1] = current_position[1]-(attTarget[0]-current_position[0])*tan(angle_b);
init_att_target[0] = 0.7*getPanelSide();
init_att_target[2] = 0; // assumes that initial satellite position z is 0
for(i=0;i<3;i++) attTarget[i] = init_att_target[i];
rotation_start_time = 0;
}
if (init_att_target[0]) { // rotate attitude target 12 degrees in YZ plane
angle_a = (rotation_start_time+1)*3.14159*12/180; // in radians
attTarget[0] = 0.7*getPanelSide(); // does not change as attitude target is moved
attTarget[1] = init_att_target[1]*cos(angle_a); //Y
attTarget[2] = init_att_target[1]*sin(angle_a); //Z
rotation_start_time++;
printf("rotating\n");
// Note that inaccurate pointing, leads and lags don't matter as long as the
// search cone is within the panel circle, and pointed at a new spot.
// Note that the attitude target is on the circle, so that satellite is in
// position to search if it arrives at center without finding the panel
}
distance_to_movTarget = Vfunc(6,current_position,movTarget,movTarget,0);
if (distance_to_movTarget < .02 && !panel_state[0]) { // got to movement target and didn't find panel
printf(" at movement target without finding panel! time is %f\n",time);
// regular panel finding routine could be flagged for start here
}
if (isPanelFound() ) {
printf("Panel Found at time = %f\n",time);
getPanelState(panel_state); // store panel state
}
if (panel_state[0]) { //panel has been found i.e. x is non-0
for(i=0;i<3;i++) movTarget[i] = panel_state[i]; // reset move target to panel
for(i=0;i<3;i++) attTarget[i] = panel_state[i]; // reset attitude target to panel
movTarget[1] *= .5; // reset move to a point inside the of the panel's location
movTarget[2] *= .5;
}
// docking movement goes here
//done every time
ZRSetPositionTarget(movTarget);
Vfunc(9,current_position,attTarget,target_attitude,1);
ZRSetAttitudeTarget(target_attitude);
// report pointing errors
if(time == 0) printf("time, x att, x tar, y att, y tar, z att, z tar,dist to movTarget,fuel left, charge left, angle err\n");
else {
float angle_err;
for(i=0;i<3;i++) currdegatt[i] = current_attitude[i]*180/3.14519; // convert to degrees
for (i=0;i<3;i++)targdegatt[i] = attTarget[i]*180/3.14159;
distance_to_movTarget = Vfunc(6,current_position,movTarget,movTarget,0); // done done again here in case target has changed
angle_err = Vfunc(8, prior_target_attitude, current_attitude,current_attitude,0);
printf(" %f, %f, %f, %f, %f, %f, %f, %f, %f, %f, %f \n",time,currdegatt[0], targdegatt[0], currdegatt[1],targdegatt[1], currdegatt[2],targdegatt[2],distance_to_movTarget,getPercentFuelRemaining(), getPercentChargeRemaining(),angle_err);
// record for next iteration
for(i=0;i<3;i++) prior_target_attitude[i] = target_attitude[i];
prior_angle_err = angle_err;
}