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 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 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. |