[tuxdroid-svn] r469 - firmware/tuxcore/trunk

[Paul_R <c2m...@c2...>]

Author: Paul_R
Date: 2007-08-28 14:59:08 +0200 (Tue, 28 Aug 2007)
New Revision: 469

Modified:
   firmware/tuxcore/trunk/global.h
   firmware/tuxcore/trunk/main.c
   firmware/tuxcore/trunk/motors.c
   firmware/tuxcore/trunk/motors.h
Log:
Fix bug #4 : spin movements
- Add test to determine if the rotation is reversed, to prevent wrong rotation
  angle.
- Add routine to brake the motor when final position is reached.


Modified: firmware/tuxcore/trunk/global.h
===================================================================
--- firmware/tuxcore/trunk/global.h	2007-08-23 12:41:48 UTC (rev 468)
+++ firmware/tuxcore/trunk/global.h	2007-08-28 12:59:08 UTC (rev 469)
@@ -169,6 +169,7 @@
 #define MOUTH_OPEN_DLY  2       /* 4ms unit */
 #define MOUTH_ICLOSE_DLY 8      /* 4ms unit */
 #define SPIN_INTT_DLY   10      /* 4ms unit */
+#define SPIN_STOP_DLY 10	/* 4ms unit */
 #define WINGS_STOP_DLY  4       /* 4ms unit */
 
 /*

Modified: firmware/tuxcore/trunk/main.c
===================================================================
--- firmware/tuxcore/trunk/main.c	2007-08-23 12:41:48 UTC (rev 468)
+++ firmware/tuxcore/trunk/main.c	2007-08-28 12:59:08 UTC (rev 469)
@@ -160,6 +160,17 @@
         }
     }
 
+    /* spin braking (motor reversed) delay */
+    if (spinStpCnt)
+    {
+        spinStpCnt--;
+	if (!spinStpCnt)
+	{
+            spinPwmMask &= ~MOT_SPIN_MK;
+            stopSpinMot();
+        }
+    }
+
     /* Every 100ms, update status and send it to the computer */
     if (t4ms_tim == 25)
     {

Modified: firmware/tuxcore/trunk/motors.c
===================================================================
--- firmware/tuxcore/trunk/motors.c	2007-08-23 12:41:48 UTC (rev 468)
+++ firmware/tuxcore/trunk/motors.c	2007-08-28 12:59:08 UTC (rev 469)
@@ -30,7 +30,7 @@
 uint8_t wings_tim, wings_prev_tim;      /* timer for absolute position of the wings */
 uint8_t eyesStpCnt, eyesPosCnt; //, eyesPwm;
 uint8_t mouthStpCnt, mouthPosCnt, mouth_pos;    //, mouthPwm;
-uint8_t spinPosCnt, spinPwm = 5;
+uint8_t spinPosCnt, spinStpCnt, spinPwm = 5, spinDirection;
 
 /* pwm mask registers */
 uint8_t pwmMaskB;
@@ -176,7 +176,7 @@
    This interrupt stops the spinning motor when the desired number of movements
    have been executed.
 
-   The switch is pushed each XXX degrees. We set the interrupt in falling edge
+   The switch is pushed each 90 degrees. We set the interrupt in falling edge
    mode so there's no interrupt generated when the switch is released.
  */
 ISR(SIG_INTERRUPT1)
@@ -184,12 +184,16 @@
     if (spinPosCnt)
     {
         spinPosCnt--;
+        
         if (!spinPosCnt)
         {
-            {
-                spinPwmMask &= ~MOT_SPIN_MK;
-                stopSpinMot();
-            }
+            spinPwmMask &= ~MOT_SPIN_MK;
+            stopSpinMot();
+            spinStpCnt = SPIN_STOP_DLY;
+            if (spinDirection == LEFT)
+                spinPwmMask |= MOT_SPIN_R_MK;
+            else
+                spinPwmMask |= MOT_SPIN_L_MK;   
         }
     }
 }
@@ -286,6 +290,7 @@
 /**
    \brief Blink the eyes
    \ingroup eyes
+
    \param cnt number of movements before the wings will stop
    \note Any mouth command is cancelled
 
@@ -371,18 +376,33 @@
 
 void spinLeft(uint8_t angle, uint8_t pwm)
 {
+    /* Check if direction is changing and if pos. switch isn't push. Then,
+     * increment angle value to prevent too short rotation */
+    if (spinDirection == RIGHT && (PSW_SPIN_PIN & PSW_SPIN_MK))
+    {
+        angle += 1;
+    }
+    spinDirection = LEFT;
+    spinPwmMask &= ~MOT_SPIN_MK;
     spinPosCnt = angle;
     spinPwm = pwm;
-    spinPwmMask &= ~MOT_SPIN_R_MK;
+    spinPwmMask &= ~MOT_SPIN_R_MK; /* XXX This line can be deleted ? */
     spinPwmMask |= MOT_SPIN_L_MK;
     // runSpinLeft();
 }
 
 void spinRight(uint8_t angle, uint8_t pwm)
 {
+    /* Check if direction is changing and if pos. switch isn't push. Then,
+     * increment angle value to prevent too short rotation */
+    if (spinDirection == LEFT && (PSW_SPIN_PIN & PSW_SPIN_MK))
+    {
+        angle += 1;
+    }
+    spinDirection = RIGHT;
     spinPosCnt = angle;
     spinPwm = pwm;
-    spinPwmMask &= ~MOT_SPIN_L_MK;
+    spinPwmMask &= ~MOT_SPIN_L_MK; /* XXX */
     spinPwmMask |= MOT_SPIN_R_MK;
     // runSpinRight();
 }

Modified: firmware/tuxcore/trunk/motors.h
===================================================================
--- firmware/tuxcore/trunk/motors.h	2007-08-23 12:41:48 UTC (rev 468)
+++ firmware/tuxcore/trunk/motors.h	2007-08-28 12:59:08 UTC (rev 469)
@@ -26,13 +26,15 @@
 #define WINGS_TIM_DIFF          0x10
 enum mouth_pos_s
 { OPEN, CLOSE, UNDEFINED };
+enum spin_direction
+{ LEFT, RIGHT };
 
 /* counters to stop eye/mouth and wings at timer int */
 extern uint8_t wingsStpCnt, wingsPosCnt, wingsPwm;
 extern uint8_t wings_tim, wings_prev_tim;
 extern uint8_t eyesStpCnt, eyesPosCnt;
 extern uint8_t mouthStpCnt, mouthPosCnt, mouth_pos;
-extern uint8_t spinPosCnt, spinPwm;
+extern uint8_t spinPosCnt, spinStpCnt, spinPwm;
 
 /* counter to disable spin interrupts at PC int */
 extern volatile uint8_t intt_spin_cnt;