/demos/trunk/rtdac4/test.c |
---|
0,0 → 1,47 |
/* |
* Project: S.Ha.R.K. |
* |
* Coordinators: |
* Giorgio Buttazzo <giorgio@sssup.it> |
* Paolo Gai <pj@gandalf.sssup.it> |
* |
* Authors : |
* Giacomo Guidi <giacomo@gandalf.sssup.it> |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://shark.sssup.it |
*/ |
#include "kernel/kern.h" |
#include "rtdac4.h" |
int main(int argc, char **argv) |
{ |
char buf_char[4]; |
short buf_short[2]; |
long prescaler; |
RTDAC4_open(); |
RTDAC4_read_states(buf_char); |
buf_short[0] = 0; |
buf_short[1] = 0; |
RTDAC4_set_motor(buf_short); |
/* Reset encoders */ |
RTDAC4_ioctl(1, 0); |
prescaler = 0; |
/* Set prescaler */ |
RTDAC4_ioctl(2, prescaler); |
return 0; |
} |
/demos/trunk/rtdac4/rtdac4.c |
---|
0,0 → 1,142 |
#include "kernel/kern.h" |
#define BASE_ADDRESS 0xDC00 // Base address of PCI card |
#define MAX_SIGNAL 650 // Max output signal |
int i; |
int b; |
short tempNbr1, tempNbr2, tempNbr3, tempNbr4, tempNbr5; |
int RTDAC4_open() { |
printk("Init RT-DAC4/PCI - Base Address (%x)\n",(int)(BASE_ADDRESS)); |
return 0; |
} |
int RTDAC4_close() { |
return 0; |
} |
/* Reads state information into vector buf. |
Angle address: BASE_ADDRESS + 8 (16 bits) |
Cart address: BASE_ADDRESS + 12 (16 bits) |
*/ |
int RTDAC4_read_states(char *buf) { |
for (i=0; i<2; i++) { |
*(buf + i) = inp(BASE_ADDRESS + 4 +8 + i); |
*(buf + 2 + i) = inp(BASE_ADDRESS + 4 + 12 + i); |
} |
return 0; |
} |
/* Control the electric motor. |
Max output when testing: 650. |
Max to motor: 1023. |
*/ |
int RTDAC4_set_motor(const short *buff) { |
unsigned short dir; |
unsigned short brake; |
unsigned short readBack; |
if (BASE_ADDRESS == 0) |
return 0; |
dir = inp(BASE_ADDRESS + 4*0x24); // ? |
brake = inp(BASE_ADDRESS + 4*0x25); // ? |
tempNbr1 = (short) buff[0]; |
tempNbr2 = (short) buff[1]; |
if(abs(tempNbr1) <= MAX_SIGNAL) { |
outpw(BASE_ADDRESS + 4*0x21, ((short) abs((int) tempNbr1))); |
readBack = inpw(BASE_ADDRESS + 4*0x21); |
if (readBack == 0) |
brake = brake | 0x01; |
else |
brake = brake & 0xFE; |
if ((short) tempNbr1 < 0) |
dir = dir | 0x01; |
else |
dir = dir & 0xFE; |
} |
if(abs(tempNbr2) <= MAX_SIGNAL) { |
outpw(BASE_ADDRESS + 4*0x22, ((short) abs((int) tempNbr2))); |
readBack = inpw(BASE_ADDRESS + 4*0x22); |
if (readBack == 0) |
brake = brake | 0x02; |
else |
brake = brake & 0xFD; |
if ((short) tempNbr1 < 0) |
dir = dir | 0x02; |
else |
dir = dir & 0xFD; |
} |
outpw(BASE_ADDRESS + 4*0x24, dir); |
outpw(BASE_ADDRESS + 4*0x25, brake); |
return 0; |
} |
int RTDAC4_ioctl(unsigned int request, unsigned long l) { |
/** |
* Reset Encoders |
*/ |
if(request == 1) { |
unsigned short aux; |
tempNbr1 = 0; |
tempNbr2 = 0; |
tempNbr3 = 0; |
tempNbr4 = 0; |
tempNbr5 = 0; |
if ( BASE_ADDRESS == 0 ) |
{ |
printk("RTDAC4 ERROR: Reset encoders: BASE_ADDRESS = 0\n"); |
return -1; |
} |
aux = inp(BASE_ADDRESS + 4*0x05); |
if( tempNbr1 == 0 ) aux = aux & 0x00FE; |
if( tempNbr1 == 1 ) aux = aux | 0x0001; |
if( tempNbr2 == 0 ) aux = aux & 0x00FD; |
if( tempNbr2 == 1 ) aux = aux | 0x0002; |
if( tempNbr3 == 0 ) aux = aux & 0x00FB; |
if( tempNbr3 == 1 ) aux = aux | 0x0004; |
if( tempNbr4 == 0 ) aux = aux & 0x00F7; |
if( tempNbr4 == 1 ) aux = aux | 0x0008; |
if( tempNbr5 == 0 ) aux = aux & 0x00EF; |
if( tempNbr5 == 1 ) aux = aux | 0x0010; |
outp(BASE_ADDRESS + 4*0x05, aux); |
aux = inp(BASE_ADDRESS + 4*0x05); |
return 0; |
} |
/** |
* Prescaler. |
*/ |
else if(request == 2) { |
tempNbr1 = (short) l; |
if ( BASE_ADDRESS == 0 ) { |
return -1; |
} |
if( ( tempNbr1 >= 0 ) && ( tempNbr1 <= 63 ) ) |
{ |
outp(BASE_ADDRESS + 4*0x18, tempNbr1); |
} |
return 0; |
} |
return 0; |
} |
/demos/trunk/rtdac4/initfile.c |
---|
0,0 → 1,164 |
/* |
* Project: S.Ha.R.K |
* |
* Coordinators: Giorgio Buttazzo <giorgio@sssup.it> |
* |
* ReTiS Lab (Scuola Superiore S.Anna - Pisa - Italy) |
* |
* http://www.sssup.it |
* http://retis.sssup.it |
* http://hartik.sssup.it |
*/ |
#include "kernel/kern.h" |
#include "modules/edf.h" |
#include "modules/cbs.h" |
#include "modules/rr.h" |
#include "modules/dummy.h" |
#include "modules/intdrive.h" |
#include "modules/sem.h" |
#include "modules/hartport.h" |
#include "modules/pi.h" |
#include "modules/pc.h" |
#include "modules/srp.h" |
#include "modules/npp.h" |
#include "modules/nop.h" |
#include <drivers/shark_linuxc26.h> |
#include <drivers/shark_input26.h> |
#include <drivers/shark_keyb26.h> |
#define FRAME_BUFFER_DEVICE 0 |
/*+ sysyem tick in us +*/ |
#define TICK 0 |
/*+ RR tick in us +*/ |
#define RRTICK 10000 |
/*+ Interrupt Server +*/ |
#define INTDRIVE_Q 1000 |
#define INTDRIVE_T 10000 |
#define INTDRIVE_FLAG 0 |
void call_shutdown_task(void *arg); |
int device_drivers_init(); |
int device_drivers_close(); |
void set_shutdown_task(); |
TASK shutdown_task_body(void *arg); |
PID shutdown_task_PID = 1; |
TIME __kernel_register_levels__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
INTDRIVE_register_level(INTDRIVE_Q,INTDRIVE_T,INTDRIVE_FLAG); |
EDF_register_level(EDF_ENABLE_ALL); |
CBS_register_level(CBS_ENABLE_ALL, 1); |
RR_register_level(RRTICK, RR_MAIN_YES, mb); |
dummy_register_level(); |
SEM_register_module(); |
PI_register_module(); |
PC_register_module(); |
NPP_register_module(); |
SRP_register_module(); |
NOP_register_module(); |
return TICK; |
} |
TASK __init__(void *arg) |
{ |
struct multiboot_info *mb = (struct multiboot_info *)arg; |
HARTPORT_init(); |
/* Create the shutdown task. It will be activated at RUNLEVEL |
SHUTDOWN */ |
set_shutdown_task(); |
/* Init the drivers */ |
device_drivers_init(); |
/* Set the shutdown task activation */ |
sys_atrunlevel(call_shutdown_task, NULL, RUNLEVEL_SHUTDOWN); |
__call_main__(mb); |
return (void *)0; |
} |
void set_shutdown_task() { |
/* WARNING: the shutdown task is a background thread. It cannot execute |
if the system is overloaded */ |
NRT_TASK_MODEL nrt; |
nrt_task_default_model(nrt); |
nrt_task_def_system(nrt); |
shutdown_task_PID = task_create("Shutdown Task",shutdown_task_body,&nrt,NULL); |
if (shutdown_task_PID == NIL) { |
sys_shutdown_message("Error: Cannot create shutdown task\n"); |
sys_end(); |
} |
} |
int device_drivers_init() { |
KEYB_PARMS kparms = BASE_KEYB; |
LINUXC26_register_module(); |
INPUT26_init(); |
keyb_def_ctrlC(kparms, NULL); |
KEYB26_init(&kparms); |
return 0; |
} |
int device_drivers_close() { |
KEYB26_close(); |
INPUT26_close(); |
return 0; |
} |
#define SHUTDOWN_TIMEOUT_SEC 3 |
void call_shutdown_task(void *arg) |
{ |
struct timespec t; |
sys_gettime(&t); |
t.tv_sec += SHUTDOWN_TIMEOUT_SEC; |
/* Emergency timeout to exit from RUNLEVEL_SHUTDOWN */ |
kern_event_post(&t,(void *)((void *)sys_abort_shutdown),(void *)0); |
task_activate(shutdown_task_PID); |
} |
TASK shutdown_task_body(void *arg) { |
device_drivers_close(); |
sys_shutdown_message("-- S.Ha.R.K. Closed --\n"); |
sys_abort_shutdown(0); |
return NULL; |
} |
/demos/trunk/rtdac4/rtdac4.h |
---|
0,0 → 1,25 |
#ifndef __RTDAC4__ |
#define __RTDAC4__ |
/* Open the RT-DAC4/PCI card */ |
int RTDAC4_open(void); |
/* Close the RT-DAC4/PCI card */ |
int RTDAC4_close(void); |
/* Reads state information into vector buf. |
Angle address: BASE_ADDRESS + 8 (16 bits) |
Cart address: BASE_ADDRESS + 12 (16 bits) |
*/ |
int RTDAC4_read_states(char *buf); |
/* Control the electric motor. |
Max output when testing: 650. |
Max to motor: 1023. |
*/ |
int RTDAC4_set_motor(const short *buff); |
/* IOCTL emulation */ |
int RTDAC4_ioctl(unsigned int request, unsigned long l); |
#endif |
/demos/trunk/rtdac4/makefile |
---|
0,0 → 1,16 |
# |
# |
# |
ifndef BASE |
BASE=../.. |
endif |
include $(BASE)/config/config.mk |
PROGS= test |
include $(BASE)/config/example.mk |
test: |
make -f $(SUBMAKE) APP=test INIT= OTHEROBJS="initfile.o rtdac4.o" SHARKOPT="__LINUXC26__ __PCI__ __INPUT__" |