direcs  2012-09-30
 All Classes Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
motor.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 "motor.h"
22 
23 Motor::Motor(InterfaceAvr *i, QMutex *m)
24 {
25  // get the name of this class (this is for debugging messages)
26  className = this->staticMetaObject.className();
27 
28  // copy the pointer from the original object
29  interface1 = i;
30  mutex = m;
31 
32  // Initialize the motor step counters
33  //steps1 = 0;
34  //steps2 = 0;
35 
36  // Initialize the motor revolutions counters
37  revolutions1 = 0;
38  revolutions2 = 0;
39  revolutions3 = 0;
40  revolutions4 = 0;
41 
42  // Initialize the distance to next objects in front of the robot
43  drivenDistance1 = 0;
44  drivenDistance2 = 0;
45  drivenDistance3 = 0;
46  drivenDistance4 = 0;
47 
48  // Initialize the motor speed (not to fast, before reading the real values from a file!)
49  motor1Speed = 20;
50  motor2Speed = 20;
51  motor3Speed = 20;
52  motor4Speed = 20;
53  motorSpeedAllMotors = 20;
54 
55  robotState = ON; // Wer're thinking positive. The robot is ON untill whe know nothing other. :-)
56 }
57 
58 
59 Motor::~Motor()
60 {
61  /*
62  parkStepper(STEPPER1);
63  parkStepper(STEPPER2);
64  */
65 }
66 
67 
68 /*
69 unsigned int Motor::getSteps1()
70 {
71  return steps1;
72 }
73 
74 
75 unsigned int Motor::getSteps2()
76 {
77  return steps2;
78 }
79 */
80 
81 
82 unsigned int Motor::getRevolutions(unsigned char motor)
83 {
84  switch (motor)
85  {
86  case MOTOR1:
87  return revolutions1;
88  break;
89  case MOTOR2:
90  return revolutions2;
91  break;
92  case MOTOR3:
93  return revolutions3;
94  break;
95  case MOTOR4:
96  return revolutions4;
97  break;
98  }
99 
100  return 0;
101 }
102 
103 
104 double Motor::getDrivenDistance(unsigned char motor)
105 {
106  switch (motor)
107  {
108  case MOTOR1:
109  return drivenDistance1;
110  break;
111  case MOTOR2:
112  return drivenDistance2;
113  break;
114  case MOTOR3:
115  return drivenDistance3;
116  break;
117  case MOTOR4:
118  return drivenDistance4;
119  break;
120  }
121 
122  return 0;
123 }
124 
125 
126 bool Motor::motorControl(int motor, bool power, int direction)
127 {
128  QString command = "bst"; // stop robot
129  QString answer = "error";
130 
131 
132  if (robotState == ON)
133  {
134  // Lock the mutex. If another thread has locked the mutex then this call will block until that thread has unlocked it.
135  mutex->lock();
136 
137  switch (motor)
138  {
139  //----------------------------------------------------------------------------------
140  // ALLMOTORS is for sending only -one- command for all motors over the serial line!
141  //----------------------------------------------------------------------------------
142  case ALLMOTORS:
143  // convert the given direction into the corresponding serial command for the avr code
144  switch (direction)
145  {
146  case FORWARD:
147  // bot drive forward
148  command = "bdf";
149  break;
150  case BACKWARD:
151  // bot drive backward
152  command = "bdb";
153  break;
154  case LEFT:
155  // bot drive left
156  command = "bdl";
157  break;
158  case RIGHT:
159  // bot drive right
160  command = "bdr";
161  break;
162  case TURNLEFT:
163  // bot turn left
164  command = "btl";
165  break;
166  case TURNRIGHT:
167  // bot turn right
168  command = "btr";
169  break;
170  case START:
171  // bot "go"
172  command = "bgo";
173  break;
174  case STOP:
175  // bot stop
176  command = "bst";
177  break;
178  case WAIT:
179  // bot wait
180  command = "bwa";
181  break;
182  }
183 
184  // send command to bot
185  if (interface1->sendString(command, className) == true)
186  {
187  // check if the robot answers with the sent command
188  if (interface1->receiveString(answer, className) == true)
189  {
190  if (answer == QString("*%1#").arg(command))
191  {
192  // Unlock the mutex
193  mutex->unlock();
194  return true;
195  }
196  }
197  }
198  //qDebug("ERROR sending to serial port (MotorControl)");
199  // Unlocks the mutex
200  mutex->unlock();
201  return false;
202  break;
203 
204  //-------------------------
205  // Motor 1
206  //-------------------------
207  case MOTOR1:
208  if (power == ON)
209  {
210  // Motors with gearings don't have to be turned on!
211  //
212  // (This function is obsolete because of no longer using stepping motors)
213  }
214  else
215  {
216  // turn off the MOTOR -> break!
217  if (interface1->sendString("mp1of", className) == true)
218  {
219  // check if the robot answers with the sent command
220  if ( interface1->receiveString(answer, className) == true)
221  {
222  if (answer == "*mp1of#")
223  {
224  // Unlock the mutex
225  mutex->unlock();
226  return true;
227  }
228  }
229  }
230  //qDebug("ERROR sending to serial port (MotorControl)");
231  // Unlocks the mutex
232  mutex->unlock();
233  return false;
234  }
235 
236  if (direction == FORWARD)
237  {
238  // set the direction
239  if (interface1->sendString("md1cw", className) == true)
240  {
241  // check if the robot answers with the sent command
242  if ( interface1->receiveString(answer, className) == true)
243  {
244  if (answer == "*md1cw#")
245  {
246  // Unlock the mutex
247  mutex->unlock();
248  return true;
249  }
250  }
251  }
252  //qDebug("ERROR sending to serial port (MotorControl)");
253  // Unlocks the mutex
254  mutex->unlock();
255  return false;
256  }
257 
258  if (direction == BACKWARD)
259  {
260  // set the direction
261  if (interface1->sendString("md1cc", className) == true)
262  {
263  // check if the robot answers with the sent command
264  if ( interface1->receiveString(answer, className) == true)
265  {
266  if (answer == "*md1cc#")
267  {
268  // Unlock the mutex
269  mutex->unlock();
270  return true;
271  }
272  }
273  }
274  //qDebug("ERROR sending to serial port (MotorControl)");
275  // Unlocks the mutex
276  mutex->unlock();
277  return false;
278  }
279 
280  if (direction == SAME)
281  {
282  // don't change the direction (motor was only turned on or off)!
283  }
284 
285  // Unlocks the mutex
286  mutex->unlock();
287  break;
288 
289 
290  //-------------------------
291  // Motor 2
292  //-------------------------
293  case MOTOR2:
294  if (power == ON)
295  {
296  // Motors with gearings don't have to be turned on!
297  //
298  // (This function is obsolete because of no longer using stepping motors)
299  }
300  else
301  {
302  // turn off the MOTOR -> break!
303  if (interface1->sendString("mp2of", className) == true)
304  {
305  // check if the robot answers with the sent command
306  if ( interface1->receiveString(answer, className) == true)
307  {
308  if (answer == "*mp2of#")
309  {
310  // Unlock the mutex
311  mutex->unlock();
312  return true;
313  }
314  }
315  }
316  //qDebug("ERROR sending to serial port (MotorControl)");
317  // Unlocks the mutex
318  mutex->unlock();
319  return false;
320  }
321 
322  if (direction == FORWARD)
323  {
324  // set the direction
325  if (interface1->sendString("md2cw", className) == true)
326  {
327  // check if the robot answers with the sent command
328  if ( interface1->receiveString(answer, className) == true)
329  {
330  if (answer == "*md2cw#")
331  {
332  // Unlock the mutex
333  mutex->unlock();
334  return true;
335  }
336  }
337  }
338  //qDebug("ERROR sending to serial port (MotorControl)");
339  // Unlocks the mutex
340  mutex->unlock();
341  return false;
342  }
343 
344  if (direction == BACKWARD)
345  {
346  // set the direction
347  if (interface1->sendString("md2cc", className) == true)
348  {
349  // check if the robot answers with the sent command
350  if ( interface1->receiveString(answer, className) == true)
351  {
352  if (answer == "*md2cc#")
353  {
354  // Unlock the mutex
355  mutex->unlock();
356  return true;
357  }
358  }
359  }
360  //qDebug("ERROR sending to serial port (MotorControl)");
361  // Unlocks the mutex
362  mutex->unlock();
363  return false;
364  }
365 
366  if (direction == SAME)
367  {
368  // don't change the direction (motor was only turned on or off)!
369  }
370 
371  // Unlocks the mutex
372  mutex->unlock();
373  break;
374 
375 
376  //-------------------------
377  // Motor 3
378  //------------------------
379  case MOTOR3:
380  if (power == ON)
381  {
382  // Motors with gearings don't have to be turned on!
383  //
384  // (This function is obsolete because of no longer using stepping motors)
385  }
386  else
387  {
388  // turn off the MOTOR -> break!
389  if (interface1->sendString("mp3of", className) == true)
390  {
391  // check if the robot answers with the sent command
392  if ( interface1->receiveString(answer, className) == true)
393  {
394  if (answer == "*mp3of#")
395  {
396  // Unlock the mutex
397  mutex->unlock();
398  return true;
399  }
400  }
401  }
402  //qDebug("ERROR sending to serial port (MotorControl)");
403  // Unlocks the mutex
404  mutex->unlock();
405  return false;
406  }
407 
408  if (direction == FORWARD)
409  {
410  // set the direction
411  if (interface1->sendString("md3cw", className) == true)
412  {
413  // check if the robot answers with the sent command
414  if ( interface1->receiveString(answer, className) == true)
415  {
416  if (answer == "*md3cw#")
417  {
418  // Unlock the mutex
419  mutex->unlock();
420  return true;
421  }
422  }
423  }
424  //qDebug("ERROR sending to serial port (MotorControl)");
425  // Unlocks the mutex
426  mutex->unlock();
427  return false;
428  }
429 
430  if (direction == BACKWARD)
431  {
432  // set the direction
433  if (interface1->sendString("md3cc", className) == true)
434  {
435  // check if the robot answers with the sent command
436  if ( interface1->receiveString(answer, className) == true)
437  {
438  if (answer == "*md3cc#")
439  {
440  // Unlock the mutex
441  mutex->unlock();
442  return true;
443  }
444  }
445  }
446  //qDebug("ERROR sending to serial port (MotorControl)");
447  // Unlocks the mutex
448  mutex->unlock();
449  return false;
450  }
451 
452  if (direction == SAME)
453  {
454  // don't change the direction (motor was only turned on or off)!
455  }
456 
457  // Unlocks the mutex
458  mutex->unlock();
459  break;
460 
461 
462  //-------------------------
463  // Motor 4
464  //------------------------
465  case MOTOR4:
466  if (power == ON)
467  {
468  // Motors with gearings don't have to be turned on!
469  //
470  // (This function is obsolete because of no longer using stepping motors)
471  }
472  else
473  {
474  // turn off the MOTOR -> break!
475  if (interface1->sendString("mp4of", className) == true)
476  {
477  // check if the robot answers with the sent command
478  if ( interface1->receiveString(answer, className) == true)
479  {
480  if (answer == "*mp4of#")
481  {
482  // Unlock the mutex
483  mutex->unlock();
484  return true;
485  }
486  }
487  }
488  //qDebug("ERROR sending to serial port (MotorControl)");
489  // Unlocks the mutex
490  mutex->unlock();
491  return false;
492  }
493 
494  if (direction == FORWARD)
495  {
496  // set the direction
497  if (interface1->sendString("md4cw", className) == true)
498  {
499  // check if the robot answers with the sent command
500  if ( interface1->receiveString(answer, className) == true)
501  {
502  if (answer == "*md4cw#")
503  {
504  // Unlock the mutex
505  mutex->unlock();
506  return true;
507  }
508  }
509  }
510  //qDebug("ERROR sending to serial port (MotorControl)");
511  // Unlocks the mutex
512  mutex->unlock();
513  return false;
514  }
515 
516  if (direction == BACKWARD)
517  {
518  // set the direction
519  if (interface1->sendString("md4cc", className) == true)
520  {
521  // check if the robot answers with the sent command
522  if ( interface1->receiveString(answer, className) == true)
523  {
524  if (answer == "*md4cc#")
525  {
526  // Unlock the mutex
527  mutex->unlock();
528  return true;
529  }
530  }
531  }
532  //qDebug("ERROR sending to serial port (MotorControl)");
533  // Unlocks the mutex
534  mutex->unlock();
535  return false;
536  }
537 
538  if (direction == SAME)
539  {
540  // don't change the direction (motor was only turned on or off)!
541  }
542 
543  // Unlocks the mutex
544  mutex->unlock();
545  break;
546 
547 
548  /*
549  //-------------------------
550  // stepper 1
551  //------------------------
552  case STEPPER1:
553  if (power == ON)
554  {
555  // turn on stepper 1
556  if (interface1->sendString(STEPPER1_ON) == true) // FIXME: use sendString with a new nice string!
557  {
558  // check if the robot answers with the sent command
559  if ( interface1->receiveString(answer) == true)
560  {
561  if (answer == " fix this ")
562  {
563  // Unlock the mutex
564  mutex->unlock();
565  return true;
566  }
567  }
568  }
569  //qDebug("ERROR sending to serial port (MotorControl)");
570  // Unlocks the mutex
571  mutex->unlock();
572  return false;
573  }
574  else
575  {
576  // turn off stepper 1
577  if (interface1->sendString(STEPPER1_OFF) == true) // FIXME: use sendString with a new nice string!
578  {
579  // check if the robot answers with the sent command
580  if ( interface1->receiveString(answer) == true)
581  {
582  if (answer == " fix this ")
583  {
584  // Unlock the mutex
585  mutex->unlock();
586  return true;
587  }
588  }
589  }
590  //qDebug("ERROR sending to serial port (MotorControl)");
591  // Unlocks the mutex
592  mutex->unlock();
593  return false;
594  }
595 
596  if (direction == CLOCKWISE)
597  {
598  // set the direction
599  if (interface1->sendString(STEPPER1_CLOCKWISE) == true) // FIXME: use sendString with a new nice string!
600  {
601  // check if the robot answers with the sent command
602  if ( interface1->receiveString(answer) == true)
603  {
604  if (answer == " fix this ")
605  {
606  // Unlock the mutex
607  mutex->unlock();
608  return true;
609  }
610  }
611  }
612  //qDebug("ERROR sending to serial port (MotorControl)");
613  // Unlocks the mutex
614  mutex->unlock();
615  return false;
616  }
617 
618  if (direction == COUNTERCLOCKWISE)
619  {
620  // set the direction
621  if (interface1->sendString(STEPPER1_COUNTERCLOCKWISE) == true) // FIXME: use sendString with a new nice string!
622  {
623  // check if the robot answers with the sent command
624  if ( interface1->receiveString(answer) == true)
625  {
626  if (answer == " fix this ")
627  {
628  // Unlock the mutex
629  mutex->unlock();
630  return true;
631  }
632  }
633  }
634  //qDebug("ERROR sending to serial port (MotorControl)");
635  // Unlocks the mutex
636  mutex->unlock();
637  return false;
638  }
639 
640  if (direction == SAME)
641  {
642  // don't change the direction (motor was only turned on or off)!
643  }
644 
645  // Unlocks the mutex. Attempting to unlock a mutex in a different thread to the one that locked it results in an error.
646  mutex->unlock();
647 
648  break;
649 
650 
651  //-------------------------
652  // stepper 2
653  //------------------------
654  case STEPPER2:
655  if (power == ON)
656  {
657  // turn on stepper 2
658  if (interface1->sendString(STEPPER2_ON) == true) // FIXME: use sendString with a new nice string!
659  {
660  // check if the robot answers with the sent command
661  if ( interface1->receiveString(answer) == true)
662  {
663  if (answer == " fix this ")
664  {
665  // Unlock the mutex
666  mutex->unlock();
667  return true;
668  }
669  }
670  }
671  //qDebug("ERROR sending to serial port (MotorControl)");
672  // Unlocks the mutex
673  mutex->unlock();
674  return false;
675  }
676  else
677  {
678  // turn off stepper 2
679  if (interface1->sendString(STEPPER2_OFF) == true) // FIXME: use sendString with a new nice string!
680  {
681  // check if the robot answers with the sent command
682  if ( interface1->receiveString(answer) == true)
683  {
684  if (answer == " fix this ")
685  {
686  // Unlock the mutex
687  mutex->unlock();
688  return true;
689  }
690  }
691  }
692  //qDebug("ERROR sending to serial port (MotorControl)");
693  // Unlocks the mutex
694  mutex->unlock();
695  return false;
696  }
697 
698  if (power == CLOCK)
699  {
700  // make a stepper with every enabled motor
701  if (interface1->sendString(STEPPER_CLOCK) == true) // FIXME: use sendString with a new nice string!
702  {
703  // check if the robot answers with the sent command
704  if ( interface1->receiveString(answer) == true)
705  {
706  if (answer == " fix this ")
707  {
708  // Unlock the mutex
709  mutex->unlock();
710  return true;
711  }
712  }
713  }
714  //qDebug("ERROR sending to serial port (MotorControl)");
715  // Unlocks the mutex
716  mutex->unlock();
717  return false;
718  }
719 
720  if (direction == CLOCKWISE)
721  {
722  // set the direction
723  if (interface1->sendString(STEPPER2_CLOCKWISE) == true) // FIXME: use sendString with a new nice string!
724  {
725  // check if the robot answers with the sent command
726  if ( interface1->receiveString(answer) == true)
727  {
728  if (answer == " fix this ")
729  {
730  // Unlock the mutex
731  mutex->unlock();
732  return true;
733  }
734  }
735  }
736  //qDebug("ERROR sending to serial port (MotorControl)");
737  // Unlocks the mutex
738  mutex->unlock();
739  return false;
740  }
741 
742  if (direction == COUNTERCLOCKWISE)
743  {
744  // set the direction
745  if (interface1->sendString(STEPPER2_COUNTERCLOCKWISE) == true) // FIXME: use sendString with a new nice string!
746  {
747  // check if the robot answers with the sent command
748  if ( interface1->receiveString(answer) == true)
749  {
750  if (answer == " fix this ")
751  {
752  // Unlock the mutex
753  mutex->unlock();
754  return true;
755  }
756  }
757  }
758  //qDebug("ERROR sending to serial port (MotorControl)");
759  // Unlocks the mutex
760  mutex->unlock();
761  return false;
762  }
763 
764  if (direction == SAME)
765  {
766  // don't change the direction (motor was only turned on or off)!
767  }
768 
769  // Unlocks the mutex. Attempting to unlock a mutex in a different thread to the one that locked it results in an error.
770  mutex->unlock();
771 
772  break;
773  */
774  }
775 
776  // Unlocks the mutex, if no case statement fits in the switch command
777  mutex->unlock();
778  } // robot is ON
779 
780 
781  return false;
782 }
783 
784 
785 /*
786 void Motor::makeSteps(int steps)
787 {
788  if (robotState == ON)
789  {
790  for (int i=steps; i>=steps; i--)
791  {
792  qDebug("step %d", i);
793 
794  // Lock the mutex. If another thread has locked the mutex then this call will block until that thread has unlocked it.
795  mutex->lock();
796 
797  if (interface1->sendChar(STEPPER_CLOCK) == false) // FIXME: use sendString with a new nice string!
798  {
799  //qDebug("ERROR sending to serial port (Motor)");
800  }
801 
802  // Unlocks the mutex. Attempting to unlock a mutex in a different thread to the one that locked it results in an error.
803  mutex->unlock();
804 
805  }
806  }
807 }
808 
809 
810 void Motor::parkStepper(unsigned char motor)
811 {
812  if (robotState == ON)
813  {
815  motorControl(STEPPER1, OFF, SAME);
816  motorControl(STEPPER2, OFF, SAME);
817  }
818 }
819 */
820 
821 void Motor::calculateMovement()
822 {
823 /* test
824 
826 
827  //--------------------------------------------------------------------------
828  // if motor 1 is ON, step counter + 1
829  //--------------------------------------------------------------------------
830 
831  // string for converting number to string
832  QString str;
833 
834  if ((usbPort->port0 & MOTOR1POWER) == MOTOR1POWER)
835  {
836  // Steps + 1
837  str = QString("%L1").arg(++steps1);
838  // set the label text
839  ui.labelSteps1->setText(str);
840 
841 
842  // Measure driven distance and display it
843  //
844  // One motor-step = 1,8 -> 360 = 200 steps = one revolution
845  // Diameter wheel = 66 mm
846  // Wheel circumference = PI*d = PI*66 mm = 207,345 mm
847  // Distance per step: 207,345 mm / 200 steps = 1,036725 mm = 0,1036725 cm
848  //
850  drivenDistance1 += 0.1036725;
851 
852  //
853  // cut all digits after the comma / point
854  // for nice displaying the string in a label.
855  //
856  str = QString("%L1").arg(drivenDistance1);
857  // search for the comma in the string and
858  // truncate the string at the position of the comma + 2
859  str.truncate(str.indexOf(commaSeparator) + 3);
860  // append the measure unit
861  str.append(" cm");
862  // set the label text
863  ui.labelDistance1->setText(str);
864 
865 
866  //
867  // Check if already enough steps are made for one revolution
868  // (divide the made steps through 200)
869  //
870  div_t value = div(steps1, 200);
871 
872  // if remainder = 0 then 200 steps are made (one revolution)
873  if (value.rem == 0)
874  {
875  // Revolutions + 1
876  str = QString("%L1").arg(++revolutions1);
877  ui.labelRevolutions1->setText(str);
878  }
879  } // Motor 1 on
880 
881 
882  //--------------------------------------------------------------------------
883  // if motor 2 is ON, step counter + 1
884  //--------------------------------------------------------------------------
885  if ((usbPort->port0 & MOTOR2POWER) == MOTOR2POWER)
886  {
887  // Steps + 1
888  str = QString("%L1").arg(++steps2);
889  // set the label text
890  ui.labelSteps2->setText(str);
891 
892 
893  // Measure driven distance and display it
894  //
895  // One motor-step = 1,8 -> 360 = 200 steps = one revolution
896  // Diameter wheel = 66 mm
897  // Wheel circumference = PI*d = PI*66 mm = 207,345 mm
898  // Distance per step: 207,345 mm / 200 steps = 1,036725 mm = 0,1036725 cm
899  //
901  drivenDistance2 += 0.1036725;
902 
903  //
904  // cut all digits after the comma / point
905  // for nice displaying the string in a label.
906  //
907  str = QString("%L1").arg(drivenDistance2);
908  // search for the comma in the string and
909  // truncate the string at the position of the comma + 2
910  str.truncate(str.indexOf(commaSeparator) + 3);
911  // append the measure unit
912  str.append(" cm");
913  // set the label text
914  ui.labelDistance2->setText(str);
915 
916 
917  //
918  // Check if already enough steps are made for one revolution
919  // (divide the made steps through 200)
920  //
921  div_t value = div(steps2, 200);
922 
923  // if remainder = 0 then 200 steps are made (one revolution)
924  if (value.rem == 0)
925  {
926  // Revolutions + 1
927  str = QString("%1").arg(++revolutions2);
928  ui.labelRevolutions2->setText(str);
929 
930  }
931  } // Motor 2 on
932 test */
933 }
934 
935 
936 void Motor::resetMovementCounter(short int motor)
937 {
938  switch (motor)
939  {
940  case MOTOR1:
941  {
942  // Initialize the motor step counters
943  //steps1 = 0;
944 
945  // Initialize the motor revolutions counters
946  revolutions1 = 0;
947 
948  // Initialize the distance to next objects in front of the robot
949  drivenDistance1 = 0;
950  return;
951  break;
952  }
953  case MOTOR2:
954  {
955  // Initialize the motor step counters
956  //steps2 = 0;
957 
958  // Initialize the motor revolutions counters
959  revolutions2 = 0;
960 
961  // Initialize the distance to next objects in front of the robot
962  drivenDistance2 = 0;
963  return;
964  break;
965  }
966  case MOTOR3:
967  {
968  // Initialize the motor step counters
969  //steps3 = 0;
970 
971  // Initialize the motor revolutions counters
972  revolutions3 = 0;
973 
974  // Initialize the distance to next objects in front of the robot
975  drivenDistance3 = 0;
976  return;
977  break;
978  }
979  case MOTOR4:
980  {
981  // Initialize the motor step counters
982  //steps4 = 0;
983 
984  // Initialize the motor revolutions counters
985  revolutions4 = 0;
986 
987  // Initialize the distance to next objects in front of the robot
988  drivenDistance4 = 0;
989  return;
990  break;
991  }
992  }
993 }
994 
995 
996 bool Motor::setMotorSpeed(int motor, int speed)
997 {
998  QString answer = "error";
999 
1000 
1001  if (robotState == ON)
1002  {
1003  if (speed < 0)
1004  {
1005  speed = 0;
1006  }
1007 
1008  if (speed > 255)
1009  {
1010  speed = 255;
1011  }
1012 
1013 
1014  // Lock the mutex. If another thread has locked the mutex then this call will block until that thread has unlocked it.
1015  mutex->lock();
1016  switch (motor)
1017  {
1018  case ALLMOTORS: // "motor 0"
1019  // store the speed
1020  motorSpeedAllMotors = speed;
1021  motor1Speed = speed;
1022  motor2Speed = speed;
1023  motor3Speed = speed;
1024  motor4Speed = speed;
1025  // send command to microcontroller
1026  if (interface1->sendString(QString("*mv0%1#").arg(speed), className) == true)
1027  {
1028  // check if the robot answers with "ok"
1029  if ( interface1->receiveString(answer, className) == true)
1030  {
1031  if (answer == "*mv0#")
1032  {
1033  // Unlock the mutex
1034  mutex->unlock();
1035  return true;
1036  }
1037  }
1038  }
1039  //qDebug("ERROR sending to serial port (MotorControl)");
1040  // Unlocks the mutex
1041  mutex->unlock();
1042  return false;
1043  break;
1044 
1045  case MOTOR1:
1046  // store the speed
1047  motor1Speed = speed;
1048  // send command to microcontroller
1049  if (interface1->sendString(QString("*mv1%1#").arg(speed), className) == true)
1050  {
1051  // check if the robot answers with "ok"
1052  if ( interface1->receiveString(answer, className) == true)
1053  {
1054  if (answer == "*mv1#")
1055  {
1056  // Unlock the mutex
1057  mutex->unlock();
1058  return true;
1059  }
1060  }
1061  }
1062  //qDebug("ERROR sending to serial port (MotorControl)");
1063  // Unlocks the mutex
1064  mutex->unlock();
1065  return false;
1066  break;
1067 
1068  case MOTOR2:
1069  // store the speed
1070  motor2Speed = speed;
1071  // send command to microcontroller
1072  if (interface1->sendString(QString("*mv2%1#").arg(speed), className) == true)
1073  {
1074  // check if the robot answers with "ok"
1075  if ( interface1->receiveString(answer, className) == true)
1076  {
1077  if (answer == "*mv2#")
1078  {
1079  // Unlock the mutex
1080  mutex->unlock();
1081  return true;
1082  }
1083  }
1084  }
1085  //qDebug("ERROR sending to serial port (MotorControl)");
1086  // Unlocks the mutex
1087  mutex->unlock();
1088  return false;
1089  break;
1090 
1091  case MOTOR3:
1092  // store the speed
1093  motor3Speed = speed;
1094  // send command to microcontroller
1095  if (interface1->sendString(QString("*mv3%1#").arg(speed), className) == true)
1096  {
1097  // check if the robot answers with "ok"
1098  if ( interface1->receiveString(answer, className) == true)
1099  {
1100  if (answer == "*mv3#")
1101  {
1102  // Unlock the mutex
1103  mutex->unlock();
1104  return true;
1105  }
1106  }
1107  }
1108  //qDebug("ERROR sending to serial port (MotorControl)");
1109  // Unlocks the mutex
1110  mutex->unlock();
1111  return false;
1112  break;
1113 
1114  case MOTOR4:
1115  // store the speed
1116  motor4Speed = speed;
1117  // send command to microcontroller
1118  if (interface1->sendString(QString("*mv4%1#").arg(speed), className) == true)
1119  {
1120  // check if the robot answers with "ok"
1121  if ( interface1->receiveString(answer, className) == true)
1122  {
1123  if (answer == "*mv4#")
1124  {
1125  // Unlock the mutex
1126  mutex->unlock();
1127  return true;
1128  }
1129  }
1130  }
1131  //qDebug("ERROR sending to serial port (MotorControl)");
1132  // Unlocks the mutex
1133  mutex->unlock();
1134  return false;
1135  break;
1136  } // switch
1137 
1138  // Unlock the mutex.
1139  mutex->unlock();
1140  } // robot is ON
1141 
1142 
1143  // robot is off
1144  return false;
1145 }
1146 
1147 
1148 void Motor::setMaximumSpeed(int speed)
1149 {
1150  // FIXME: set the maximum speed for the robot / in this class!
1151  Q_UNUSED(speed);
1152 }
1153 
1154 
1155 int Motor::getMotorSpeed(int motor)
1156 {
1157  switch (motor)
1158  {
1159  case MOTOR1:
1160  return motor1Speed;
1161  break;
1162  case MOTOR2:
1163  return motor2Speed;
1164  break;
1165  case MOTOR3:
1166  return motor3Speed;
1167  break;
1168  case MOTOR4:
1169  return motor4Speed;
1170  break;
1171  }
1172 
1173  return -1;
1174 }
1175 
1176 
1177 bool Motor::flashlight(bool state)
1178 {
1179  QString answer = "error";
1180 
1181 
1182  if (robotState == ON)
1183  {
1184  // Lock the mutex. If another thread has locked the mutex then this call will block until that thread has unlocked it.
1185  mutex->lock();
1186 
1187  if (state == ON)
1188  {
1189  // send command to microcontroller
1190  if (interface1->sendString("*f0on#", className) == true)
1191  {
1192  // check if the robot answers with "ok"
1193  if ( interface1->receiveString(answer, className) == true)
1194  {
1195  if (answer == "*f0on")
1196  {
1197  // Unlock the mutex
1198  mutex->unlock();
1199  return true;
1200  }
1201  }
1202  }
1203  //qDebug("ERROR sending to serial port (flashlight)");
1204  // Unlocks the mutex
1205  mutex->unlock();
1206  return false;
1207  }
1208  else
1209  {
1210  // send command to microcontroller
1211  if (interface1->sendString("*f0of#", className) == true)
1212  {
1213  // check if the robot answers with "ok"
1214  if ( interface1->receiveString(answer, className) == true)
1215  {
1216  if (answer == "*f0of")
1217  {
1218  // Unlock the mutex
1219  mutex->unlock();
1220  return true;
1221  }
1222  }
1223  }
1224  //qDebug("ERROR sending to serial port (flashlight)");
1225  // Unlocks the mutex
1226  mutex->unlock();
1227  return false;
1228  }
1229 
1230  // Unlocks the mutex. Attempting to unlock a mutex in a different thread to the one that locked it results in an error.
1231  mutex->unlock();
1232  } // robot is ON
1233 
1234 
1235  return false;
1236 }
1237 
1238 
1239 void Motor::setRobotState(bool state)
1240 {
1241  // store the state within this class
1242  robotState = state;
1243 
1244  emit message(QString("Robot state set to %1 in %2").arg(state).arg(className));
1245 }