Context Navigation

source: trunk/MagicSoft/Cosy/main/MCosy.cc@ 1806

Visit:

Last change on this file since 1806 was 1803, checked in by tbretz, 23 years ago
* empty log message *
File size: 50.1 KB

Line
1	#include "MCosy.h"
2	#include "MCosy.h"
3
4	#include <iomanip.h>
5	#include <fstream.h>
6	#include <iostream.h>
7
8	#include <TROOT.h>
9	#include <TEnv.h>
10	#include <TSystem.h>
11	#include <TApplication.h>
12	#include <TTimer.h>
13
14	#include <TH2.h>
15	#include <TProfile.h>
16	#include <TCanvas.h>
17
18	#include "MGCosy.h"
19	#include "SlaStars.h"
20
21	#include "slalib/slalib.h" // FIXME: REMOVE
22
23	#include "macs.h"
24	#include "base/timer.h"
25	#include "shaftencoder.h"
26
27	//#include <sys/resource.h> // PRIO_PROCESS
28
29	ClassImp(MCosy);
30
31	typedef struct tm tm_t;
32
33	/*
34	#define GEAR_RATIO_ALT 2475.6 // [U_mot/U_tel(360deg)]
35	#define GEAR_RATIO_AZ 5891.7 // [U_mot/U_tel(360deg)]
36
37	#define RES_RE 500 // [re/U_mot]
38	#define RES_SE 16384 // [se/U_tel(360deg)]
39	*/
40	/*
41	#define GEAR_RATIO_ALT (75.55*16384/1500) // 75.25 VERY IMPORTANT! unit=U_mot/U_tel
42	#define GEAR_RATIO_AZ (179.8*16384/1500) // VERY IMPORTANT! unit=U_mot/U_tel
43	*/
44
45	//const XY kGearRatio (GEAR_RATIO_ALTRES_RE/RES_SE, GEAR_RATIO_AZRES_RE/RES_SE); //[re/se]
46	//const XY kGearRatio2(GEAR_RATIO_ALTRES_RE/360.0, GEAR_RATIO_AZRES_RE/360.0); //[re/deg]
47
48	/* +===================================+
49	FIXME: What if fMac3 (Sync) died?
50	+===================================+
51	*/
52
53	double MCosy::Rad2SE(double rad) const
54	{
55	return 16384.0/k2Pi*rad;
56	}
57
58	double MCosy::Rad2ZdRE(double rad) const
59	{
60	return 16384.0/k2PiradkGearRatio.X();
61	}
62
63	double MCosy::Rad2AzRE(double rad) const
64	{
65	return 16384.0/k2PiradkGearRatio.Y();
66	}
67
68	double MCosy::Deg2ZdRE(double rad) const
69	{
70	return rad*kGearRatio2.X();
71	}
72
73	double MCosy::Deg2AzRE(double rad) const
74	{
75	return rad*kGearRatio2.Y();
76	}
77
78	ZdAz MCosy::CorrectTarget(const ZdAz &src, const ZdAz &dst)
79	{
80	// CorrectTarget [se]
81
82	// src [se]
83	// dst [rad]
84
85	// fAltMax = 70
86	// fAltMin = -105/110
87	// fAzMin = -355
88	// fAzMax = 355
89
90	ZdAz source = src * 360.0/16384.0;
91	ZdAz dest = dst * kRad2Deg;
92
93	if (dest.Zd()>-3 && dest.Zd()<3)
94	dest.Zd(dest.Zd()<0?-3:3);
95
96	if (dest.Zd()>-1e-6 && dest.Zd()<1e-6)
97	return dst*(16384.0/k2Pi);
98
99	const float fZdMin = -67;
100	const float fZdMax = 67;
101	const float fAzMin = -29;
102	const float fAzMax = 423;
103
104	//
105	// This corrects to target for the shortest distance, not for the fastest move!
106	//
107	ZdAz s = source-dest;
108
109	float min = s.Sqr();
110
111	//
112	// Is it enought to search inside one revolution?
113	//
114	ZdAz ret = dest;
115
116	for (int i=-5; i<5+1; i++)
117	{
118	const ZdAz p(i%2 ? -dest.Zd() : dest.Zd(), dest.Az() - i*180);
119
120	//
121	// Range Check
122	//
123	if (p.Zd()<fZdMin \|\| p.Zd()>fZdMax)
124	continue;
125
126	if (p.Az()<fAzMin \|\| p.Az()>fAzMax)
127	continue;
128
129	//
130	// Calculate distance
131	//
132	s = source-p;
133
134	const float dist = s.Sqr();
135
136	if (dist > min)
137	continue;
138
139	//
140	// New shortest distance
141	//
142	ret = p;
143	min = dist;
144	}
145	return ret*(16384.0/360.0);
146	}
147
148	// --------------------------------------------------------------------------
149	//
150	// GetSePos, reads the Shaftencoder positions from the Can-drivers
151	// for the shaftencoders. The two shaft encoders at the elevation axis
152	// are avaraged. The values are returned as a ZdAz object.
153	//
154	// If one of the two shaftencoders on the elevation axis is missing
155	// the other one's position is returned.
156	//
157	// The positions are alway up-to-date because the shaftencoders are
158	// sending all changes immediatly.
159	//
160	ZdAz MCosy::GetSePos()
161	{
162	const int pa = fAz->GetPos();
163	if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
164	return ZdAz(0, pa);
165
166	//
167	// Get the values
168	//
169	int p1 = (fZd1->GetPos()+8192)%16384;
170	int p2 = -(fZd2->GetPos()+8192)%16384;
171
172	if (fZd1->IsZombieNode())
173	return ZdAz(p2, pa);
174	if (fZd2->IsZombieNode())
175	return ZdAz(p1, pa);
176
177	//
178	// interpolate shaft encoder positions
179	//
180	float p = (float)(p1+p2)/2;
181
182	return ZdAz(p, pa);
183	}
184
185	// --------------------------------------------------------------------------
186	//
187	// request the current positions from the rotary encoders.
188	// use GetRePos to get the psotions. If the request fails the function
189	// returns kFALSE, otherwise kTRUE
190	//
191	Bool_t MCosy::RequestRePos()
192	{
193	//
194	// Send request
195	//
196	fMac2->RequestSDO(0x6004);
197	fMac1->RequestSDO(0x6004);
198
199	//
200	// Wait until the objects are received.
201	//
202	fMac2->WaitForSdo(0x6004);
203	fMac1->WaitForSdo(0x6004);
204
205	// FIXME, what when waiting times out (Zombie)
206	// WaitForSdos();
207
208	//
209	// If waiting was not interrupted everything is ok. return.
210	//
211	if (!(Break() \|\| HasError()) && !HasZombie())
212	return kTRUE;
213
214	//
215	// If the waiting was interrupted due to a network error,
216	// print some logging message.
217	//
218	if (HasError())
219	lout << "Error while requesting re pos from Macs (SDO #6004)" << endl;
220
221	return kFALSE;
222	}
223
224	// --------------------------------------------------------------------------
225	//
226	// reads the Rotary encoder positions from the last request of the Macs.
227	//
228	// The positions are returned as a ZdAz object. Use RequestRePos to request
229	// the current positions first.
230	//
231	ZdAz MCosy::GetRePos()
232	{
233	return ZdAz(fMac2->GetPos(), fMac1->GetPos());
234	}
235
236	// --------------------------------------------------------------------------
237	//
238	// reads the Rotary encoder positions from the Macs.
239	//
240	// The positions are returned as a ZdAz object. The positions are the ones
241	// which are send as PDOs to the computer. This is done at a given
242	// frequency. Which means, that this positions are not ought to be
243	// up-to-date.
244	//
245	ZdAz MCosy::GetRePosPdo()
246	{
247	return ZdAz(fMac2->GetPdoPos(), fMac1->GetPdoPos());
248	}
249
250	// --------------------------------------------------------------------------
251	//
252	// set the velocity and accelerations for position maneuvers.
253	//
254	// The acceleratin is set as given (in percent of maximum).
255	// The velocity is given in percent, depending on the ratio (<1 or >1)
256	// one of the axis becomes a slower velocity. This is used for maneuvers
257	// in which both axis are moved synchromously and should reach their
258	// target position at the same time.
259	//
260	void MCosy::SetPosVelocity(const Float_t ratio, Float_t vel, Float_t acc)
261	{
262	//
263	// Set velocities
264	//
265	const int vr = fMac1->GetVelRes();
266
267	vel *= vr;
268	acc *= vr;
269
270	if (ratio <1)
271	{
272	fMac1->SetVelocity(vel);
273	fMac2->SetVelocity(vel*ratio);
274
275	fMac1->SetAcceleration(acc);
276	fMac2->SetAcceleration(acc*ratio);
277
278	fMac1->SetDeceleration(acc);
279	fMac2->SetDeceleration(acc*ratio);
280	}
281	else
282	{
283	fMac1->SetVelocity(vel/ratio);
284	fMac2->SetVelocity(vel);
285
286	fMac1->SetAcceleration(acc/ratio);
287	fMac2->SetAcceleration(acc);
288
289	fMac1->SetDeceleration(acc/ratio);
290	fMac2->SetDeceleration(acc);
291	}
292	}
293
294	// --------------------------------------------------------------------------
295	//
296	// Does a relative positioning.
297	//
298	// The steps to move are given in a ZdAz object relative to the current
299	// position. The coordinates are given in Roteryencoder steps.
300	// Axis 1 is moved only if axe1==kTRUE, Axis 2 is moved only
301	// if Axis 2==kTRUE. The function waits for the movement to be finished.
302	//
303	void MCosy::DoRelPos(const ZdAz &rd, const Bool_t axe1, const Bool_t axe2)
304	{
305	if (HasZombie())
306	return;
307	SetStatus(MCosy::kMoving);
308
309	if (axe1) fMac2->StartRelPos(rd.Zd());
310	if (axe2) fMac1->StartRelPos(rd.Az());
311
312	cout << "Waiting for positioning..." << flush;
313
314	if (axe1) fMac2->WaitForSdo(0x6004, 1);
315	if (axe2) fMac1->WaitForSdo(0x6004, 1);
316
317	WaitForEndMovement();
318
319	cout << "done." << endl;
320	}
321
322	// --------------------------------------------------------------------------
323	//
324	// check for a break-signal (from the msgqueue) and errors.
325	//
326	int MCosy::StopWaitingForSDO() const
327	{
328	return 0/Break() \|\| HasError()/;
329	}
330
331	// --------------------------------------------------------------------------
332	//
333	// Waits for a movement to become finished.
334	//
335	// First waits for all peding Sdos, then waits until both motors are stopped
336	// or waiting for SDOs was stopped (either by an error or by Break)
337	//
338	void MCosy::WaitForEndMovement()
339	{
340	// FIXME, what when waiting times out (Zombie)
341	while ((fMac1->IsPositioning() \|\| fMac2->IsPositioning()) &&
342	!(Break() \|\| HasError()) && !HasZombie())
343	usleep(1);
344	}
345
346	// --------------------------------------------------------------------------
347	//
348	// Check for an error...
349	//
350	// This is ment for usage after the Action: All Motors Stop.
351	//
352	void MCosy::CheckForError()
353	{
354	//
355	// Check all Can-Nodes for an Error. If there is no error the motor
356	// status is set to stopped.
357	//
358	if (!HasError())
359	{
360	SetStatus(MCosy::kStopped);
361	return;
362	}
363
364	//
365	// If there is an error, the error status is set to Error.
366	//
367	SetStatus(MCosy::kError);
368
369	/*
370	FIXME: HANDLINGE ERROR
371
372	//
373	// Now try to handle the error.
374	//
375	fMac1->HandleError();
376	fMac2->HandleError();
377
378	//
379	// If the error couldn't get solved return
380	//
381	if (HasError())
382	return;
383
384	//
385	// Set motor status to stopped
386	//
387	SetStatus(MCosy::kStopped);
388	*/
389	}
390
391	// --------------------------------------------------------------------------
392	//
393	// Move the telescope to the given position. The position must be given in
394	// a ZdAz object in rad.
395	//
396	// The first positioning is done absolutely. If we didn't reach the
397	// correct psotion we try to correct for this by 10 relative position
398	// maneuvers. If this doesn't help positioning failed.
399	//
400	// As a reference the shaftencoder values are used.
401	//
402	int MCosy::SetPosition(const ZdAz &dst) // [rad]
403	{
404	// FIXME: Correct by fOffset ?
405
406	const ZdAz d = dst*kRad2Deg;
407
408	lout << "Target Position: " << d.Zd() << "deg, " << d.Az() << "deg (Zd/Az)" << endl;
409
410	if (d.Zd()<fMin.Zd() \|\| d.Zd()>fMax.Zd())
411	{
412	lout << "ERROR: Requested Zenith Angle (" << d.Zd() << "deg) not ";
413	lout << "inside allowed range." << endl;
414	return kFALSE;
415	}
416
417	if (d.Az()<fMin.Az() \|\| d.Az()>fMax.Az())
418	{
419	lout << "ERROR: Requested Azimuth Angle (" << d.Az() << "deg) not ";
420	lout << "inside allowed range." << endl;
421	return kFALSE;
422	}
423
424
425	/*
426	if (fMac1->IsZombieNode() \|\| fMac2->IsZombieNode())
427	{
428	cout << "SetPosition: No connection to at least one of the MACS!" << endl;
429	return TRUE;
430	}
431	*/
432
433	//
434	// Make sure that the motors are in sync mode (necessary if the
435	// MACS has been rebooted from a Zombie state.
436	//
437	//InitSync();
438	//if (fMac3->IsZombieNode())
439	// return false;
440
441	//
442	// Calculate new target position (shortest distance to go)
443	//
444	const ZdAz src = GetSePos(); // [se]
445
446	//
447	// Because we agreed on I don't search for the shortest move
448	// anymore
449	//
450	// const ZdAz dest = CorrectTarget(src, dst);
451	//
452	const ZdAz dest = fBending(dst)*16384/2/TMath::Pi(); // [se]
453	fZdAzSoll = dst;
454
455	cout << "Source Zd: " << src.Zd() << "se Az:" << src.Az() << "se" << endl;
456	cout << "Destination Zd: " << Rad2SE(dst.Zd()) << "se Az:" << Rad2SE(dst.Az()) << "se" << endl;
457	cout << "Bend'd Dest Zd: " << Rad2SE(fZdAzSoll.Zd()) << "se Az:" << Rad2SE(fZdAzSoll.Az()) << "se" << endl;
458	cout << "Shortest Dest Zd: " << dest.Zd() << "se Az:" << dest.Az() << "se" << endl;
459
460	int i;
461	for (i=0; i<10 && !(Break() \|\| HasError()) && !HasZombie(); i++)
462	{
463
464	lout << "Step #" << i << endl;
465	//
466	// Get Shaft Encoder Positions
467	//
468	const ZdAz p=GetSePos();
469
470	//
471	// calculate control deviation and rounded cd
472	//
473	ZdAz rd = dest-p; // [se]
474
475	ZdAz cd = rd; // [se]
476	cd.Round();
477
478	//
479	// Check if there is a control deviation on the axis
480	//
481	const Bool_t cdzd = (int)cd.Zd() ? kTRUE : kFALSE;
482	const Bool_t cdaz = (int)cd.Az() ? kTRUE : kFALSE;
483
484	//
485	// check if we reached the correct position already
486	//
487	if (!cdzd && !cdaz)
488	{
489	lout << "Positioning done in " << i << (i==1?" step.":" steps.") << endl;
490	SetStatus(MCosy::kStopped);
491	return TRUE;
492	}
493
494	//
495	// change units from se to re
496	//
497	rd *= kGearRatio; // [re]
498
499	//
500	// Initialize Velocities so that we reach both positions
501	// at the same time
502	//
503	lout << "SetVelocity" << endl;
504	if (i)
505	SetPosVelocity(1.0, 0.05, 0.1);
506	else // vel(0.6) acc(0.5)
507	SetPosVelocity(fabs(rd.Ratio()), 0.15, 0.4);
508	// SetPosVelocity(fabs(rd.Ratio()), 0.1, 0.3);
509
510	rd.Round();
511
512	// FIXME? Check for Error or Zombie?
513
514	/*
515	cout << " + " << (int)cdzd << " " << (int)cdaz << endl;
516	cout << " + APOS: Zd=" << setw(6) << p.Zd() << "se Az=" << setw(6) << p.Az() << "se" << endl;
517	cout << " + dZd=" << setw(6) << cd.Zd() << "se dAz=" << setw(6) << cd.Az() << "se" << endl;
518	cout << " + dZd=" << setw(6) << rd.Zd() << "re dAz=" << setw(6) << rd.Az() << "re" << endl;
519	cout << " + Ratio: Zd=" << setw(6) << kGearRatio.X() << "se Az=" << setw(6) << kGearRatio.Y() << "se" << endl;
520	*/
521
522	//
523	// repositioning (relative)
524	//
525	lout << "Do Relative Positioning Done" << endl;
526	DoRelPos(rd, cdzd, cdaz);
527
528	lout << "Relative Positioning Done" << endl;
529	}
530
531	StopMovement();
532	lout << "Warning: Requested position not reached (i=" << i << ")" << endl;
533	return FALSE;
534	}
535
536	// --------------------------------------------------------------------------
537	//
538	// Sets the tracking velocity
539	//
540	// The velocities are given in a ZdAz object in re/min. Return kTRUE
541	// in case of success, kFALSE in case of failure.
542	//
543	Bool_t MCosy::SetVelocity(const ZdAz &v)
544	{
545	//
546	// Send the new velocities for both axes.
547	//
548	fMac2->SendSDO(0x3006, 1, (LWORD_t)v.Zd()); // SetRpmVelocity [re/min]
549	fMac1->SendSDO(0x3006, 1, (LWORD_t)v.Az()); // SetRpmVelocity [re/min]
550
551	//
552	// Wait for the objects to be OKed.
553	//
554
555	fMac2->WaitForSdo(0x3006, 1);
556	fMac1->WaitForSdo(0x3006, 1);
557
558	// FIXME, what when waiting times out (Zombie)
559	// WaitForSdos();
560
561	//
562	// If the waiting for the objects wasn't interrupted return kTRUE
563	//
564	if (!(Break() \|\| HasError()) && !HasZombie())
565	return kTRUE;
566
567	//
568	// print a message if the interruption was due to a Can-node Error
569	//
570	if (HasError())
571	lout << "Error while setting velocity (SDO #3006)" << endl;
572
573	return kFALSE;
574	}
575
576	// --------------------------------------------------------------------------
577	//
578	// Initializes Tracking mode
579	//
580	// Initializes the accelerations of both axes with 90% of the maximum
581	// acceleration. Set the status for moving and tracking and starts thr
582	// revolution mode.
583	//
584	bool MCosy::InitTracking()
585	{
586	// FIXME? Handling of Zombie OK?
587	if (fMac1->IsZombieNode() \|\| fMac2->IsZombieNode())
588	return false;
589
590	//
591	// Start revolution mode
592	//
593	fMac2->SetAcceleration(0.90*fMac2->GetVelRes());
594	fMac2->SetDeceleration(0.90*fMac2->GetVelRes());
595	if (fMac2->IsZombieNode())
596	return false;
597
598	fMac1->SetAcceleration(0.90*fMac1->GetVelRes());
599	fMac1->SetDeceleration(0.90*fMac1->GetVelRes());
600	if (fMac1->IsZombieNode())
601	return false;
602
603	SetStatus(MCosy::kMoving \| MCosy::kTracking);
604
605	fMac2->SetRpmMode(TRUE);
606	if (fMac2->IsZombieNode())
607	return false;
608
609	fMac1->SetRpmMode(TRUE);
610	if (fMac1->IsZombieNode())
611	return false;
612
613	return true;
614	}
615
616	// --------------------------------------------------------------------------
617	//
618	// Limits the speed.
619	//
620	// This function should work as a limiter. If a tracking error is too large
621	// to be corrected fast enough we would get enormous velocities. These
622	// velocities are limited to the maximum velocity.
623	//
624	void MCosy::LimitSpeed(ZdAz *vt, const ZdAz &vcalc) const
625	{
626	//
627	// How to limit the speed. If the wind comes and blowes
628	// we cannot forbid changing of the sign. But on the other hand
629	// we don't want fast changes!
630	//
631
632	ULong_t vrzd = fMac1->GetVelRes();
633	ULong_t vraz = fMac2->GetVelRes();
634
635	#define sgn(x) (x<0?-1:1)
636
637	const Float_t limit = 0.25;
638	/*
639	if (sgn(vt->Az()) != sgn(vcalc.Az()))
640	vt->Az(0);
641	// else
642	{
643	if (fabs(vt->Az()) < fabs(vcalc.Az()) *0.5)
644	vt->Az(0.5*vcalc.Az());
645
646	if (fabs(vt->Az()) > fabs(vcalc.Az()) *1.5)
647	vt->Az(1.5*vcalc.Az());
648	}
649
650	if (sgn(vt->Zd()) != sgn(vcalc.Zd()))
651	vt->Zd(0);
652	// else
653	{
654	if (fabs(vt->Zd()) > fabs(vcalc.Az()) *1.5)
655	vt->Zd(1.5*vcalc.Zd());
656
657	if (fabs(vt->Zd()) < fabs(vcalc.Az()) *0.5)
658	vt->Zd(0.5*vcalc.Zd());
659	}
660	*/
661
662	if (sgn(vt->Az()) != sgn(vcalc.Az())
663	&& fabs(vt->Az()) < limit*fabs(vcalc.Az())
664	)
665	vt->Az(0);
666	else
667	if (fabs(vt->Az()) > 0.9*vraz)
668	{
669	lout << "Warning: Azimuth speed limit exceeded. Limiting speed to 90% max." << endl;
670	vt->Az(0.9vrazsgn(vt->Az()));
671	}
672
673	if (sgn(vt->Zd()) != sgn(vcalc.Zd())
674	&& fabs(vt->Zd()) < limit*fabs(vcalc.Zd())
675	)
676	vt->Zd(0);
677	else
678	if (fabs(vt->Zd()) > 0.9*vrzd)
679	{
680	lout << "Warning: Altitude speed limit exceeded. Limiting speed to 90% max." << endl;
681	vt->Zd(0.9vrzdsgn(vt->Zd()));
682	}
683	}
684
685	void MCosy::TrackPosition(const RaDec &dst) // ra, dec [rad]
686	{
687	lout << "Track Position: " << dst.Ra()kRad2Deg/15 << "h, " << dst.Dec()kRad2Deg << "deg (Ra/Dec)" << endl;
688
689	SlaStars sla(fObservatory);
690
691	//
692	// Position to actual position
693	//
694	sla.Now();
695	ZdAz dest = sla.CalcZdAz(dst);
696
697	if (!SetPosition(dest))
698	{
699	lout << "Error: Cannot start tracking, positioning failed." << endl;
700	return;
701	}
702
703	//
704	// calculate offset from present se position
705	//
706	const ZdAz sepos = GetSePos()*kGearRatio;
707
708	if (!RequestRePos())
709	return;
710
711	//
712	// Estimate Offset before starting to track
713	//
714	fOffset = sepos-GetRePos();
715
716	/*
717	cout << "Sepos: " << sepos.Zd() << "re, " << sepos.Az() << "re" << endl;
718	cout << "Repos: " << repos.Zd() << "re, " << repos.Az() << "re" << endl;
719	cout << "Offset: " << fOffset.Zd() << "re, " << fOffset.Az() << "re" << endl;
720	*/
721
722	//
723	// Init accelerations and Rpm Mode
724	//
725	if (!InitTracking())
726	return;
727
728	XY xy(Rad2Deg(dst.Ra())*24/360, Rad2Deg(dst.Dec()));
729
730	lout << "Start tracking:";
731	lout << " Ra: " << xy.X() << "h " << "Dec: " << xy.Y() << "\xb0" << endl;
732
733	ofstream fout("coordinates.txt");
734	fout << xy;
735	fout.close();
736
737	//
738	// Initialize Tracker (slalib or starguider)
739	//
740	fRaDec = dst;
741	fBackground = kBgdTracking;
742
743	//--- ofstream fout("log/cosy.pos");
744	//--- fout << "Tracking:";
745	//--- fout << " Ra: " << Rad2Deg(dst.Ra()) << "\x9c ";
746	//--- fout << "Dec: " << Rad2Deg(dst.Dec()) << "\x9c" << endl << endl;
747	//--- fout << " Mjd/10ms V/re/min/4" << endl;
748
749	//
750	// We want to reach the theoretical position exactly in about 0.5s
751	//
752	// OLDconst float dt = 1; // 1 second
753	const float dt = 3; // 1 second
754	while (!(Break() \|\| HasError()) && !HasZombie())
755	{
756	//
757	// Request Target position for this moment
758	//
759	sla.Now();
760
761	//
762	// Request theoretical Position for a time in the future (To+dt) from CPU
763	//
764	sla.SetMjd(sla.GetMjd()+dt/(606024));
765	ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
766	dest = CorrectTarget(GetSePos(), dummy); // [se]
767
768	const ZdAz d = dest*360./16384; // [deg]
769	dest *= kGearRatio; // [re]
770
771	ZdAz min = fBending(fMin/kRad2Deg)*kRad2Deg;
772	ZdAz max = fBending(fMax/kRad2Deg)*kRad2Deg;
773
774	if (d.Zd()<min.Zd() \|\| d.Az()<min.Az())
775	{
776	lout << "ERROR: Calculated position for T+dt not inside allowed range." << endl;
777	lout << "< " << d.Zd() << " " << min.Zd() << " " << d.Az() << " " << min.Az() << endl;
778	break;
779	}
780	if (d.Zd()>max.Zd() \|\| d.Az()>max.Az())
781	{
782	lout << "ERROR: Calculated position for T+dt not inside allowed range." << endl;
783	lout << "> " << d.Zd() << " " << max.Zd() << " " << d.Az() << " " << max.Az() << endl;
784	break;
785	}
786
787	ZdAz vcalc = sla.GetApproxVel(fRaDec) * kGearRatio2*4./60.; // [re/min]
788
789	//
790	// Request absolute position of rotary encoder from Macs
791	//
792	if (!RequestRePos())
793	break;
794
795	//
796	// distance between (To+dt) and To [re]
797	// position time difference < 5usec
798	// fOffset does the synchronization between the
799	// Shaft- and the rotary encoders
800	//
801	dest -= GetRePos() + fOffset;
802
803	//
804	// Velocity to go [re/min] to reach the right position at time t+dt
805	// correct for the duration of RaDec2AltAz
806	//
807	const ZdAz v = dest60.0/(dt/-(fMac2->GetTime()-sla)*/);
808
809	//
810	// calculate real velocity of future [re/min]
811	// believing the Macs manual '/4' shouldn't be necessary, but it is.
812	//
813	ZdAz vt = v/4;
814	LimitSpeed(&vt, vcalc);
815	vt.Round();
816
817	//
818	// check if the drive is fast enough to follow the star
819	//
820	if (vt.Zd()>.9fMac1->GetVelRes() \|\| vt.Az()>.9fMac2->GetVelRes())
821	{
822	lout << "Error: Tracking speed faster than 90% of possible maximum velocity." << endl;
823	break;
824	}
825
826	//
827	// Set theoretical velocity (as early after calculation as possible)
828	// Maybe we should attenuate the changes
829	//
830	if (!SetVelocity(vt))
831	break;
832
833	//
834	// Now do 'unnecessary' things
835	//
836	fVelocity = vt/kGearRatio2*4;
837
838	//--- const double mjd = fMac2->GetMjd();
839	//--- fout << setprecision(15) << setw(17) << mjd60.60.*24. << " ";
840	//--- fout << setw(4) << vt.Zd() << " ";
841	//--- fout << setw(4) << vt.Az() << endl;
842	//
843	// FIXME? Calculate an accuracy for the tracking system?
844	// How good do we reach the calculated position in 'real'
845	// re valus?
846	//
847
848
849	//
850	// Update speed as often as possible.
851	// make sure, that dt is around 10 times larger than the
852	// update time
853	//
854	//
855	// The loop should not be executed faster than the ramp of
856	// a change in the velocity can be followed.
857	// (This is important on fast machines >500MHz)
858	//
859	usleep(50000); // 0.25s
860	//usleep(50000); // 0.05s
861	}
862
863	fBackground = kBgdNone;
864	StopMovement();
865	lout << "Tracking stopped." << endl;
866	}
867
868	// --------------------------------------------------------------------------
869	//
870	// Stops the movement of both motors.
871	//
872	// Sets the status to stopping. Sets the deceleration to 50% of the maximum.
873	// stops. Quits the revolution mode and wait for the end of the movement.
874	//
875	void MCosy::StopMovement()
876	{
877	//
878	// Set status to Stopping
879	//
880	SetStatus(MCosy::kStopping);
881
882	//
883	// set deceleration to 50%
884	//
885	cout << "Stopping movement..." << endl;
886	fMac1->SetDeceleration(0.5*fMac1->GetVelRes());
887	fMac2->SetDeceleration(0.5*fMac2->GetVelRes());
888
889	fMac1->SetRpmMode(FALSE);
890	fMac2->SetRpmMode(FALSE);
891
892	//
893	// Wait for the movement to really be finished.
894	//
895	cout << "Waiting for silence..." << endl;
896	WaitForEndMovement();
897
898	//
899	// Check whether everything works fine.
900	//
901	CheckForError();
902	cout << "Movement stopped." << endl;
903	}
904
905	bool MCosy::CheckNetwork()
906	{
907	//return kTRUE;
908	//CheckConnections();
909
910	if (HasZombie())
911	{
912	lout << "- Found Zombies in Network..." << endl;
913	if (!RebootZombies())
914	return false;
915	}
916
917	/*
918	FIXME HANDLING ERROR
919	*/
920	if (HasError())
921	{
922	fMac1->HandleError();
923	fMac2->HandleError();
924	fMac3->HandleError();
925	if (HasError() \|\| HasZombie())
926	return false;
927	}
928
929	return true;
930	}
931
932	void MCosy::Proc(int msg, void mp)
933	{
934	switch (msg)
935	{
936	case WM_WAIT:
937	cout << "Wait for execution of Proc(WM_*, ): done." << endl;
938	return NULL;
939
940	case WM_STOP:
941	cout << "MCosy::Proc: Stop." << endl;
942	if (!CheckNetwork())
943	return (void*)0xebb0;
944	StopMovement();
945	return NULL;
946
947	case WM_PRESET:
948	cout << "WM_Preset: start." << endl;
949	if (!CheckNetwork())
950	return (void*)0xebb0;
951	fZd1->SetPreset();
952	fZd2->SetPreset();
953	fAz->SetPreset();
954	cout << "WM_Preset: done. (return 0xaffe)" << endl;
955	return (void*)0xaffe;
956
957	case WM_CALIB:
958	{
959	cout << "WM_Calib: start." << endl;
960	if (!CheckNetwork())
961	return (void*)0xebb0;
962
963	SlaStars sla(fObservatory);
964	sla.Now();
965
966	RaDec rd = ((RaDec)mp);
967
968	//RaDec rd(37.94, 89.2644); // POLARIS
969	//RaDec rd(213.915417, 19.1825); // ACTURUS
970
971	cout << "Calibrating to: " << rd.Ra()*24/360 << "h " << rd.Dec() << "°" << endl;
972
973	ZdAz za=sla.CalcZdAz(rdkDeg2Rad)16384.0/k2Pi;
974
975	cout << "Calc Zd: " << za.Zd() << " Az: " << za.Az() << endl;
976
977	ZdAz sepos = GetSePos();
978	cout << "Got Zd: " << sepos.Zd() << " Az: " << sepos.Az() << endl;
979
980	fZd1->SetPreset(za.Zd());
981	fZd2->SetPreset(-za.Zd());
982	fAz->SetPreset(za.Az());
983
984	cout << "WM_Calib: done. (return 0xaffe)" << endl;
985	}
986	return (void*)0xaffe;
987
988	case WM_TPOINT:
989	{
990	cout << "WM_TPoint: start." << endl;
991	SlaStars sla(fObservatory);
992	sla.Now();
993
994	RaDec rd = ((RaDec)mp);
995	cout << "TPoint Star: " << rd.Ra()/15 << "h " << rd.Dec() << "°" << endl;
996
997	AltAz za=sla.CalcAltAz(rdkDeg2Rad)kRad2Deg;
998
999	cout << " Alt/Az: " << za.Alt() << "° " << za.Az() << "°" << endl;
1000	*tpout << za.Az() << " " << za.Alt() << " ";
1001
1002	ZdAz sepos = GetSePos()TMath::Pi()2/16384;;
1003	za.Set(TMath::Pi()/2-sepos.Zd(), sepos.Az());
1004	za *= kRad2Deg;
1005
1006	cout << " SE-Pos: " << za.Alt() << "° " << za.Az() << "°" << endl;
1007	*tpout << fmod(za.Az()+360, 360) << " " << za.Alt() << endl;
1008
1009	cout << "WM_TPoint: done. (return 0xaffe)" << endl;
1010	}
1011	return (void*)0xca1b;
1012
1013	case WM_POSITION:
1014	cout << "WM_Position: start." << endl;
1015	{
1016	if (!CheckNetwork())
1017	return (void*)0xebb0;
1018
1019	ZdAz dest = ((ZdAz)mp);
1020	SetPosition(dest*kDeg2Rad);
1021	}
1022	cout << "WM_Position: done. (return 0x7777)" << endl;
1023	return (void*)0x7777;
1024
1025	case WM_TESTSE:
1026	cout << "WM_TestSe: start." << endl;
1027	fBackground = mp ? kBgdSeTest : kBgdNone;
1028	cout << "WM_TestSe: done. (return 0x1e51)" << endl;
1029	return (void*)0x1e51;
1030
1031	case WM_GEAR:
1032	cout << "WM_Gear: start." << endl;
1033	fBackground = mp ? kBgdGear : kBgdNone;
1034	cout << "WM_Gear: done. (return 0xfeaf)" << endl;
1035	return (void*)0xfeaf;
1036
1037	case WM_DISPLAY:
1038	cout << "WM_Display: start." << endl;
1039	fTriggerDisplay = kTRUE;
1040	cout << "WM_Disply: done. (return 0xd1e1)" << endl;
1041	return (void*)0xd1e1;
1042
1043	case WM_TRACK:
1044	cout << "WM_Track: START" << endl;
1045	{
1046	if (!CheckNetwork())
1047	return (void*)0xebb0;
1048
1049	RaDec dest = ((RaDec)mp);
1050	TrackPosition(dest*kDeg2Rad);
1051	}
1052	cout << "WM_Track: done. (return 0x8888)" << endl;
1053	return (void*)0x8888;
1054
1055	case WM_NEWTRACK:
1056	cout << "WM_NewTrack: START" << endl;
1057	fRaDec = ((RaDec)mp);
1058	cout << "WM_NewTrack: done. (return 0x9999)" << endl;
1059	return (void*)0x9999;
1060
1061	case WM_LOADBENDING:
1062	cout << "WM_LoadBending: START" << endl;
1063	fBending.Load("bending.txt");
1064	cout << "WM_LoadBending: done. (return 0xbe0d)" << endl;
1065	return (void*)0xbe0d;
1066
1067	case WM_RESETBENDING:
1068	cout << "WM_ResetBending: START" << endl;
1069	fBending.Reset();
1070	cout << "WM_ResetBending: done. (return 0xbe0e)" << endl;
1071	return (void*)0xbe0e;
1072
1073	case WM_HOME:
1074	cout << "WM_Home: START" << endl;
1075	if (!CheckNetwork())
1076	return (void*)0xebb0;
1077	else
1078	{
1079	cout << "HOME NOT ALLOWED... for Magic." << endl;
1080	/*
1081	cout << "Going Home..." << endl;
1082	TEnv env(".cosyrc");
1083
1084	SetStatus(MCosy::kMoving);
1085
1086	fMac1->SetHome(250000, env.GetValue("Az_MaxTime2ReachHome[s]", 100));
1087	fMac2->SetHome(250000, env.GetValue("Zd_MaxTime2ReachHome[s]", 100));
1088
1089	lout << "SETHOME DONE" << endl;
1090
1091	SetStatus(HasError() ? MCosy::kError : MCosy::kStopped);
1092
1093	fAz->SetPreset();
1094	fZd1->SetPreset();
1095	fZd2->SetPreset();
1096
1097	fMac1->ReqPos();
1098	fMac2->ReqPos();
1099	fMac3->StopMotor();
1100	*/
1101	}
1102	cout << "WM_Home: done. (return 0x403e)" << endl;
1103	return (void*)0x403e;
1104
1105	case WM_CALCALTAZ:
1106	{
1107	cout << endl;
1108
1109	SlaStars sla(fObservatory);
1110	sla.Now();
1111
1112	XY xy = ((XY)mp);
1113	RaDec rd(xy.X()*15., xy.Y()); // [deg]
1114
1115	ZdAz a0 = sla.CalcZdAz(rd*kDeg2Rad);
1116	ZdAz a1 = fBending(a0);
1117	ZdAz se = CorrectTarget(GetSePos(), a1);
1118	a0 *= kRad2Deg;
1119	a1 *= kRad2Deg;
1120	ZdAz a2 = a1*16384/360;
1121	cout << "Ra/Dec source: " << xy.X() << "h " << xy.Y() << "°" << endl;
1122	cout << "Zd/Az source: " << a0.Zd() << "° " << a0.Az() << "°" << endl;
1123	cout << "Zd/Az bended: " << a1.Zd() << "° " << a1.Az() << "°" << endl;
1124	cout << "SE bended: " << a2.Zd() << " " << a2.Az() << endl;
1125	cout << "SE target: " << se.Zd() << " " << se.Az() << endl;
1126	}
1127	return (void*)0xa17a;
1128
1129	case WM_QUIT:
1130	cout << "WM_Quit: now." << endl;
1131	if (!CheckNetwork())
1132	{
1133	lout << "ERROR: Cannot shutdown CANbus network." << endl;
1134	return (void*)0xebb0;
1135	}
1136	TerminateApp();
1137	cout << "WM_Quit: done." << endl;
1138	return (void*)0xaaaa;
1139	}
1140	cout << "MCosy::Proc: Unknown message 0x" << msg << endl;
1141	return (void*)0xffffffff;
1142	}
1143
1144	void *MTTalk::Thread()
1145	{
1146	fCosy->TalkThread();
1147	return NULL;
1148	}
1149
1150	void MCosy::ReadConfig()
1151	{
1152	cout << "Reading configuration file..." << flush;
1153	TEnv env(".cosyrc");
1154	cout << "done." << endl;
1155
1156	cout << "Reading telescope range..." << flush;
1157	const Double_t amin = env.GetValue("Az_Min[deg]", -74.5);
1158	const Double_t zmin = env.GetValue("Zd_Min[deg]", -90.0);
1159	fMin.Set(zmin, amin);
1160
1161	cout << " Min: " << zmin << "deg " << amin << "deg" << endl;
1162
1163	const Double_t amax = env.GetValue("Az_Max[deg]", 318.0);
1164	const Double_t zmax = env.GetValue("Zd_Max[deg]", 100.5);
1165	fMax.Set(zmax, amax);
1166
1167	cout << " Max: " << zmax << "deg " << amax << "deg" << endl;
1168
1169	cout << "Reading gear ratios..." << flush;
1170	const Double_t gaz = env.GetValue("Az_GearRatio[U_mot/U_tel]", 1000.0);
1171	const Double_t gzd = env.GetValue("Zd_GearRatio[U_mot/U_tel]", 1000.0);
1172
1173	Double_t resreaz = 0;
1174	if (fMac1 && !fMac1->IsZombieNode())
1175	resreaz = fMac1->GetRes();
1176	else
1177	if (fMac3 && !fMac3->IsZombieNode())
1178	resreaz = fMac3->GetRes();
1179	else
1180	resreaz = env.GetValue("Az_ResRE[re/U_mot]", 1500);
1181
1182	Double_t resrezd = 0;
1183	if (fMac2 && !fMac2->IsZombieNode())
1184	resrezd = fMac2->GetRes();
1185	else
1186	resrezd = env.GetValue("Zd_ResRE[re/U_mot]", 1500);
1187
1188	Double_t ressezd = 0;
1189	if (fZd1 && !fZd1->IsZombieNode())
1190	ressezd = fZd1->GetPhysRes();
1191	else
1192	if (fZd2 && !fZd2->IsZombieNode())
1193	ressezd = fZd2->GetPhysRes();
1194	else
1195	ressezd = env.GetValue("Zd_ResSE[se/U_mot]", 16384);
1196
1197	Double_t resseaz = 0;
1198	if (fAz && !fAz->IsZombieNode())
1199	resseaz = fAz->GetPhysRes();
1200	else
1201	resseaz = env.GetValue("Az_ResSE[se/U_mot]", 16384);
1202
1203	kGearRatio.Set (gzdresrezd4/ressezd, gazresreaz4/resseaz); //[re/se]
1204	kGearRatio2.Set(gzdresrezd4/360.0, gazresreaz4/360.0); //[re/deg]
1205	cout << "done." << endl;
1206
1207	cout << " * Setting Gear Ratios:" << endl;
1208	cout << " --------------------" << endl;
1209	cout << " * X: " << gzd << "*" << resrezd << "/" << ressezd << "=" << kGearRatio.X() << endl;
1210	cout << " * Y: " << gaz << "*" << resreaz << "/" << resseaz << "=" << kGearRatio.Y() << endl;
1211	}
1212
1213	void MCosy::InitSync()
1214	{
1215	if (!fMac3)
1216	{
1217	lout << "Unable to Init Sync! Mac3 not available." << endl;
1218	return;
1219	}
1220
1221	const int res = fMac3->GetVelRes();
1222
1223	fMac3->SetVelocity(res);
1224	fMac3->SetAcceleration(res);
1225	fMac3->SetDeceleration(res);
1226	fMac3->StartPosSync();
1227	}
1228
1229	void MCosy::TalkThreadTracking()
1230	{
1231	if (fZd1->IsZombieNode() && fZd2->IsZombieNode())
1232	return;
1233
1234	if (fAz->IsZombieNode())
1235	return;
1236
1237	if (!fMac1 \|\| !fMac2)
1238	return;
1239
1240	lout << "Tracking Thread started..." << endl;
1241
1242	SlaStars sla(fObservatory);
1243
1244	ZdAz old;
1245	ZdAz ist;
1246
1247	ZdAz sollzd;
1248	ZdAz sollaz;
1249
1250	ZdAz istre = -fOffset; // [re]
1251	ZdAz time;
1252
1253	//
1254	// only update fTrackingError while tracking
1255	//
1256	bool phca1=false;
1257	bool phca2=false;
1258	bool phcaz=false;
1259
1260	while (fBackground==kBgdTracking)
1261	{
1262	//
1263	// Make changes (eg wind) smoother - attenuation of control function
1264	//
1265	const float weight = 1.; //0.3;
1266
1267	//
1268	// This is the time constant which defines how fast
1269	// you correct for external influences (like wind)
1270	//
1271	fZd1->ResetPosHasChanged();
1272	fZd2->ResetPosHasChanged();
1273	fAz->ResetPosHasChanged();
1274	do
1275	{
1276	phca1 = fZd1->PosHasChanged();
1277	phca2 = fZd2->PosHasChanged();
1278	phcaz = fAz->PosHasChanged();
1279	usleep(1);
1280	} while (!phca1 && !phca2 && !phcaz && fBackground==kBgdTracking);
1281
1282	//---usleep(100000); // 0.1s
1283
1284	//
1285	// get position, where we are
1286	//
1287	old = ist;
1288	ist = GetSePos(); // [se]
1289
1290	//
1291	// if the position didn't change continue
1292	//
1293	/*---
1294	if ((int)ist.Zd() == (int)old.Zd() &&
1295	(int)ist.Az() == (int)old.Az())
1296	continue;
1297	*/
1298	istre = GetRePosPdo();
1299
1300	//
1301	// Get time from last shaftencoder position change (position: ist)
1302	// FIXME: I cannot take the avarage
1303	//
1304	// FIXME
1305	//time.Zd(fZd1->GetMjd());
1306	/* OLD* */
1307	if (fZd1->GetMjd()>fZd2->GetMjd())
1308	time.Zd(fZd1->GetMjd());
1309	else
1310	time.Zd(fZd2->GetMjd());
1311
1312	//time.Zd((fZd1->GetMjd()+fZd2->GetMjd())/2.0);
1313	time.Az(fAz->GetMjd());
1314
1315	//
1316	// if Shaftencoder changed position
1317	// calculate were we should be
1318	//
1319	if (phca1 \|\| phca2 /(int)ist.Zd() != (int)old.Zd()/)
1320	{
1321	sla.SetMjd(time.Zd());
1322
1323	ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
1324	sollzd = CorrectTarget(ist, dummy); // [se]
1325
1326	fOffset.Zd(fOffset.Zd()(1.-weight)+(ist.Zd()kGearRatio.X()-istre.Zd())*weight);
1327	}
1328
1329	if (phcaz /(int)ist.Az() != (int)old.Az()/)
1330	{
1331	sla.SetMjd(time.Az());
1332
1333	ZdAz dummy = fBending(sla.CalcZdAz(fRaDec));
1334	sollaz = CorrectTarget(ist, dummy); // [se]
1335
1336	fOffset.Az(fOffset.Az()(1.-weight)+(ist.Az()kGearRatio.Y()-istre.Az())*weight);
1337	}
1338
1339	ZdAz soll(sollzd.Zd(), sollaz.Az()); // [se]
1340	fZdAzSoll = fBending.CorrectBack(soll2TMath::Pi()/16384);
1341
1342	fTrackingError.Set((ist.Zd()-sollzd.Zd())*kGearRatio.X(),
1343	(ist.Az()-sollaz.Az())*kGearRatio.Y());
1344
1345	//--- fout << setprecision(15) << setw(17) << time.Zd()60.60.*24. << " ";
1346	//--- fout << setprecision(5) << setw(7) << fTrackingError.Zd() << " ";
1347	//--- fout << setprecision(15) << setw(17) << time.Az()60.60.*24. << " ";
1348	//--- fout << setprecision(5) << setw(7) << fTrackingError.Az() << endl;
1349	}
1350
1351	lout << "Tracking Thread done." << endl;
1352
1353	//--- fout << endl << endl;
1354	}
1355
1356	void MCosy::TalkThreadSeTest()
1357	{
1358	// if (fZd1->IsZombieNode() \|\| fZd2->IsZombieNode())
1359	// return;
1360
1361	if (fHist)
1362	{
1363	lout << "You are much too fast... try again." << endl;
1364	return;
1365	}
1366
1367	fHist = new TH2F("Diff", "Difference of SE values",
1368	201, fMin.Zd(), fMax.Zd(), 41, -10.5, 10.5);
1369	fHist->SetXTitle("ZA [\\circ]");
1370	fHist->SetYTitle("\\Delta SE");
1371
1372	Double_t offset = 0;
1373
1374	int cnt = 0;
1375
1376	lout << "Starting Shaftencoder Test..." << endl;
1377
1378	while (fBackground==kBgdSeTest)
1379	{
1380	fZd1->ResetPosHasChanged();
1381	fZd2->ResetPosHasChanged();
1382
1383	while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
1384	fBackground==kBgdSeTest)
1385	usleep(1);
1386
1387	const Double_t pos[3] = {
1388	(fZd1->GetPos()+8192)%16384,
1389	(fZd2->GetPos()+8192)%16384,
1390	fAz->GetPos() };
1391
1392	//
1393	// Estimate Offset from the first ten positions
1394	//
1395	if (cnt++<10)
1396	{
1397	offset += pos[0]+pos[1];
1398	continue;
1399	}
1400	if (cnt==11)
1401	{
1402	offset /= 10;
1403	cnt++;
1404	}
1405
1406	Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384;
1407
1408	ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
1409	fHist->Fill(bend.Zd(), pos[0]+pos[1]-offset);
1410	}
1411
1412	lout << "Shaftencoder Test Stopped... displaying Histogram." << endl;
1413
1414	fBackground=kBgdSeTestDispl;
1415	}
1416
1417	void MCosy::TalkThreadGear()
1418	{
1419	// if (fZd1->IsZombieNode() \|\| fZd2->IsZombieNode())
1420	// return;
1421
1422	if (fHist)
1423	{
1424	lout << "You are much too fast... try again." << endl;
1425	return;
1426	}
1427
1428	// fHist = new TH2F("Gear", "Gear Ratio Re/Se",
1429	// 201, fMin.Zd(), fMax.Zd(), 61, 349.5, 500.5);
1430	fHist = new TH2F("Gear", "Gear Ratio Re/Se",
1431	201, fMin.Az(), fMax.Az(), 61, 419.5, 570.5);
1432	fHist->SetXTitle("ZA [\\circ]");
1433	fHist->SetYTitle("Re/Se");
1434
1435	lout << "Starting Gear determination..." << endl;
1436
1437	ZdAz se0 = GetSePos();
1438	ZdAz re0 = GetRePosPdo();
1439
1440	while (fBackground==kBgdGear)
1441	{
1442	fZd1->ResetPosHasChanged();
1443	fZd2->ResetPosHasChanged();
1444	fAz->ResetPosHasChanged();
1445
1446	while (!fZd1->PosHasChanged() && !fZd2->PosHasChanged() &&
1447	!fAz->PosHasChanged() && fBackground==kBgdGear)
1448	usleep(1);
1449
1450	ZdAz se = GetSePos();
1451	ZdAz re = GetRePosPdo();
1452
1453	ZdAz dse = se-se0;
1454	ZdAz dre = re-re0;
1455
1456	if (fabs(dse.Zd())*144>16384) // Each 2.5deg
1457	{
1458	se0.Zd(se.Zd());
1459	re0.Zd(re.Zd());
1460
1461	ZdAz bend = fBending.CorrectBack(se2TMath::Pi()/16384)*kRad2Deg;
1462	fHist->Fill(bend.Zd(), dre.Zd()/dse.Zd());
1463	}
1464
1465	if (fabs(dse.Az())*144>16384) // Each 2.5deg
1466	{
1467	se0.Az(se.Az());
1468	re0.Az(re.Az());
1469
1470	ZdAz bend = fBending.CorrectBack(se2TMath::Pi()/16384)*kRad2Deg;
1471	fHist->Fill(bend.Az(), dre.Az()/dse.Az());
1472
1473	cout << bend.Az() << ": " << dre.Az()/dse.Az() << endl;
1474	}
1475
1476	/*
1477	const Double_t pos[3] = {
1478	(fZd1->GetPos()+8192)%16384,
1479	(fZd2->GetPos()+8192)%16384,
1480	fAz->GetPos() };
1481
1482	//
1483	// Estimate Offset from the first ten positions
1484	//
1485	if (cnt++<10)
1486	{
1487	offset += pos[0]+pos[1];
1488	continue;
1489	}
1490	if (cnt==11)
1491	{
1492	offset /= 10;
1493	cnt++;
1494	}
1495
1496	Double_t apos = (pos[0]-pos[1])/2 * TMath::Pi()*2 / 16384;
1497
1498	ZdAz bend = fBending.CorrectBack(ZdAz(apos, pos[2]))*kRad2Deg;
1499	fHistTestSe->Fill(bend.Zd(), pos[0]+pos[1]-offset);
1500	*/
1501	}
1502	lout << "Gear Test Stopped... displaying Histogram." << endl;
1503
1504	fBackground=kBgdGearDispl;
1505	}
1506
1507	void MCosy::TalkThread()
1508	{
1509	/* ========== FIXME? =============
1510	if (fMac1->IsZombieNode() \|\| fMac2->IsZombieNode())
1511	return;
1512	*/
1513
1514	if (fMac1 && fMac2)
1515	{
1516	fMac1->ReqPos();
1517	fMac2->ReqPos();
1518	}
1519
1520	InitSync();
1521
1522	/* FOR DEMO MODE */
1523	if (!fZd1 \|\| !fZd2 \|\| !fAz)
1524	return;
1525	/* FOR DEMO MODE */
1526
1527	//
1528	// Start the Network
1529	//
1530	while (1)
1531	{
1532	//
1533	// wait until a tracking session is started
1534	//
1535	while (fBackground==kBgdNone)
1536	usleep(1);
1537
1538	switch (fBackground)
1539	{
1540	case kBgdNone:
1541	continue;
1542
1543	case kBgdTracking:
1544	TalkThreadTracking();
1545	continue;
1546
1547	case kBgdSeTest:
1548	TalkThreadSeTest();
1549	continue;
1550
1551	case kBgdGear:
1552	TalkThreadGear();
1553	continue;
1554
1555	default:
1556	continue;
1557	}
1558	}
1559	}
1560
1561	Bool_t MCosy::HandleTimer(TTimer *t)
1562	{
1563	//
1564	// Update Gui, foremer MTGui.
1565	//
1566	if (fZd1)
1567	fZd1->DisplayVal();
1568	if (fZd2)
1569	fZd2->DisplayVal();
1570	if (fAz)
1571	fAz->DisplayVal();
1572
1573	ZdAz seist = GetSePos()2TMath::Pi()/16384; // [se]
1574	ZdAz bendist = fBending.CorrectBack(seist);
1575
1576	Byte_t avail = 0;
1577
1578	avail \|= (fMac1 && !fMac1->IsZombieNode()) ? 0x01 : 0;
1579	avail \|= (fMac2 && !fMac2->IsZombieNode()) ? 0x02 : 0;
1580	avail \|= (fMac3 && !fMac3->IsZombieNode()) ? 0x04 : 0;
1581	avail \|= (fZd1 && !fZd1->IsZombieNode()) ? 0x08 : 0;
1582	avail \|= (fZd2 && !fZd2->IsZombieNode()) ? 0x10 : 0;
1583	avail \|= (fAz && !fAz->IsZombieNode()) ? 0x20 : 0;
1584
1585	if (HasError())
1586	SetStatus(MCosy::kError);
1587
1588	lout.UpdateGui();
1589
1590	fWin->Update(bendist*(360.0/2/TMath::Pi()), fTrackingError/kGearRatio2,
1591	fVelocity, fOffset, fRaDec, fZdAzSoll, fStatus, avail);
1592
1593	/*
1594	cout << (int)(fMac1->GetStatus()&Macs::kOutOfControl) << " ";
1595	cout << (int)(fMac2->GetStatus()&Macs::kOutOfControl) << " ";
1596	cout << (int)(fMac3->GetStatus()&Macs::kOutOfControl) << endl;
1597	*/
1598
1599	const Bool_t trigger = fTriggerDisplay;
1600	fTriggerDisplay = kFALSE;
1601
1602	if (fBackground==kBgdSeTestDispl \|\| (trigger&&fBackground==kBgdSeTest))
1603	DisplayHistTestSe(!trigger);
1604
1605	if (fBackground==kBgdGearDispl \|\| (trigger&&fBackground==kBgdGear))
1606	DisplayHistGear(!trigger);
1607
1608	return kTRUE;
1609	}
1610
1611	void MCosy::DisplayHistTestSe(Bool_t del)
1612	{
1613	lout << "Displaying histogram..." << endl;
1614
1615	TH2F &hist = *fHist;
1616
1617	if (del)
1618	{
1619	fHist = NULL;
1620	fBackground = kBgdNone;
1621	}
1622
1623	TCanvas *c=new TCanvas("c1", "", 1000, 1000);
1624	c->Divide(1,2);
1625
1626	c->cd(1);
1627	TH2 h=(TH2)hist.DrawCopy();
1628
1629	TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
1630	p->SetLineColor(kBlue);
1631	p->Draw("same");
1632	p->SetBit(kCanDelete);
1633
1634	c->cd(2);
1635
1636	TH1F p2("spread", "Spread of the differences", hist.GetNbinsX(), hist.GetBinLowEdge(1),
1637	hist.GetBinLowEdge(hist.GetNbinsX()+1));
1638	p2.SetXTitle("ZA [\\circ]");
1639	for (int i=0; i<hist.GetNbinsX(); i++)
1640	p2.SetBinError(i, p->GetBinError(i));
1641	p2.SetLineColor(kRed);
1642	p2.SetStats(0);
1643	p2.DrawCopy();
1644
1645	if (del)
1646	delete &hist;
1647	}
1648
1649	void MCosy::DisplayHistGear(Bool_t del)
1650	{
1651	lout << "Displaying histogram..." << endl;
1652
1653	TH2F &hist = *fHist;
1654
1655	if (del)
1656	{
1657	fHist = NULL;
1658	fBackground = kBgdNone;
1659	}
1660
1661	TCanvas *c=new TCanvas("c1", "", 1000, 1000);
1662	c->Divide(1,2);
1663
1664	c->cd(1);
1665	TH2 h=(TH2)hist.DrawCopy();
1666
1667	TProfile *p = h->ProfileX("_pfx", -1, 9999, "s");
1668	p->SetLineColor(kBlue);
1669	p->Draw("same");
1670	p->SetBit(kCanDelete);
1671
1672	c->cd(2);
1673
1674	TH1F p2("spread", "Spread of the gear [16384/1500/4*U/U]", hist.GetNbinsX(), hist.GetBinLowEdge(1),
1675	hist.GetBinLowEdge(hist.GetNbinsX()+1));
1676	p2.SetXTitle("ZA [\\circ]");
1677	for (int i=0; i<hist.GetNbinsX(); i++)
1678	p2.SetBinError(i, p->GetBinError(i));
1679	p2.SetLineColor(kRed);
1680	p2.SetStats(0);
1681	p2.DrawCopy();
1682
1683	if (del)
1684	delete &hist;
1685	}
1686
1687	// --------------------------------------------------------------------------
1688	//
1689	// Start the work of the application:
1690	//
1691	// Start the Can-Network.
1692	// Start the MCosy::TalkThread thread.
1693	// turn on the gui update
1694	//
1695	void MCosy::Start()
1696	{
1697	// Don't call this function twice!
1698	Network::Start();
1699
1700	ReadConfig();
1701
1702	lout << "- Starting TX Thread." << endl;
1703	fTTalk = new MTTalk(this);
1704
1705	lout << "- Starting GUI update." << endl;
1706	fUpdateGui->TurnOn();
1707	}
1708
1709	// --------------------------------------------------------------------------
1710	//
1711	// Start the work of the application:
1712	//
1713	// Turn of the gui update
1714	// stop the MCosy::TalkThread thread.
1715	// Stop the network
1716	//
1717	void MCosy::Stop()
1718	{
1719	lout << "- Stopping GUI update." << endl;
1720	fUpdateGui->TurnOff();
1721	lout << "- GUI Update stopped." << endl;
1722
1723	delete fTTalk;
1724	lout << "- TX Thread stopped." << endl;
1725
1726	Network::Stop();
1727	}
1728
1729	// --------------------------------------------------------------------------
1730	//
1731	// Disable the synchronization by using a negative CAN Id for id2.
1732	//
1733	void MCosy::Constructor(Int_t id1, Int_t id2, Int_t id3,
1734	Int_t id4, Int_t id5, Int_t id6)
1735	{
1736	//
1737	// Create Nodes
1738	//
1739	lout << "- Setting up network." << endl;
1740
1741	fMac1=new Macs(id1, "Mac/Az", lout);
1742	fMac2=new Macs(id3, "Mac/Zd", lout);
1743	if (id2>=0)
1744	fMac3=new Macs(id2, "Mac/Az-Sync", lout);
1745
1746	fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
1747	fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
1748	fAz =new ShaftEncoder(id6, "SE/Az", lout);
1749
1750	lout << "- Connecting devices to network." << endl;
1751
1752	//
1753	// Connect the devices to the network
1754	//
1755	SetNode(fMac1);
1756	SetNode(fMac2);
1757	if (id2>=0)
1758	SetNode(fMac3);
1759	SetNode(fZd1);
1760	SetNode(fZd2);
1761	SetNode(fAz);
1762
1763	//
1764	// Create Gui Event timer and Gui
1765	//
1766	lout << "- Initializing GUI Timer." << endl;
1767	fUpdateGui = new TTimer(this, 100); // 100ms
1768
1769	lout << "- Starting GUI." << endl;
1770	fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
1771	}
1772
1773	void MCosy::ConstructorSE(Int_t id4, Int_t id5, Int_t id6)
1774	{
1775	//
1776	// Create Nodes
1777	//
1778	lout << "- Setting up network." << endl;
1779
1780	fZd1=new ShaftEncoder(id4, "SE/Zd1", lout);
1781	fZd2=new ShaftEncoder(id5, "SE/Zd2", lout);
1782	fAz =new ShaftEncoder(id6, "SE/Az", lout);
1783
1784	lout << "- Connecting devices to network." << endl;
1785
1786	//
1787	// Connect the devices to the network
1788	//
1789	SetNode(fZd1);
1790	SetNode(fZd2);
1791	SetNode(fAz);
1792
1793	//
1794	// Create Gui Event timer and Gui
1795	//
1796	lout << "- Initializing GUI Timer." << endl;
1797	fUpdateGui = new TTimer(this, 100); // 100ms
1798
1799	lout << "- Starting GUI." << endl;
1800	fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
1801	}
1802
1803	void MCosy::ConstructorDemo()
1804	{
1805	//
1806	// Create Nodes
1807	//
1808	lout << "- Setting up network." << endl;
1809
1810	//
1811	// Create Gui Event timer and Gui
1812	//
1813	lout << "- Initializing GUI Timer." << endl;
1814	fUpdateGui = new TTimer(this, 100); // 100ms
1815
1816	lout << "- Starting GUI." << endl;
1817	fWin=new MGCosy(fObservatory, this, gClient->GetRoot(), 1, 1);
1818	}
1819
1820	bool MCosy::HasZombie() const
1821	{
1822	bool ses = fZd1->IsZombieNode() \| fZd2->IsZombieNode();
1823
1824	return fMac1->IsZombieNode() \| fMac2->IsZombieNode() \| fAz->IsZombieNode() \| ses;
1825	}
1826
1827	MCosy::MCosy(int mode, const char *dev, const int baud, MLog &out)
1828	: Network(dev, baud, out), fObservatory(MObservatory::kMagic1), fZd1(0), fZd2(0), fAz(0), fMac1(0), fMac2(0), fMac3(0), fBackground(kBgdNone)
1829	{
1830	TEnv env(".cosyrc");
1831	const Int_t id1 = env.GetValue("Az_Id-MAC1", 1); //1
1832	const Int_t id2 = env.GetValue("Az_Id-MAC2", 2); //2
1833	const Int_t id3 = env.GetValue("Zd_Id-MAC", 3); //3
1834	const Int_t id4 = env.GetValue("Zd_Id-SE1", 4); //4
1835	const Int_t id5 = env.GetValue("Zd_Id-SE2", 5); //5
1836	const Int_t id6 = env.GetValue("Az_Id-SE", 6); //6
1837
1838	lout << "- Program in ";
1839	switch (mode)
1840	{
1841	case 0:
1842	lout << "<<Stanard mode>>" << endl;
1843	fBending.Load("bending.txt");
1844	Constructor(id1, id2, id3, id4, id5, id6);
1845	break;
1846	case 1:
1847	lout << "<<SE mode>>" << endl;
1848	fBending.Load("bending.txt");
1849	ConstructorSE(id4, id5, id6);
1850	break;
1851	default:
1852	lout << "<<Demo mode>>" << endl;
1853	ConstructorDemo();
1854	}
1855
1856	lout.SetOutputGui(fWin->GetLog(), kTRUE);
1857
1858	fZd1->SetDisplay(fWin->GetLabel2());
1859	fZd2->SetDisplay(fWin->GetLabel3());
1860	fAz->SetDisplay(fWin->GetLabel1());
1861
1862	int i=0;
1863	char name[100];
1864	while (1)
1865	{
1866	sprintf(name, "/home/tbretz/TPoint/tpoint%03d.txt", i++);
1867	if (gSystem->AccessPathName(name, kFileExists))
1868	break;
1869	}
1870
1871	Timer time;
1872	time.Now();
1873
1874	tpout = new ofstream(name);
1875	*tpout << "Magic Model TPOINT data file" << endl;
1876	*tpout << ": ALTAZ" << endl;
1877	*tpout << "49 48 0 ";
1878	*tpout << time.Year() << " " << time.Month() << " " << time.Day() << " ";
1879	tpout << /"20 1013.25 300 0.5 0.55 0.0065" <<*/ endl;
1880	// temp(°C) pressure(mB) height(m) humidity(1) wavelength(microm) troplapserate(K/m)
1881	}
1882
1883	void MCosy::TerminateApp()
1884	{
1885	cout << "MCosy::TerminateApp()" << endl;
1886	/*
1887	Int_t rc;
1888	TGMessageBox msg(this, gClient->GetRoot(),
1889	"Information",
1890	"Cosy is shutting down the system - this may take wa while!",
1891	kMBIconExclamation,
1892	kMBOK, //kMBClose
1893	&rc, 0);
1894	*/
1895
1896	lout.DisableOutputDevice(MLog::eGui);
1897	// FIXME: WHY DOES THIS CRASH THE APPLICATIOn WHILE TRAKING?
1898	// lout.SetOutputGui(NULL, kFALSE);
1899
1900	gApplication->Terminate(0);
1901	}
1902
1903	MCosy::~MCosy()
1904	{
1905	*tpout << "END" << endl;
1906	delete tpout;
1907
1908	cout << "Deleting GUI timer." << endl;
1909
1910	delete fUpdateGui;
1911
1912	cout << "Deleting Nodes." << endl;
1913
1914	delete fAz;
1915	delete fZd1;
1916	delete fZd2;
1917	delete fMac1;
1918	delete fMac2;
1919	if (fMac3)
1920	delete fMac3;
1921
1922	cout << "Deleting MGCosy." << endl;
1923
1924	lout.DisableOutputDevice(MLog::eGui);
1925
1926	delete fWin;
1927
1928	cout << "MGCosy destructed." << endl;
1929	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: