Main Page   Class Hierarchy   Alphabetical List   Compound List   File List   Compound Members   File Members   Related Pages  

kernel/dmotor.c

Go to the documentation of this file.
00001 
00006 /*
00007  *  The contents of this file are subject to the Mozilla Public License
00008  *  Version 1.0 (the "License"); you may not use this file except in
00009  *  compliance with the License. You may obtain a copy of the License at
00010  *  http://www.mozilla.org/MPL/
00011  *
00012  *  Software distributed under the License is distributed on an "AS IS"
00013  *  basis, WITHOUT WARRANTY OF ANY KIND, either express or implied. See the
00014  *  License for the specific language governing rights and limitations
00015  *  under the License.
00016  *
00017  *  The Original Code is legOS code, released October 17, 1999.
00018  *
00019  *  The Initial Developer of the Original Code is Markus L. Noga.
00020  *  Portions created by Markus L. Noga are Copyright (C) 1999
00021  *  Markus L. Noga. All Rights Reserved.
00022  *
00023  *  Contributor(s): Markus L. Noga <markus@noga.de>
00024  *                  Lou Sortman <lou (at) sunsite (dot) unc (dot) edu>
00025  */
00026 
00027 #include <sys/dmotor.h>
00028 #include <dlcd.h>
00029 
00030 #ifdef CONF_DMOTOR
00031 
00032 #include <sys/h8.h>
00033 #include <sys/irq.h>
00034 
00036 //
00037 // Variables
00038 //
00040 
00042 /*! to be indexed with MotorDirections
00043     \sa MotorDirections
00044 */
00045 #ifdef CONF_DMOTOR_HOLD
00046 const unsigned char dm_a_pattern[]={0xc0,0x40,0x80,0x00},
00047                     dm_b_pattern[]={0x0c,0x04,0x08,0x00},
00048                     dm_c_pattern[]={0x03,0x01,0x02,0x00};
00049 #else
00050 const unsigned char dm_a_pattern[]={0x00,0x80,0x40,0xc0},
00051                     dm_b_pattern[]={0x00,0x08,0x04,0x0c},
00052                     dm_c_pattern[]={0x00,0x02,0x01,0x03};
00053 #endif
00054 
00055 MotorState dm_a,                  
00056            dm_b,                  
00057            dm_c;                  
00058 
00059         
00061 //
00062 // Functions
00063 //
00065 
00067 /*! called by system timer in the 16bit timer OCIA irq
00068 */
00069 extern void dm_handler(void);
00070 #ifndef DOXYGEN_SHOULD_SKIP_THIS
00071 __asm__("
00072 .text
00073 .align 1
00074 .global _dm_handler
00075 _dm_handler:
00076                 ; r6 saved by ROM
00077                 ; r0 saved by systime_handler
00078 "
00079 #ifdef CONF_DMOTOR_HOLD
00080 "               mov.b #0xcf,r6l                 ; r6l is output\n"
00081 #else
00082 "               sub.w r6,r6                     ; r6l is output\n"
00083 #endif
00084 "               ; we simultaneously load delta (r0h) and sum (r0l)
00085                 ; this depends on byte order, but the H8 will stay MSB
00086                 ; and the resulting code is efficient and compact.
00087                 
00088                 ; motor A
00089                 
00090                 mov.w   @_dm_a,r0
00091                 add.b   r0h,r0l                 ; add delta to sum
00092                 bcc     dm0                     ; sum overflow?
00093                   mov.b @_dm_a+2,r6h            ; -> output drive pattern
00094                   xor.b r6h,r6l
00095             dm0:mov.b   r0l,@_dm_a+1            ; save sum
00096                                                 ; (clears overflow flag)
00097 
00098                 ; motor B
00099                 
00100                 mov.w   @_dm_b,r0
00101                 add.b   r0h,r0l                 ; add delta to sum
00102                 bcc     dm1                     ; sum overflow?
00103                   mov.b @_dm_b+2,r6h            ; -> output drive pattern
00104                   xor.b r6h,r6l
00105             dm1:mov.b   r0l,@_dm_b+1            ; save sum
00106                                                 ; (clears overflow flag)
00107 
00108                 ; motor C
00109                 
00110                 mov.w   @_dm_c,r0
00111                 add.b   r0h,r0l                 ; add delta to sum
00112                 bcc     dm2                     ; sum overflow?
00113                   mov.b @_dm_c+2,r6h            ; -> output drive pattern
00114                   xor.b r6h,r6l
00115             dm2:mov.b   r0l,@_dm_c+1            ; save sum
00116                                                 ; (clears overflow flag)
00117 
00118                 ; driver chip
00119                   
00120                 mov.b   r6l,@0xf000:16          ; output motor waveform
00121                 
00122                 rts             
00123         ");
00124 #endif // DOXYGEN_SHOULD_SKIP_THIS
00125         
00126                 
00128 //
00129 void dm_init(void) {
00130   dm_shutdown();                                // shutdown hardware
00131 }
00132 
00133 
00135 //
00136 void dm_shutdown(void) {
00137   motor_controller=0x00;                // shutdown hardware
00138 
00139   motor_a_dir(off);                     // initialize driver data
00140   motor_b_dir(off);
00141   motor_c_dir(off);
00142 
00143   motor_a_speed(MAX_SPEED);
00144   motor_b_speed(MAX_SPEED);
00145   motor_c_speed(MAX_SPEED);
00146 }
00147 
00148 #ifdef CONF_VIS
00149 /*
00150 ** Define non-inline versions to display arrows
00151 */
00152 
00153 void motor_a_dir(MotorDirection dir)
00154 {
00155   dm_a.dir = dm_a_pattern[dir];
00156   dlcd_hide(LCD_A_LEFT);
00157   dlcd_hide(LCD_A_RIGHT);
00158   if (dir == fwd || dir == brake)
00159     dlcd_show(LCD_A_RIGHT);
00160   else if (dir == rev || dir == brake)
00161     dlcd_show(LCD_A_LEFT);
00162 }
00163 
00164 void motor_b_dir(MotorDirection dir)
00165 {
00166   dm_b.dir = dm_b_pattern[dir];
00167   dlcd_hide(LCD_B_LEFT);
00168   dlcd_hide(LCD_B_RIGHT);
00169   if (dir == fwd || dir == brake)
00170     dlcd_show(LCD_B_RIGHT);
00171   else if (dir == rev || dir == brake)
00172     dlcd_show(LCD_B_LEFT);
00173 }
00174 
00175 void motor_c_dir(MotorDirection dir)
00176 {
00177   dm_c.dir = dm_c_pattern[dir];
00178   dlcd_hide(LCD_C_LEFT);
00179   dlcd_hide(LCD_C_RIGHT);
00180   if (dir == fwd || dir == brake)
00181     dlcd_show(LCD_C_RIGHT);
00182   else if (dir == rev || dir == brake)
00183     dlcd_show(LCD_C_LEFT);
00184 }
00185 
00186 #endif // ifdef CONF_VIS
00187 
00188 #endif // CONF_DMOTOR

brickOS is released under the Mozilla Public License.
Original code copyright 1998-2002 by the authors.

Generated on Tue Dec 10 00:09:12 2002 for brickOS Kernel Developer by doxygen 1.2.15