Context Navigation

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

Visit:

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

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

Download in other formats: