direcs  2012-09-30
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
sensorThread.cpp
Go to the documentation of this file.
1 /*************************************************************************
2  * Copyright (C) Markus Knapp *
3  * www.direcs.de *
4  * *
5  * This file is part of direcs. *
6  * *
7  * direcs is free software: you can redistribute it and/or modify it *
8  * under the terms of the GNU General Public License as published *
9  * by the Free Software Foundation, version 3 of the License. *
10  * *
11  * direcs is distributed in the hope that it will be useful, *
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
14  * GNU General Public License for more details. *
15  * *
16  * You should have received a copy of the GNU General Public License *
17  * along with direcs. If not, see <http://www.gnu.org/licenses/>. *
18  * *
19  *************************************************************************/
20 
21 #include "sensorThread.h"
22 
23 SensorThread::SensorThread(InterfaceAvr *i, QMutex *m)
24 {
25  // get the name of this class (this is for debugging messages)
26  className = this->staticMetaObject.className();
27 
28  stopped = false;
29  simulationMode = false;
30 
31  // copy the pointer from the original object
32  interface1 = i;
33  mutex = m;
34 
35  // Array for storing the measured values from the infrared sensors
36  iRSensorValue[SENSOR1] = 0;
37  iRSensorValue[SENSOR2] = 0;
38  iRSensorValue[SENSOR3] = 0;
39  iRSensorValue[SENSOR4] = 0;
40  iRSensorValue[SENSOR5] = 0;
41  iRSensorValue[SENSOR6] = 0;
42 // iRSensorValue[SENSOR7] = 0; -> now voltage 12 V (measuring the power supply / accumulators), sensor 1 !!
43 // iRSensorValue[SENSOR8] = 0; -> now voltage 24 V (measuring the power supply / accumulators) sensor 2 !!
44 
45  // Array for storing the measured voltage values
46  voltageSensorValue[VOLTAGESENSOR1] = 0;
47  voltageSensorValue[VOLTAGESENSOR2] = 0;
48 
49  // for storing a virtual heartbeat value (high=5V)
50  heartbeatValue[0] = 0;
51 
52  // initialisation
53  for (int i=0; i<USSENSORARRAYSIZE; i++)
54  {
55  usSensorValue[i] = 0;
56  }
57 
58  // initialisation
59  for (int i=0; i<MOTORSENSORARRAYSIZE; i++)
60  {
61  motorSensorValue[i] = 0;
62  }
63 
64  // Array for storing the measured contact values
65  // initialisation
66  for (int i=0; i<CONTACTARRAYSIZE; i++)
67  {
68  contactValue[i] = 0;
69  }
70 
71  // initialisation
72  for (int i=0; i<DRIVENDISTANCEARRAYSIZE; i++)
73  {
74  drivenDistance[i] = 0;
75  }
76 
77  // init the 3D compass values
78  xAxis = 0.0;
79  yAxis = 0.0;
80  zAxis = 0.0;
81  heading = 0.0;
82 
83 
84  // These are the measured values from the AD-Conversion from the IR-Sensors
85  // Add 8 to the index, then you have the distance in centimeters (cm)
86  //
87  // Example:
88  //
89  // Value from AD converter = 1011
90  // Index = 0
91  //
92  // 0 + 8 = 8 cm
93  //
94  // 8 cm = 1011
95  //
96  //
97  // This means, that the sensor measures from 8 centimeters (0+8) to 47 (39+8) centimeters.
98  //
99  iRDistance[0] = 1011;
100  iRDistance[1] = 993;
101  iRDistance[2] = 926;
102  iRDistance[3] = 854;
103  iRDistance[4] = 789;
104  iRDistance[5] = 733;
105  iRDistance[6] = 683;
106  iRDistance[7] = 639;
107  iRDistance[8] = 605;
108  iRDistance[9] = 577;
109  iRDistance[10] = 547;
110  iRDistance[11] = 520;
111  iRDistance[12] = 497;
112  iRDistance[13] = 470;
113  iRDistance[14] = 452;
114  iRDistance[15] = 430;
115  iRDistance[16] = 418;
116  iRDistance[17] = 400;
117  iRDistance[18] = 388;
118  iRDistance[19] = 374;
119  iRDistance[20] = 359;
120  iRDistance[21] = 343;
121  iRDistance[22] = 338;
122  iRDistance[23] = 326;
123  iRDistance[24] = 313;
124  iRDistance[25] = 306;
125  iRDistance[26] = 299;
126  iRDistance[27] = 291;
127  iRDistance[28] = 277;
128  iRDistance[29] = 272;
129  iRDistance[30] = 266;
130  iRDistance[31] = 262;
131  iRDistance[32] = 253;
132  iRDistance[33] = 249;
133  iRDistance[34] = 240;
134  iRDistance[35] = 234;
135  iRDistance[36] = 230;
136  iRDistance[37] = 227;
137  iRDistance[38] = 217;
138  iRDistance[39] = 210;
139 
140  robotState = ON; // Wer're thinking positive. The robot is ON untill whe know nothing other. :-)
141  compassState = false;
142 }
143 
144 
145 SensorThread::~SensorThread()
146 {
147  // send "off" or "shutdown" heartbeat signal over network to remote app
148  emit heartbeat(RED);
149 
150  // send heartbeat over the network
151  // *0h2# means 'heartbeat no. 0 is DEAD'
152  emit sendNetworkString("*0h2#");
153 }
154 
155 
156 void SensorThread::stop()
157 {
158  stopped = true;
159 
160  // send "off" or "shutdown" heartbeat signal over network to remote app
161  emit heartbeat(RED);
162 
163  // send heartbeat over the network
164  // *0h2# means 'heartbeat no. 0 is DEAD'
165  emit sendNetworkString("*0h2#");
166 }
167 
168 
169 void SensorThread::run()
170 {
171  bool heartbeatToggle = false;
172 
173 
174  // start "threading"...
175  while (!stopped)
176  {
177  // let the thread sleep some time for having more time for the other threads
178  msleep(THREADSLEEPTIME);
179 
180  if ( (robotState == ON) && (simulationMode == false) )
181  {
182  // Lock the mutex. If another thread has locked the mutex then this call will block until that thread has unlocked it.
183  mutex->lock();
184 
185  //-----------------
186  // voltage sensors
187  //-----------------
188  if (readVoltageSensor(VOLTAGESENSOR1) == false) // sensor 8 is the former infrared sensor 8 ! This is now the 12 V battery!
189  {
190  emit message(QString("<font color=\"#FF0000\">ERROR reading voltage sensor 1. Stopping %1!</font>").arg(className));
191  // Unlock the mutex.
192  mutex->unlock();
193  // stop this thread
194  stop();
195  // inform other modules
196  emit systemerror(-2);
197  return;
198  }
199  // send value over the network
200  // *0v42# means voltagesensor1 with 42 V (the digits after the decimal points are ignored here!)
201  emit sendNetworkString( QString("*%1v%2#").arg(VOLTAGESENSOR1).arg( (int) voltageSensorValue[VOLTAGESENSOR1]));
202 
203  if (readVoltageSensor(VOLTAGESENSOR2) == false) // sensor 7 is the former infrared sensor 7 ! This is now the 24 V battery!
204  {
205  emit message("<font color=\"#FF0000\">ERROR reading voltage sensor 2. Stopping sensorThread!</font>");
206  // Unlock the mutex.
207  mutex->unlock();
208  // stop this thread
209  stop();
210  // inform other modules
211  emit systemerror(-2);
212  return;
213  }
214  // send value over the network
215  // *0v42# means voltagesensor1 with 42 V (the digits after the decimal points are ignored here!)
216  emit sendNetworkString( QString("*%1v%2#").arg(VOLTAGESENSOR2).arg( (int) voltageSensorValue[VOLTAGESENSOR2]));
217 
218 /* @todo: implement this in direcsstm !
219  //---------------
220  // motor sensors
221  //---------------
222  if (readMotorSensor(MOTORSENSOR1) == false)
223  {
224  emit message("<font color=\"#FF0000\">ERROR reading motor sensor 1. Stopping sensorThread!</font>");
225  // Unlock the mutex.
226  mutex->unlock();
227  // stop this thread
228  stop();
229  // inform other modules
230  emit systemerror(-2);
231  return;
232  }
233  // send value over the network
234  // *0m42# means motorsensor1 with 42 mA
235  emit sendNetworkString( QString("*%1m%2#").arg(MOTORSENSOR1).arg(getMAmpere(MOTORSENSOR1)));
236 
237  if (readMotorSensor(MOTORSENSOR2) == false)
238  {
239  emit message("<font color=\"#FF0000\">ERROR reading motor sensor 2. Stopping sensorThread!</font>");
240  // Unlock the mutex.
241  mutex->unlock();
242  // stop this thread
243  stop();
244  return;
245  }
246  // send value over the network
247  // *1m42# means motorsensor2 with 42 mA
248  emit sendNetworkString( QString("*%1m%2#").arg(MOTORSENSOR2).arg(getMAmpere(MOTORSENSOR2)));
249 */
251 /*
252 
253  if (readMotorSensor(MOTORSENSOR3) == false)
254  {
255  emit message("<font color=\"#FF0000\">ERROR reading motor sensor 3. Stopping sensorThread!</font>");
256  // Unlock the mutex.
257  mutex->unlock();
258  // stop this thread
259  // inform other modules
260  emit systemerror(-2);
261  stop();
262  return;
263  }
264  // send value over the network
265  // *1m42# means motorsensor2 with 42 mA
266  emit sendNetworkString( QString("*%1m%2#").arg(MOTORSENSOR3).arg(getMAmpere(MOTORSENSOR3)));
267 
268  if (readMotorSensor(MOTORSENSOR4) == false)
269  {
270  emit message("<font color=\"#FF0000\">ERROR reading motor sensor 4. Stopping sensorThread!</font>");
271  // Unlock the mutex.
272  mutex->unlock();
273  // stop this thread
274  stop();
275  // inform other modules
276  emit systemerror(-2);
277  return;
278  }
279  // send value over the network
280  // *1m42# means motorsensor2 with 42 mA
281  emit sendNetworkString( QString("*%1m%2#").arg(MOTORSENSOR4).arg(getMAmpere(MOTORSENSOR4)));
282 */
283 
284  //====================================================================
285  // send an optical heartbeat signal to the GUI
286  if (!heartbeatToggle)
287  {
288  // set plot value to high = 5 Volt
289  heartbeatValue[0] = MAXIMUMPLOTHEARTBEAT;
290  emit heartbeat(GREEN);
291 
292  // send heartbeat over the network
293  // *0h1# means 'heartbeat no. 0 is HIGH'
294  emit sendNetworkString("*0h1#");
295  }
296  else
297  {
298  // set plot value to low = 0 Volt
299  heartbeatValue[0] = 0;
300  emit heartbeat(LEDOFF);
301 
302  // send heartbeat over the network
303  // *0h1# means 'heartbeat no. 0 is LOW'
304  emit sendNetworkString("*0h0#");
305  }
306  heartbeatToggle = !heartbeatToggle;
307  //====================================================================
308 
309 /* @todo: implement this in direcsstm !
310  //-----------------
311  // driven distance
312  //-----------------
313  if (readDrivenDistance(DRIVENDISTANCE1) == false)
314  {
315  emit message("<font color=\"#FF0000\">ERROR reading driven distance 1. Stopping sensorThread!</font>");
316  // Unlock the mutex.
317  mutex->unlock();
318  // stop this thread
319  stop();
320  // inform other modules
321  emit systemerror(-2);
322  return;
323  }
324 
325  if (readDrivenDistance(DRIVENDISTANCE2) == false)
326  {
327  emit message("<font color=\"#FF0000\">ERROR reading driven distance 2. Stopping sensorThread!</font>");
328  // Unlock the mutex.
329  mutex->unlock();
330  // stop this thread
331  stop();
332  // inform other modules
333  emit systemerror(-2);
334  return;
335  }
336 
337 
338  // if initCircuit found out that the compas module is connected
339  if (compassState == true)
340  {
341  //-------------------------------------------
342  // read value from magnetic sensor / compass
343  //-------------------------------------------
344  if (readCompassAxis(XAXIS) == false)
345  {
346  emit message("<font color=\"#FF0000\">ERROR reading compass axis X. Stopping sensorThread!</font>");
347  // Unlock the mutex.
348  mutex->unlock();
349  // stop this thread
350  stop();
351  // inform other modules
352  emit systemerror(-2);
353  return;
354  }
355  // send value over the network
356  // *xc42# means axis x of the compass has 42°
357  // CONVERT TO INT! Only for displaying!
358  emit sendNetworkString( QString("*xc%1#").arg( (int) xAxis ));
359 
360  if (readCompassAxis(YAXIS) == false)
361  {
362  emit message("<font color=\"#FF0000\">ERROR reading compass axis Y. Stopping sensorThread!</font>");
363  // Unlock the mutex.
364  mutex->unlock();
365  // stop this thread
366  stop();
367  // inform other modules
368  emit systemerror(-2);
369  return;
370  }
371  // send value over the network
372  // *yc42# means axis y of the compass has 42°
373  // CONVERT TO INT! Only for displaying!
374  emit sendNetworkString( QString("*yc%1#").arg( (int) yAxis ));
375 
376  if (readCompassAxis(ZAXIS) == false)
377  {
378  emit message("<font color=\"#FF0000\">ERROR reading compass axis Z. Stopping sensorThread!</font>");
379  // Unlock the mutex.
380  mutex->unlock();
381  // stop this thread
382  stop();
383  // inform other modules
384  emit systemerror(-2);
385  return;
386  }
387  // send value over the network
388  // *zc42# means axis z of the compass has 42°
389  // CONVERT TO INT! Only for displaying!
390  emit sendNetworkString( QString("*zc%1#").arg( (int) zAxis ));
391 
392  // Only *after* all axes were read:
393  calculateHeading();
394 
395  // emit ALL compass axes values
396  emit compassDataComplete(xAxis, yAxis, zAxis, heading);
397  }
398 */
399 /* infrared Sensors temporarily removed from robot!!
400 
401  //------------------
402  // infrared sensors
403  //------------------
404  if (readInfraredSensor(SENSOR1) == false)
405  {
406  emit message("<font color=\"#FF0000\">ERROR reading infrared sensor 1. Stopping sensorThread!</font>");
407  // Unlock the mutex.
408  mutex->unlock();
409  // stop this thread
410  stop();
411  // inform other modules
412  emit systemerror(-2);
413  return;
414  }
415 
416  if (readInfraredSensor(SENSOR2) == false)
417  {
418  emit message("<font color=\"#FF0000\">ERROR reading infrared sensor 2. Stopping sensorThread!</font>");
419  // Unlock the mutex.
420  mutex->unlock();
421  // stop this thread
422  stop();
423  // inform other modules
424  emit systemerror(-2);
425  return;
426  }
427 
428  if (readInfraredSensor(SENSOR3) == false)
429  {
430  emit message("<font color=\"#FF0000\">ERROR reading infrared sensor 3. Stopping sensorThread!</font>");
431  // Unlock the mutex.
432  mutex->unlock();
433  // stop this thread
434  stop();
435  // inform other modules
436  emit systemerror(-2);
437  return;
438  }
439 
440  if (readInfraredSensor(SENSOR4) == false)
441  {
442  emit message("<font color=\"#FF0000\">ERROR reading infrared sensor 4. Stopping sensorThread!</font>");
443  // Unlock the mutex.
444  mutex->unlock();
445  // stop this thread
446  stop();
447  // inform other modules
448  emit systemerror(-2);
449  return;
450  }
451 
452  if (readInfraredSensor(SENSOR5) == false)
453  {
454  emit message("<font color=\"#FF0000\">ERROR reading infrared sensor 5. Stopping sensorThread!</font>");
455  // Unlock the mutex.
456  mutex->unlock();
457  // stop this thread
458  stop();
459  // inform other modules
460  emit systemerror(-2);
461  return;
462  }
463 
464  if (readInfraredSensor(SENSOR6) == false)
465  {
466  emit message("<font color=\"#FF0000\">ERROR reading infrared sensor 6. Stopping sensorThread!</font>");
467  // Unlock the mutex.
468  mutex->unlock();
469  // stop this thread
470  stop();
471  // inform other modules
472  emit systemerror(-2);
473  return;
474  }
475 
476  // infrared sensors 7 and 8 are now the voltage sensors!
477 */
478 /* ultrasonic Sensors temporarily removed from robot!!
479 
480  //---------------------
481  // ulatrasonic sensors
482  //---------------------
483  if (readUltrasonicSensor(SENSOR16) == false)
484  {
485  emit message("<font color=\"#FF0000\">ERROR reading ultrasonic sensor. Stopping sensorThread!</font>");
486  // Unlock the mutex.
487  mutex->unlock();
488  // stop this thread
489  stop();
490  // inform other modules
491  emit systemerror(-2);
492  return;
493  }
494 */
495 
496 /* contacts temporarily removed from robot!!
497  // read value of contact 1 (cam pan L)
498  if (readContact(CONTACT1) == false)
499  {
500  emit message("<font color=\"#FF0000\">ERROR reading contact sensor 1. Stopping sensorThread!</font>");
501  // Unlock the mutex.
502  mutex->unlock();
503  // stop this thread
504  stop();
505  // inform other modules
506  emit systemerror(-2);
507  return;
508  }
509  else
510  {
511  // emit
512  if (contactValue[CONTACT1] == 0)
513  {
514  emit contactAlarm(LEFT, false);
515  }
516  else
517  {
518  emit contactAlarm(LEFT, true);
519  }
520  }
521 
522  // read value of contact 2 (cam pan R)
523  if (readContact(CONTACT2) == false)
524  {
525  emit message("<font color=\"#FF0000\">ERROR reading contact sensor 2. Stopping sensorThread!</font>");
526  // Unlock the mutex.
527  mutex->unlock();
528  // stop this thread
529  stop();
530  // inform other modules
531  emit systemerror(-2);
532  return;
533  }
534  else
535  {
536  // emit
537  if (contactValue[CONTACT2] == 0)
538  {
539  emit contactAlarm(RIGHT, false);
540  }
541  else
542  {
543  emit contactAlarm(RIGHT, true);
544  }
545  }
546 
547  // read value of contact 3 (cam tilt L/TOP)
548  if (readContact(CONTACT3) == false)
549  {
550  emit message("<font color=\"#FF0000\">ERROR reading contact sensor 3. Stopping sensorThread!</font>");
551  // Unlock the mutex.
552  mutex->unlock();
553  // stop this thread
554  stop();
555  // inform other modules
556  emit systemerror(-2);
557  return;
558  }
559  else
560  {
561  // emit
562  if (contactValue[CONTACT3] == 0)
563  {
564  emit contactAlarm(TOP, false);
565  }
566  else
567  {
568  emit contactAlarm(TOP, true);
569  }
570  }
571 
572  // read value of contact 4 (cam tilt R/BOTTOM)
573  if (readContact(CONTACT4) == false)
574  {
575  emit message("<font color=\"#FF0000\">ERROR reading contact sensor 4. Stopping sensorThread!</font>");
576  // Unlock the mutex.
577  mutex->unlock();
578  // stop this thread
579  stop();
580  // inform other modules
581  emit systemerror(-2);
582  return;
583  }
584  else
585  {
586  // emit
587  if (contactValue[CONTACT1] == 0)
588  {
589  emit contactAlarm(BOTTOM, false);
590  }
591  else
592  {
593  emit contactAlarm(BOTTOM, true);
594  }
595  }
596 contacts temporarily removed from robot!! */
597 
598  // Unlock the mutex.
599  mutex->unlock();
600 
601  } // simulation = false
602 
603  if (simulationMode)
604  {
605  // now we're sleeping additional 500ms because we're only simulating.
606  msleep(500);
607 
608  // send an (simulated) optical heartbeat signal to the GUI
609  if (!heartbeatToggle)
610  {
611  emit heartbeat(GREEN);
612  }
613  else
614  {
615  emit heartbeat(LEDOFF);
616  }
617  heartbeatToggle = !heartbeatToggle;
618  }
619 
620  // e m i t Signal
621  emit sensorDataComplete();
622  }
623  stopped = false;
624 }
625 
626 
627 int SensorThread::convertToDistance(int sensorValue)
628 {
629  unsigned char calibrationValue = 8;
630 
631 
632  // search the array for the measured value
633  for (int n=0; n<=(IRSENSORARRAYSIZE-1); n++)
634  {
635  if (sensorValue >= iRDistance[n])
636  {
637  // return the index number plus calibration value
638  // (this IS the measured distance in cm!!)
639  return (n+calibrationValue);
640  }
641  }
642 
643  // value not found in the array
644  // return the highest DISTANCE (the highest index number plus calibration value!)
645  return (IRSENSORARRAYSIZE + calibrationValue);
646 }
647 
648 
649 int SensorThread::convertToSensorValue(int distance)
650 {
651  unsigned char calibrationValue = 8;
652 
653 
654  if ((distance-calibrationValue) <= 0)
655  {
656  return iRDistance[0];
657  }
658 
659  if ((distance-calibrationValue) >= IRSENSORARRAYSIZE-1)
660  {
661  return iRDistance[IRSENSORARRAYSIZE-1];
662  }
663 
664  return (iRDistance[distance-calibrationValue]);
665 }
666 
667 
668 int SensorThread::getMAmpere(int sensor)
669 {
670  if ((sensor < MOTORSENSOR1) || (sensor > MOTORSENSOR4))
671  {
672  qDebug("ERROR sensorThread, getMAmpere: wrong motor sensor");
673  return 0;
674  }
675 
676 
677  // convert the measured value to mili Ampere (mA)
678  //
679  // sensorValue 29 = 1000 mili Ampere (mA)
680  //
681  // return AND convert to mA !!
682  return (motorSensorValue[sensor] * CONVERSIONFACTORMOTORSENSOR);
683 }
684 
685 
686 float SensorThread::getVoltage(int sensor)
687 {
688  if ((sensor < VOLTAGESENSOR1) || (sensor > VOLTAGESENSOR2))
689  {
690  qDebug("ERROR sensorThread, getVoltage: wrong voltage sensor");
691  return 0;
692  }
693 
694 
695  // return the converted measured value in Volt (V)
696  return convertToVolt(sensor);
697 }
698 
699 
700 int SensorThread::getContactValue(int contact)
701 {
702  if ((contact < CONTACT1) || (contact > CONTACT4))
703  {
704  qDebug("ERROR sensorThread, getContactValue: wrong contact number");
705  return 0;
706  }
707 
708  // typecasting for convenience!
709  return (int) contactValue[contact];
710 }
711 
712 
713 int SensorThread::getIrSensorValue(int sensor)
714 {
715  if ((sensor < SENSOR1) || (sensor > SENSOR8))
716  {
717  qDebug("ERROR: wrong ir sensor");
718  return 0;
719  }
720 
721  return iRSensorValue[sensor];
722 }
723 
724 
725 int SensorThread::getDistance(int sensor)
726 {
727  int distance = 0;
728 
729  if ((sensor < SENSOR1) || (sensor > SENSOR8))
730  {
731  qDebug("ERROR: wrong ir sensor");
732  return 0;
733  }
734 
735  // get the stored distance
736  distance = convertToDistance(iRSensorValue[sensor]);
737 
738  //qDebug("sensor: %1", sensor);
739 
740  //--------------------------------------------------------------------------------------
741  // correct the value from sensors because of their position on the bot
742  // e.g. the measured distance from the sensor is not the same then this from the bot.
743  //--------------------------------------------------------------------------------------
744  if (sensor == SENSOR1) // sensor front left
745  distance -= 9;
746 
747  if (sensor == SENSOR2) // sensor front right
748  distance -= 9;
749 
750  if (sensor == SENSOR3) // sensor back right
751  distance -= 9;
752 
753  if (sensor == SENSOR4) // sensor back left
754  distance -= 7;
755 
756  if (sensor == SENSOR5) // sensor left
757  distance -= 1;
758 
759  if (sensor == SENSOR6) // front senor (under the laser)
760  distance -= 19;
761 
762  if (sensor == SENSOR7) // sensor right
763  distance -= 1;
764 
765  if (sensor == SENSOR8) // middle back senor
766  distance -= 9;
767 
768 
769 
770  return distance;
771 }
772 
773 
774 int SensorThread::getDrivenDistance(int sensor)
775 {
776  if ((sensor < MOTORSENSOR1) || (sensor > DRIVENDISTANCEARRAYSIZE-1))
777  {
778  qDebug("ERROR: wrong motor sensor");
779  return 0;
780  }
781 
782  return drivenDistance[sensor];
783 }
784 
785 
786 bool SensorThread::resetDrivenDistance(int sensor)
787 {
788  QString answer = "error";
789 
790 
791  if ((sensor < MOTORSENSOR1) || (sensor > DRIVENDISTANCEARRAYSIZE-1))
792  {
793  qDebug("ERROR: wrong motor sensor '%d' at resetDrivenDistance()", sensor);
794  return false;
795  }
796 
797  // Lock the mutex. If another thread has locked the mutex then this call will block until that thread has unlocked it.
798  mutex->lock();
799 
800  switch (sensor)
801  {
802  case MOTORSENSOR1:
803  // send command 'init distance 1'
804  if (interface1->sendString("id1", className) == true)
805  {
806  // check if the robot answers with "ok"
807  if ( interface1->receiveString(answer, className) == true)
808  {
809  if (answer == "*ok#")
810  {
811  // Unlock the mutex
812  mutex->unlock();
813  return true;
814  }
815  }
816  }
817 
818  // error
819  emit message("<font color=\"#FF0000\">ERROR resetting driven distance 1. Stopping sensorThread!</font>");
820  // Unlock the mutex.
821  mutex->unlock();
822  // stop this thread
823  stop();
824  // inform other modules
825  emit systemerror(-2);
826  return false;
827  break;
828  case MOTORSENSOR2:
829  // send command 'init distance 2'
830  if (interface1->sendString("id2", className) == true)
831  {
832  // check if the robot answers with "ok"
833  if ( interface1->receiveString(answer, className) == true)
834  {
835  if (answer == "*ok#")
836  {
837  // Unlock the mutex
838  mutex->unlock();
839  return true;
840  }
841  }
842  }
843 
844  // error
845  emit message("<font color=\"#FF0000\">ERROR resetting driven distance 2. Stopping sensorThread!</font>");
846  // Unlock the mutex.
847  mutex->unlock();
848  // stop this thread
849  stop();
850  // inform other modules
851  emit systemerror(-2);
852  return false;
853  break;
854  }
855 
856  // Unlocks the mutex.
857  mutex->unlock();
858  return false;
859 }
860 
861 
862 int SensorThread::getUsSensorValue(int sensor)
863 {
864  // This value represents the distance from the sensor to the front of the robot in cm.
865  // So this value is substracted from the returned result to give the distance from the robot to the next obstacle
866  // (and not the distance from the sensor to the next obstacle)
867  unsigned char calibrationValue = 53;
868 
869 
870  if ((sensor < SENSOR16) || (sensor > SENSOR16))
871  {
872  qDebug("ERROR: wrong us sensor");
873  return 0;
874  }
875 
876  switch (sensor)
877  {
878  case SENSOR16:
879  return (usSensorValue[0] - calibrationValue);
880  }
881 
882  // this line is never reached
883  return 0;
884 }
885 
886 
887 int SensorThread::getMotorSensorValue(int sensor)
888 {
889  //
890  // These values were NOT converted at any point in this method!
891  //
892 
893  if ((sensor < MOTORSENSOR1) || (sensor > MOTORSENSORARRAYSIZE-1))
894  {
895  qDebug("ERROR: wrong motor sensor");
896  return 0;
897  }
898 
899 
900  return motorSensorValue[sensor];
901 }
902 
903 
904 void SensorThread::setSimulationMode(bool state)
905 {
906  simulationMode = state;
907 
908  // fill array with some nice values
909  if (simulationMode == true)
910  {
911  // FIXME: Values are not correct / corresponding to values in GUI. Only in sim mode!
912  // all values in cm
913  iRSensorValue[SENSOR1] = convertToSensorValue(30);
914  iRSensorValue[SENSOR2] = convertToSensorValue(60);
915  iRSensorValue[SENSOR3] = convertToSensorValue(25);
916  iRSensorValue[SENSOR4] = convertToSensorValue(27);
917  iRSensorValue[SENSOR5] = convertToSensorValue(8);
918  iRSensorValue[SENSOR6] = convertToSensorValue(40);
919  iRSensorValue[SENSOR7] = convertToSensorValue(10);
920  iRSensorValue[SENSOR8] = convertToSensorValue(59);
921 
922  // all values in cm
923  usSensorValue[0] = 28;
924 
925  motorSensorValue[MOTORSENSOR1] = (int)1000/CONVERSIONFACTORMOTORSENSOR;
926  motorSensorValue[MOTORSENSOR2] = (int)2000/CONVERSIONFACTORMOTORSENSOR;
927  motorSensorValue[MOTORSENSOR3] = (int)3000/CONVERSIONFACTORMOTORSENSOR;
928  motorSensorValue[MOTORSENSOR4] = (int)4000/CONVERSIONFACTORMOTORSENSOR;
929 
930  voltageSensorValue[VOLTAGESENSOR1] = 10.500000 * CONVERSIONFACTORVOLTAGESENSOR1; // simulate an empty 12 V battery
931  voltageSensorValue[VOLTAGESENSOR2] = 24.000000 * CONVERSIONFACTORVOLTAGESENSOR2; // simulate a full 24 V battery
932 
933  // initialisation
934  for (int i=0; i<CONTACTARRAYSIZE; i++)
935  {
936  contactValue[i] = 0;
937  }
938 
939  // initialisation
940  for (int i=0; i<DRIVENDISTANCEARRAYSIZE; i++)
941  {
942  drivenDistance[i] = 42;
943  }
944 
945  // compass test values
946  xAxis = READ_AXIS_X;
947  yAxis = READ_AXIS_Y;
948  zAxis = READ_AXIS_Z;
949  heading = 42;
950  }
951  else
952  {
953  if (robotState==OFF)
954  emit heartbeat(RED);
955 
956  iRSensorValue[SENSOR1] = 0;
957  iRSensorValue[SENSOR2] = 0;
958  iRSensorValue[SENSOR3] = 0;
959  iRSensorValue[SENSOR4] = 0;
960  iRSensorValue[SENSOR5] = 0;
961  iRSensorValue[SENSOR6] = 0;
962  iRSensorValue[SENSOR7] = 0;
963  iRSensorValue[SENSOR8] = 0;
964 
965  // initialisation
966  for (int i=0; i<USSENSORARRAYSIZE; i++)
967  {
968  usSensorValue[i] = 53; // +53 = 0cm from the bot
969  }
970 
971  // initialisation
972  for (int i=0; i<MOTORSENSORARRAYSIZE; i++)
973  {
974  motorSensorValue[i] = 0;
975  }
976 
977  voltageSensorValue[VOLTAGESENSOR1] = 0;
978  voltageSensorValue[VOLTAGESENSOR2] = 0;
979 
980  // initialisation
981  for (int i=0; i<CONTACTARRAYSIZE; i++)
982  {
983  contactValue[i] = 0;
984  }
985 
986  // initialisation
987  for (int i=0; i<DRIVENDISTANCEARRAYSIZE; i++)
988  {
989  drivenDistance[i] = 0;
990  }
991 
992  xAxis = 0;
993  yAxis = 0;
994  zAxis = 0;
995  }
996 }
997 
998 /*
999 int SensorThread::getCompassValue(unsigned char axis)
1000 {
1001  switch (axis)
1002  {
1003  case READ_AXIS_X: // this is wrong! has to be XAXIS!
1004  return xAxis;
1005  break;
1006  case READ_AXIS_Y:
1007  return yAxis;
1008  break;
1009  case READ_AXIS_Z:
1010  return zAxis;
1011  break;
1012  }
1013 
1014  // this line should never be reached!
1015  qDebug("WARNING: wrong axis number in getCompassValue");
1016  return -1;
1017 }
1018 */
1019 
1020 void SensorThread::setRobotState(bool state)
1021 {
1022  // store the state within this class
1023  robotState = state;
1024 }
1025 
1026 void SensorThread::setCompassState(bool state)
1027 {
1028  // store the state within this class
1029  compassState = state;
1030 }
1031 
1032 
1033 float SensorThread::convertToDegree(int sensorValue)
1034 {
1035  // The sensorValue comes as a 16 bit integer 2's complement from the sensor!
1036  // So do we have a negative value?
1037  if ( (sensorValue & 32768) == 32768)
1038  {
1039  // delete the 'sign' bit
1040  sensorValue -= 32768;
1041 
1042  // convert into negative
1043  sensorValue = sensorValue * -1;
1044  }
1045 
1046  // 'value' has to be between 0 and 65536, because we're using a 16 bit value
1047  //
1048  // A compass has 360 degrees, so the factor for 1 degree is 360/65536
1049  return ( (float) sensorValue * (360.0 / 65536.0));
1050 // return sensorValue;
1051 }
1052 
1053 
1054 void SensorThread::calculateHeading(void)
1055 {
1056  if (xAxis == 0 && yAxis < 0)
1057  {
1058  heading = M_PI/2;
1059  }
1060 
1061  if (xAxis == 0 && yAxis > 0)
1062  {
1063  heading = 3*M_PI/2;
1064  }
1065 
1066  if (xAxis < 0)
1067  {
1068  heading = M_PI - atan(float(yAxis)/float(xAxis));
1069  }
1070 
1071  if (xAxis > 0 && yAxis < 0)
1072  {
1073  heading = -atan(float(yAxis)/float(xAxis));
1074  }
1075 
1076  if (xAxis > 0 && yAxis > 0)
1077  {
1078  heading = 2*M_PI - atan(float(yAxis)/float(xAxis));
1079  }
1080 }
1081 
1082 
1083 float SensorThread::convertToVolt(int sensor)
1084 {
1085  if ((sensor < VOLTAGESENSOR1) || (sensor > VOLTAGESENSOR2))
1086  {
1087  qDebug("ERROR sensorThread, convertToVolt: wrong voltage sensor");
1088  return 0;
1089  }
1090 
1091 
1092  if (sensor == VOLTAGESENSOR1)
1093  {
1094  // convert the measured value to Volt (V)
1095  return ( voltageSensorValue[sensor] / CONVERSIONFACTORVOLTAGESENSOR1 );
1096  }
1097 
1098  if (sensor == VOLTAGESENSOR2)
1099  {
1100  // convert the measured value to Volt (V)
1101  return ( voltageSensorValue[sensor] / CONVERSIONFACTORVOLTAGESENSOR2 );
1102  }
1103 
1104  return -1.0;
1105 }
1106 
1107 
1108 bool SensorThread::readInfraredSensor(short int sensor)
1109 {
1110  int value = 0;
1111 
1112  switch (sensor)
1113  {
1114  case SENSOR1:
1115  // read sensor
1116  if (interface1->sendChar(READ_SENSOR_1) == true)
1117  {
1118  // receive the 16 Bit answer from the MC
1119  if (interface1->receiveInt(&value) == false)
1120  {
1121  iRSensorValue[SENSOR1] = 0;
1122  //qDebug("ERROR reading infrared sensor 1");
1123  return false;
1124  }
1125 
1126  // store measured value
1127  iRSensorValue[SENSOR1] = value;
1128  return true;
1129  }
1130  else
1131  {
1132  //qDebug("ERROR reading infrared sensor 1");
1133  return false;
1134  }
1135  break;
1136  case SENSOR2:
1137  // read sensor
1138  if (interface1->sendChar(READ_SENSOR_2) == true)
1139  {
1140  // receive the 16 Bit answer from the MC
1141  if (interface1->receiveInt(&value) == false)
1142  {
1143  iRSensorValue[SENSOR2] = 0;
1144  //qDebug("ERROR reading infrared sensor 2");
1145  return false;
1146  }
1147 
1148  // store measured value
1149  iRSensorValue[SENSOR2] = value;
1150  return true;
1151  }
1152  else
1153  {
1154  //qDebug("ERROR reading infrared sensor 2");
1155  return false;
1156  }
1157  break;
1158  case SENSOR3:
1159  // read sensor
1160  if (interface1->sendChar(READ_SENSOR_3) == true)
1161  {
1162  // receive the 16 Bit answer from the MC
1163  if (interface1->receiveInt(&value) == false)
1164  {
1165  iRSensorValue[SENSOR3] = 0;
1166  //qDebug("ERROR reading infrared sensor 3");
1167  return false;
1168  }
1169 
1170  // store measured value
1171  iRSensorValue[SENSOR3] = value;
1172  return true;
1173  }
1174  else
1175  {
1176  //qDebug("ERROR reading infrared sensor 3");
1177  return false;
1178  }
1179  break;
1180  case SENSOR4:
1181  // read sensor
1182  if (interface1->sendChar(READ_SENSOR_4) == true)
1183  {
1184  // receive the 16 Bit answer from the MC
1185  if (interface1->receiveInt(&value) == false)
1186  {
1187  iRSensorValue[SENSOR4] = 0;
1188  //qDebug("ERROR reading infrared sensor 4");
1189  return false;
1190  }
1191 
1192  // store measured value
1193  iRSensorValue[SENSOR4] = value;
1194  return true;
1195  }
1196  else
1197  {
1198  //qDebug("ERROR reading infrared sensor 4");
1199  return false;
1200  }
1201  break;
1202  case SENSOR5:
1203  // read sensor
1204  if (interface1->sendChar(READ_SENSOR_5) == true)
1205  {
1206  // receive the 16 Bit answer from the MC
1207  if (interface1->receiveInt(&value) == false)
1208  {
1209  iRSensorValue[SENSOR5] = 0;
1210  //qDebug("ERROR reading infrared sensor 5");
1211  return false;
1212  }
1213 
1214  // store measured value
1215  iRSensorValue[SENSOR5] = value;
1216  return true;
1217  }
1218  else
1219  {
1220  //qDebug("ERROR reading infrared sensor 5");
1221  return false;
1222  }
1223  break;
1224  case SENSOR6:
1225  // read sensor
1226  if (interface1->sendChar(READ_SENSOR_6) == true)
1227  {
1228  // receive the 16 Bit answer from the MC
1229  if (interface1->receiveInt(&value) == false)
1230  {
1231  iRSensorValue[SENSOR6] = 0;
1232  //qDebug("ERROR reading infrared sensor 6");
1233  return false;
1234  }
1235 
1236  // store measured value
1237  iRSensorValue[SENSOR6] = value;
1238  return true;
1239  }
1240  else
1241  {
1242  //qDebug("ERROR reading infrared sensor 6");
1243  return false;
1244  }
1245  break;
1246  case SENSOR7:
1247  // read sensor
1248  if (interface1->sendChar(READ_SENSOR_7) == true)
1249  {
1250  // receive the 16 Bit answer from the MC
1251  if (interface1->receiveInt(&value) == false)
1252  {
1253  iRSensorValue[SENSOR7] = 0;
1254  //qDebug("ERROR reading infrared sensor 7");
1255  return false;
1256  }
1257 
1258  // store measured value
1259  iRSensorValue[SENSOR7] = value;
1260  return true;
1261  }
1262  else
1263  {
1264  //qDebug("ERROR reading infrared sensor 7");
1265  return false;
1266  }
1267  break;
1268  case SENSOR8:
1269  // read sensor
1270  if (interface1->sendChar(READ_SENSOR_8) == true)
1271  {
1272  // receive the 16 Bit answer from the MC
1273  if (interface1->receiveInt(&value) == false)
1274  {
1275  iRSensorValue[SENSOR8] = 0;
1276  //qDebug("ERROR reading infrared sensor 8");
1277  return false;
1278  }
1279 
1280  // store measured value
1281  iRSensorValue[SENSOR8] = value;
1282  return true;
1283  }
1284  else
1285  {
1286  //qDebug("ERROR reading infrared sensor 8");
1287  return false;
1288  }
1289  break;
1290  }
1291 
1292  // this line should be never reached
1293  qDebug("WARNING: wrong sensor number in readInfraredSensor()");
1294  return false;
1295 }
1296 
1297 
1298 bool SensorThread::readUltrasonicSensor(short int sensor)
1299 {
1300  int value = 0;
1301 
1302  switch (sensor)
1303  {
1304  case SENSOR16:
1305  // read sensor
1306  if (interface1->sendChar(READ_SENSOR_16) == true)
1307  {
1308  // receive the 16 Bit answer from the MC
1309  if (interface1->receiveInt(&value) == false)
1310  {
1311  usSensorValue[ULTRASONICSENSOR1] = 0;
1312  //qDebug("ERROR reading ultrasonic sensor 1");
1313  return false;
1314  }
1315 
1316  // store measured value
1317  usSensorValue[ULTRASONICSENSOR1] = value;
1318  return true;
1319  }
1320  else
1321  {
1322  //qDebug("ERROR reading ultrasonic sensor 1");
1323  return false;
1324  }
1325  break;
1326  }
1327 
1328  // this line should be never reached
1329  qDebug("WARNING: wrong sensor number in readUltrasonicSensor()");
1330  return false;
1331 }
1332 
1333 
1334 bool SensorThread::readVoltageSensor(short int sensor)
1335 {
1336  int value = 0;
1337  QString answer = "error";
1338 
1339 
1340  switch (sensor)
1341  {
1342  case VOLTAGESENSOR1:
1343  // read sensor
1344  if (interface1->sendString("s8", className) == true) // sensor 8 is the former infrared sensor 8 ! This is now the 12 V battery!
1345  {
1346  // check if the robot answers with answer. e.g. "*42#"
1347  if (interface1->receiveString(answer, className) == true)
1348  {
1349  // convert to int
1350  if (interface1->convertStringToInt(answer, value))
1351  {
1352  // store measured value
1353  voltageSensorValue[VOLTAGESENSOR1] = value;
1354  return true;
1355  }
1356  }
1357  }
1358 
1359  // error
1360  voltageSensorValue[VOLTAGESENSOR1] = 0;
1361  return false;
1362  break;
1363  case VOLTAGESENSOR2:
1364  // read sensor
1365  if (interface1->sendString("s7", className) == true) // sensor 7 is the former infrared sensor 7 ! This is now the 24 V battery!
1366  {
1367  // check if the robot answers with answer. e.g. "*42#"
1368  if (interface1->receiveString(answer, className) == true)
1369  {
1370  // convert to int
1371  if (interface1->convertStringToInt(answer, value))
1372  {
1373  // store measured value
1374  voltageSensorValue[VOLTAGESENSOR2] = value;
1375  return true;
1376  }
1377  }
1378  }
1379 
1380  // error
1381  voltageSensorValue[VOLTAGESENSOR2] = 0;
1382  return false;
1383  break;
1384  }
1385 
1386  // this line should be never reached
1387  qDebug("WARNING: wrong sensor number in readVoltageSensor()");
1388  return false;
1389 }
1390 
1391 
1392 bool SensorThread::readMotorSensor(short int sensor)
1393 {
1394  int value = 0;
1395  QString answer = "error";
1396 
1397  switch (sensor)
1398  {
1399  case MOTORSENSOR1:
1400  // read sensor
1401  if (interface1->sendString("ms1", className) == true)
1402  {
1403  // check if the robot answers with answer. e.g. "*42#"
1404  if (interface1->receiveString(answer, className) == true)
1405  {
1406  // convert to int
1407  if (interface1->convertStringToInt(answer, value))
1408  {
1409  // store measured value
1410  motorSensorValue[MOTORSENSOR1] = value;
1411  return true;
1412  }
1413  }
1414  }
1415 
1416  // error
1417  motorSensorValue[MOTORSENSOR1] = 0;
1418  return false;
1419  break;
1420  case MOTORSENSOR2:
1421  // read sensor
1422  if (interface1->sendString("ms2", className) == true)
1423  {
1424  // check if the robot answers with answer. e.g. "*42#"
1425  if (interface1->receiveString(answer, className) == true)
1426  {
1427  // convert to int
1428  if (interface1->convertStringToInt(answer, value))
1429  {
1430  // store measured value
1431  motorSensorValue[MOTORSENSOR2] = value;
1432  return true;
1433  }
1434  }
1435  }
1436 
1437  // error
1438  motorSensorValue[MOTORSENSOR2] = 0;
1439  return false;
1440  break;
1441  case MOTORSENSOR3:
1442  qDebug("ERROR reading motor sensor 3 not implented yet!!"); // TODO implement reading motor sensor 3
1443  /*
1444  // read sensor
1445  if (interface1->sendString("ms3") == true)
1446  {
1447  // check if the robot answers with answer. e.g. "*42#"
1448  if (interface1->receiveString(answer) == true)
1449  {
1450  // convert to int
1451  if (interface1->convertStringToInt(answer, value))
1452  {
1453  // store measured value
1454  motorSensorValue[MOTORSENSOR3] = value;
1455  return true;
1456  }
1457  }
1458  }
1459 
1460  // error
1461  motorSensorValue[MOTORSENSOR3] = 0;
1462  return false;
1463  break;
1464  */
1465  break;
1466  case MOTORSENSOR4:
1467  qDebug("ERROR reading motor sensor 4 not implented yet!!"); // TODO implement reading motor sensor 4
1468  /*
1469  // read sensor
1470  if (interface1->sendString("ms4") == true)
1471  {
1472  // check if the robot answers with answer. e.g. "*42#"
1473  if (interface1->receiveString(answer) == true)
1474  {
1475  // convert to int
1476  if (interface1->convertStringToInt(answer, value))
1477  {
1478  // store measured value
1479  motorSensorValue[MOTORSENSOR4] = value;
1480  return true;
1481  }
1482  }
1483  }
1484 
1485  // error
1486  motorSensorValue[MOTORSENSOR4] = 0;
1487  return false;
1488  break;
1489  */
1490  break;
1491  }
1492 
1493  // this line should be never reached
1494  qDebug("WARNING: wrong sensor number in readMotorSensor()");
1495  return false;
1496 }
1497 
1498 
1499 bool SensorThread::readDrivenDistance(short int sensor)
1500 {
1501  int value = 0;
1502  QString answer = "error";
1503 
1504 
1505  switch (sensor)
1506  {
1507  case DRIVENDISTANCE1:
1508  // read sensor
1509  if (interface1->sendString("dd1", className) == true)
1510  {
1511  // check if the robot answers with answer. e.g. "*42#"
1512  if (interface1->receiveString(answer, className) == true)
1513  {
1514  // convert to int
1515  if (interface1->convertStringToInt(answer, value))
1516  {
1517  // store measured value
1518  drivenDistance[DRIVENDISTANCE1] = value;
1519  return true;
1520  }
1521  }
1522  }
1523 
1524  // error
1525  drivenDistance[DRIVENDISTANCE1] = 0;
1526  return false;
1527  break;
1528  case DRIVENDISTANCE2:
1529  // read sensor
1530  if (interface1->sendString("dd2", className) == true)
1531  {
1532  // check if the robot answers with answer. e.g. "*42#"
1533  if (interface1->receiveString(answer, className) == true)
1534  {
1535  // convert to int
1536  if (interface1->convertStringToInt(answer, value))
1537  {
1538  // store measured value
1539  drivenDistance[DRIVENDISTANCE2] = value;
1540  return true;
1541  }
1542  }
1543  }
1544 
1545  // error
1546  drivenDistance[DRIVENDISTANCE2] = 0;
1547  return false;
1548  break;
1549  }
1550 
1551  // this line should be never reached
1552  qDebug("WARNING: wrong 'driven distance'' number in readDrivenDistance()");
1553  return false;
1554 }
1555 
1556 
1557 bool SensorThread::readCompassAxis(short int axis)
1558 {
1559  int value = 0;
1560  QString answer = "error";
1561 
1562 
1563  switch (axis)
1564  {
1565  case XAXIS:
1566  // read sensor
1567  if (interface1->sendString("cx", className) == true)
1568  {
1569  // check if the robot answers with answer. e.g. "*42#"
1570  if (interface1->receiveString(answer, className) == true)
1571  {
1572  // convert to int
1573  if (interface1->convertStringToInt(answer, value))
1574  {
1575  // convert the value to degrees and store the value in the class member
1576  xAxis = convertToDegree(value);
1577  return true;
1578  }
1579  }
1580  }
1581 
1582  // error
1583  xAxis = 0;
1584  return false;
1585  break;
1586  case YAXIS:
1587  // read sensor
1588  if (interface1->sendString("cy", className) == true)
1589  {
1590  // check if the robot answers with answer. e.g. "*42#"
1591  if (interface1->receiveString(answer, className) == true)
1592  {
1593  // convert to int
1594  if (interface1->convertStringToInt(answer, value))
1595  {
1596  // convert the value to degrees and store the value in the class member
1597  yAxis = convertToDegree(value);
1598  return true;
1599  }
1600  }
1601  }
1602 
1603  // error
1604  yAxis = 0;
1605  return false;
1606  break;
1607  case ZAXIS:
1608  // read sensor
1609  if (interface1->sendString("cz", className) == true)
1610  {
1611  // check if the robot answers with answer. e.g. "*42#"
1612  if (interface1->receiveString(answer, className) == true)
1613  {
1614  // convert to int
1615  if (interface1->convertStringToInt(answer, value))
1616  {
1617  // convert the value to degrees and store the value in the class member
1618  zAxis = convertToDegree(value);
1619  return true;
1620  }
1621  }
1622  }
1623 
1624  // error
1625  zAxis = 0;
1626  return false;
1627  break;
1628  }
1629 
1630  // this line should be never reached
1631  qDebug("WARNING: wrong compass axis number in readCompassAxis()");
1632  return false;
1633 }
1634 
1635 
1636 bool SensorThread::readContact(short int contact)
1637 {
1638  int value = 0;
1639 
1640  switch (contact)
1641  {
1642  case CONTACT1:
1643  // read sensor
1644  if (interface1->sendChar(READ_CONTACT1) == true)
1645  {
1646  // receive the 16 Bit answer from the MC
1647  if (interface1->receiveInt(&value) == false)
1648  {
1649  contactValue[CONTACT1] = 0;
1650  //qDebug("ERROR reading contact");
1651  return false;
1652  }
1653 
1654  contactValue[CONTACT1] = value;
1655  return true;
1656  }
1657  else
1658  {
1659  //qDebug("ERROR reading contact");
1660  return false;
1661  }
1662  break;
1663  case CONTACT2:
1664  // read sensor
1665  if (interface1->sendChar(READ_CONTACT2) == true)
1666  {
1667  // receive the 16 Bit answer from the MC
1668  if (interface1->receiveInt(&value) == false)
1669  {
1670  contactValue[CONTACT2] = 0;
1671  //qDebug("ERROR reading contact");
1672  return false;
1673  }
1674 
1675  contactValue[CONTACT2] = value;
1676  return true;
1677  }
1678  else
1679  {
1680  //qDebug("ERROR reading contact");
1681  return false;
1682  }
1683  break;
1684  case CONTACT3:
1685  // read sensor
1686  if (interface1->sendChar(READ_CONTACT3) == true)
1687  {
1688  // receive the 16 Bit answer from the MC
1689  if (interface1->receiveInt(&value) == false)
1690  {
1691  contactValue[CONTACT3] = 0;
1692  //qDebug("ERROR reading contact");
1693  return false;
1694  }
1695 
1696  contactValue[CONTACT3] = value;
1697  return true;
1698  }
1699  else
1700  {
1701  //qDebug("ERROR reading contact");
1702  return false;
1703  }
1704  break;
1705  case CONTACT4:
1706  // read sensor
1707  if (interface1->sendChar(READ_CONTACT4) == true)
1708  {
1709  // receive the 16 Bit answer from the MC
1710  if (interface1->receiveInt(&value) == false)
1711  {
1712  contactValue[CONTACT4] = 0;
1713  //qDebug("ERROR reading contact");
1714  return false;
1715  }
1716 
1717  contactValue[CONTACT4] = value;
1718  return true;
1719  }
1720  else
1721  {
1722  //qDebug("ERROR reading contact");
1723  return false;
1724  }
1725  break;
1726  }
1727 
1728  // this line should be never reached
1729  qDebug("WARNING: wrong contact number in readContact()");
1730  return false;
1731 }
1732 
1733 
1734 int SensorThread::getHeartbeatValue()
1735 {
1736  return heartbeatValue[0];
1737 }