Subject:
|
enhancing the motor driver
|
Newsgroups:
|
lugnet.robotics.rcx.legos
|
Date:
|
Wed, 24 Feb 1999 17:01:50 GMT
|
Viewed:
|
1356 times
|
| |
|
|
Hi all,
I couldn't resist the urge to do something productive after taking the
exam today. Dan sent me some code offering 256 levels of power, and I
hacked away at it.
I played around with the assembler, and here's my new version. It
adheres to the old interface (seperate specification of direction and
speed), but MAX_SPEED is up to 255 now. Any well-behaved program using
the predefined constants should continue to work. (simple-rover.c needs
a small patch ;-)
As a side note, the new interrupt handler is 12 bytes shorter than the
old one. More flexibility, less footprint. Its average execution time
should be a little higher, though.
Could somebody verify quickly if this works? I'm not at home and out of
batteries...
Markus.
> > > > > > > > > > > > > > > > > > > > > > direct_motor.c >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//
// legOS - the independent LEGO Mindstorms OS
/*!\file direct-motor.c
\brief direct motor access
\author Copyright (c) 1998-1999 by Markus L. Noga <noga@inrialpes.fr>
*/
#include "direct-motor.h"
#include "sys/h8.h"
#include "sys/irq.h"
#ifndef NO_DIRECT_MOTOR
///////////////////////////////////////////////////////////////////////////////
//
// Variables
//
///////////////////////////////////////////////////////////////////////////////
//! motor drive patterns
/*! to be indexed with MotorDirections
\sa MotorDirections
*/
const unsigned char dm_a_pattern[]={0x00,0x80,0x40,0xc0},
dm_b_pattern[]={0x00,0x08,0x04,0x0c},
dm_c_pattern[]={0x00,0x02,0x01,0x03};
MotorState dm_a, //!< motor A state
dm_b, //!< motor B state
dm_c; //!< motor C state
///////////////////////////////////////////////////////////////////////////////
//
// Functions
//
///////////////////////////////////////////////////////////////////////////////
//! direct motor output handler
/*! called by system timer in the 16bit timer OCIA irq
*/
extern void dm_handler(void);
__asm__("
.text
.align 1
.global _dm_handler
_dm_handler:
; r6 saved by ROM
; r0 saved by systime_handler
sub.w r6,r6 ; r6l is output
; we simultaneously load delta (r0h) and sum (r0l)
; this depends on byte order, but the H8 will stay MSB
; and the resulting code is efficient and compact.
; motor A
mov.w @_dm_a,r0
add.b r0h,r0l ; add delta to sum
bvc 0f ; sum overflow?
mov.b @_dm_a+2,r6h ; -> output drive pattern
or.b r6h,r6l
0:mov.b r0l,@_dm_a+1 ; save sum
; (clears overflow flag)
; motor B
mov.w @_dm_b,r0
add.b r0h,r0l ; add delta to sum
bvc 1f ; sum overflow?
mov.b @_dm_b+2,r6h ; -> output drive pattern
or.b r6h,r6l
1:mov.b r0l,@_dm_b+1 ; save sum
; (clears overflow flag)
; motor C
mov.w @_dm_c,r0
add.b r0h,r0l ; add delta to sum
bvc 2f ; sum overflow?
mov.b @_dm_c+2,r6h ; -> output drive pattern
or.b r6h,r6l
2:mov.b r0l,@_dm_c+1 ; save sum
; (clears overflow flag)
; driver chip
mov.b r6l,@0xf000:16 ; output motor waveform
rts
");
//! initialize motors
//
void dm_init(void) {
dm_shutdown(); // shutdown hardware
}
//! shutdown motors
//
void dm_shutdown(void) {
RCX_MOTORS=0x00; // shutdown hardware
motor_a_dir(off); // initialize driver data
motor_b_dir(off);
motor_c_dir(off);
motor_a_speed(MIN_SPEED);
motor_b_speed(MIN_SPEED);
motor_c_speed(MIN_SPEED);
}
#endif
> > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > > direct_motor.h >>>>>>>>>>>>>>>>>>>>>>>>>>>
//
// legOS - the independent LEGO Mindstorms OS
/*!\file direct-motor.h
\brief direct motor access
\author Copyright (c) 1998-1999 by Markus L. Noga <noga@inrialpes.fr>
\par Motor periods
the motor handler operates with 1ms period.
a motor period of n indicates one step in n periods
therefore, period 1 is the shortest possible period
it will deliver the fastest possible movement.
*/
#ifndef __direct_motor_h__
#define __direct_motor_h__
#include "config.h"
#ifndef NO_DIRECT_MOTOR
///////////////////////////////////////////////////////////////////////////////
//
// Definitions
//
///////////////////////////////////////////////////////////////////////////////
// the RCX-specific motor driver I/O address
#define RCX_MOTORS *((unsigned char*) 0xf000)
//! the motor directions
typedef enum {
off=0, //!< freewheel
fwd=1, //!< forward
rev=2, //!< reverse
brake=3 //!< hold current position
}
MotorDirection;
//! the motor status type.
typedef struct {
union {
unsigned assembler; //!< assures word alignment for
assembler
struct {
unsigned char delta; //!< the speed setting
volatile unsigned char sum; //!< running sum
} c;
} access; //!< provides access from C and
assembler
unsigned char dir; //!< output pattern when sum overflows
}
MotorState;
//! minimum motor speed
#define MIN_SPEED 0
//! maximum motor speed
#define MAX_SPEED 255
///////////////////////////////////////////////////////////////////////////////
//
// Variables
//
///////////////////////////////////////////////////////////////////////////////
//! motor drive patterns
/*! to be indexed with MotorDirections
\sa MotorDirections
*/
extern const unsigned char dm_a_pattern[4],
dm_b_pattern[4],
dm_c_pattern[4];
extern MotorState dm_a, //!< motor A state
dm_b, //!< motor B state
dm_c; //!< motor C state
///////////////////////////////////////////////////////////////////////////////
//
// Functions
//
///////////////////////////////////////////////////////////////////////////////
//! initialize motors
//
void dm_init(void);
//! shutdown motors
//
void dm_shutdown(void);
//! set motor A direction
/*! \param dir the direction
*/
extern const inline void motor_a_dir(MotorDirection dir) {
dm_a.dir=dm_a_pattern[dir];
}
//! set motor B direction
/*! \param dir the direction
*/
extern const inline void motor_b_dir(MotorDirection dir) {
dm_b.dir=dm_b_pattern[dir];
}
//! set motor C direction
/*! \param dir the direction
*/
extern const inline void motor_c_dir(MotorDirection dir) {
dm_c.dir=dm_c_pattern[dir];
}
//! set motor A speed
/*! \param speed the speed
*/
extern const inline void motor_a_speed(unsigned char speed) {
dm_a.access.c.delta=speed;
}
//! set motor B speed
/*! \param speed the speed
*/
extern const inline void motor_b_speed(unsigned char speed) {
dm_b.access.c.delta=speed;
}
//! set motor C speed
/*! \param speed the speed
*/
extern const inline void motor_c_speed(unsigned char speed) {
dm_c.access.c.delta=speed;
}
#endif // NO_DIRECT_MOTOR
#endif // __direct_motor_h__
> > > > > > > > > > > > > > > > > > > > > > > > > > > > > > end of story >>>>>>>>>>>>>>>>>>>>>>>>>>>><
--
Markus L. Noga noga@inrialpes.fr
Check out legOS! http://www.multimania.com/legos/
"Quand on n'a pas de caractere, il faut bien se donner une methode."
-Camus on Software Engineering
|
|
1 Message in This Thread:
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
