Subject:
|
Re: Rotation and Touch Sensors on one input
|
Newsgroups:
|
lugnet.robotics
|
Date:
|
Thu, 28 Jan 1999 16:28:16 GMT
|
Viewed:
|
991 times
|
| |
|
|
lego-robotics@crynwr.com (Markus L. Noga) writes:
> Hi Eric,
>
> Eric Haberfellner wrote:
> > I was playing around with legOS 0.1.5 last night (thanks for legOS Markus) • and
> > was able to get the rotation sensor code working fairly well. While doing • this
>
> You're welcome. Could you be more specific than "fairly well"? I've
> heard different stories. Some people reported rotation works perfectly
> in one direction and not at all in the other. Others experienced other
> results. Did you modify the state machine code? If yes, several people
> might want to try your patches.
>
> Ciao,
>
> Markus.
>
> --
Markus and others, here is my modified version of direct-sensor.c
Sorry that this is so long, but I do not have a web site to post this on.
--
//
// legOS - the independent LEGO Mindstorms OS
// direct-sensor.c - direct sensor access
// (c) 1998 by Markus L. Noga <noga@inrialpes.fr>
//
#include "direct-sensor.h"
#include "sys/h8.h"
#include "sys/irq.h"
#include "sys/bitops.h"
#include "rom/registers.h"
#include "unistd.h"
#include "conio.h"
#ifndef NO_DIRECT_SENSOR
///////////////////////////////////////////////////////////////////////////////
//
// Definitions
//
///////////////////////////////////////////////////////////////////////////////
#define DS_ALL_ACTIVE 0x07 // all sensors active mode
#define DS_ALL_PASSIVE (~DS_ALL_ACTIVE) // all sensors passive mode
#define RANGE_SIZE 40U
#define STATE_0_VALUE 1023U
#define STATE_1_VALUE 833U
#define STATE_2_VALUE 405U
#define STATE_3_VALUE 623U
#define IN_RANGE( val ) (raw > (val - RANGE_SIZE) && \
raw < (val + RANGE_SIZE) )
#ifndef NO_ROTATION_SENSOR
typedef enum {
STATE_0=0,
STATE_1=1,
STATE_2=2,
STATE_3=3
} RotationState; // for rotation state machine
#endif
///////////////////////////////////////////////////////////////////////////////
//
// Variables
//
///////////////////////////////////////////////////////////////////////////////
volatile unsigned char ds_channel; // current channel
unsigned char ds_activation; // channel bitmask. 1-> active
#ifndef NO_ROTATION_SENSOR
unsigned char ds_rotation; // channel bitmask. 1-> rotation
volatile int ds_rotations[3]; // rotation sensor values
static RotationState rotation_state[3]; // rotation state machine state
#endif
///////////////////////////////////////////////////////////////////////////////
//
// Functions
//
///////////////////////////////////////////////////////////////////////////////
#ifndef NO_ROTATION_SENSOR
//
// set rotation to an absolute value
// axis should be inert during the function call
// sensor: &SENSOR_1,&SENSOR_2,&SENSOR_3
//
void ds_rotation_set(unsigned* const sensor,int pos) {
unsigned channel=(unsigned) (sensor-&AD_A);
unsigned raw = (*sensor) >> 6;
RotationState state;
if(sensor>=&AD_A && sensor<=&AD_C) { // catch range violations
if ( raw <= STATE_2_VALUE ) // determine current state
state=STATE_2;
else if ( raw <= STATE_3_VALUE )
state=STATE_3;
else if ( raw <= STATE_1_VALUE )
state=STATE_1;
else
state=STATE_0;
rotation_state[channel]=state;
ds_rotations[channel]=pos; // reset counter
}
}
//
// process rotation sensor on current A/D channel
// global ds_channel: current channel
//
void ds_rotation_handler() {
unsigned channel =ds_channel;
unsigned raw =(*((&AD_A)+channel)) >> 6;
int cooked =ds_rotations[channel];
RotationState state=rotation_state[channel];
//
// the rotation sensor has 4 distinct output levels:
// ~1023 [STATE_0], ~768 [STATE_1], ~357 [STATE_2], ~525 [STATE_3]
// where ~ signifies +-10
//
// finite state machine tracks position
//
unsigned int cnt; //ERH
lcd_int ( raw ); //ERH
// lcd_int (state); //ERH
lcd_refresh (); //ERH
for ( cnt = 0; cnt < 10000; cnt++ ); //ERH
switch(state)
{
case STATE_0:
if ( IN_RANGE ( STATE_3_VALUE ) )
{
state=STATE_3;
cooked--;
}
else if ( IN_RANGE ( STATE_1_VALUE ) )
{
state=STATE_1;
cooked++;
}
break;
case STATE_1:
if ( IN_RANGE ( STATE_0_VALUE ) )
{
state=STATE_0;
cooked--;
}
else if( IN_RANGE ( STATE_2_VALUE ) )
{
state=STATE_2;
cooked++;
}
break;
case STATE_2:
if ( IN_RANGE ( STATE_3_VALUE ) )
{
state=STATE_3;
cooked++;
}
else if ( IN_RANGE ( STATE_1_VALUE ) )
{
state=STATE_1;
cooked--;
}
break;
case STATE_3:
if ( IN_RANGE ( STATE_0_VALUE ) )
{
state=STATE_0;
cooked++;
}
else if( IN_RANGE ( STATE_2_VALUE ) )
{
state=STATE_2;
cooked--;
}
break;
}
//
// update data
//
rotation_state[channel]=state;
ds_rotations[channel]=cooked;
}
#endif // NO_ROTATION_SENSOR
//
// sensor A/D conversion IRQ handler
//
extern void ds_handler(void);
__asm__("
.text
.align 1
_ds_handler:
; r6 saved by ROM
mov.b @_ds_channel,r6l ; r6l = current channel
mov.b @_ds_activation,r6h ; r6h = activation bitmask
btst r6l,r6h ; activate output?
beq 0f
bset r6l,@0xbb:8 ; activate output
0:
#ifndef NO_ROTATION_SENSOR
mov.b @_ds_rotation,r6h ; r6h = rotation bitmask
btst r6l,r6h ; process rotation sensor?
beq 1f
push r0 ; save r0..r3
push r1
push r2
push r3 ; r4..r6 saved by gcc if necessary
jsr _ds_rotation_handler ; process rotation sensor
pop r3
pop r2
pop r1
pop r0
1:
#endif
inc r6l ; next channel
and #0x03,r6l ; limit to 0-3
mov.b r6l,@_ds_channel
bclr r6l,@0xbb:8 ; output inactive
mov.b @0xe8:8,r6h ; r6h = A/D CSR
and.b #0x7c,r6h ; scan next channel
or.b r6l,r6h
mov.b r6h,@0xe8:8
bset #0x5,@0xe8:8 ; go!
rts
");
//
// initialize sensor a/d conversion
// all sensors set to passive mode
// rotation tracking disabled
//
void ds_init(void) {
int i;
ROM_PORT6_DDR|=DS_ALL_ACTIVE; // notify ROM we are using
PORT6_DDR |=DS_ALL_ACTIVE; // PORT6 bit 0..2 as outputs
ds_activation=0; // all sensors passive
ds_channel =0;
#ifndef NO_ROTATION_SENSOR
ds_rotation =0; // rotation tracking disabled
#endif
AD_IRQ =&ds_handler; // setup IRQ
AD_CR &=~ADCR_EXTERN;
AD_CSR =ADCSR_TIME_266 | ADCSR_GROUP_0 | ADCSR_AN_0 |
ADCSR_ENABLE_IRQ | ADCSR_START;
#ifndef NO_CONSOLE_IO
delay(10); // wait for initial A/D
#else
# warning "Rotation initialization might fail."
#endif
}
//
// shutdown sensor a/d conversion
// all sensors set to passive mode
//
void ds_shutdown(void) {
AD_CSR=0x00;
PORT6 &=DS_ALL_PASSIVE;
ROM_PORT6_DDR&=DS_ALL_PASSIVE;
PORT6_DDR &=DS_ALL_PASSIVE;
}
#endif
> Markus L. Noga noga@inrialpes.fr
> Check out legOS! http://www.multimania.com/legos/
>
> --
> Did you check the web site first?: http://www.crynwr.com/lego-robotics
|
|
Message has 1 Reply:
 | | Re: Rotation and Touch Sensors on one input
|
| (...) In the file that I just posted there is a section which looks like this in ds_rotation_handler () unsigned int cnt; //ERH lcd_int ( raw ); //ERH // lcd_int (state); //ERH lcd_refresh (); //ERH for ( cnt = 0; cnt < 10000; cnt++ ); //ERH This (...) (26 years ago, 28-Jan-99, to lugnet.robotics)
|
Message is in Reply To:
| | Re: Rotation and Touch Sensors on one input
|
| Hi Eric, (...) You're welcome. Could you be more specific than "fairly well"? I've heard different stories. Some people reported rotation works perfectly in one direction and not at all in the other. Others experienced other results. Did you modify (...) (26 years ago, 28-Jan-99, to lugnet.robotics)
|
4 Messages in This Thread:
 
- Entire Thread on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
This Message and its Replies on One Page:
- Nested:
All | Brief | Compact | Dots
Linear:
All | Brief | Compact
|
|
|
|
Active threads in Robotics
|
|
|
|
|
|
