usr_servo code generation
This commit is contained in:
103
python/usr_code/Makefile
Normal file
103
python/usr_code/Makefile
Normal file
@@ -0,0 +1,103 @@
|
||||
#------------------------------------------------------------------------------
|
||||
# Copyright (C) Delta Tau Data Systems Inc., 2007
|
||||
# All rights reserved.
|
||||
#
|
||||
# Generic makefile for any c realtime C plc 0, user servo or user phase
|
||||
# For a new project change the following
|
||||
#
|
||||
# 1.) usralgo-objs should be assigned the 'C' source code files that need to be compiled
|
||||
# 2.) issue the command 'make depend' the first time a project is created and
|
||||
# (every time an additional 'C' file is added to the project the command
|
||||
# 'make depend' must be issued)
|
||||
# 3.) issue the command make clean
|
||||
# 4.) issue the command make
|
||||
#
|
||||
# Notes
|
||||
# --------
|
||||
# Change DTDEBUG above to -O2 for release w/ optimization
|
||||
# Change DTDEBUG above to -g3 for debug
|
||||
# arm,i386,i385hv,ppc460-2,ppc460-1,ppc405
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
PPMAC=SAR-CPPM-EXPMX1
|
||||
PPMAC=MOTTEST-CPPM-CRM0573
|
||||
#PPMAC=SAROP11-CPPM-MOT6871
|
||||
|
||||
PMAC_ARCH=ppc465-2
|
||||
|
||||
ifeq ($(PMAC_ARCH),ppc465-2)
|
||||
ARCH=powerpc
|
||||
#CROSS_COMPILE=powerpc-meau-linux-gnu-
|
||||
CROSS_COMPILE=/opt/eldk-4.2/usr/bin/ppc_4xxFP-
|
||||
KDIR=/opt/powerpc-465-rootfs/usr/src/linux-3.2.21-serengeti-smp
|
||||
KSRC=/opt/powerpc-465-rootfs/usr/src/linux-3.2.21-serengeti-smp
|
||||
#CC=powerpc-meau-linux-gnu-gcc
|
||||
#AS=powerpc-meau-linux-gnu-as
|
||||
#LD=powerpc-meau-linux-gnu-gcc
|
||||
CC=/opt/eldk-4.2/usr/bin/ppc_4xxFP-gcc
|
||||
AS=/opt/eldk-4.2/usr/bin/ppc_4xxFP-gcc
|
||||
LD=/opt/eldk-4.2/usr/bin/ppc_4xxFP-gcc
|
||||
#STRIP=i686-meau-linux-gnu-strip
|
||||
INCLUDE=/opt/powerpc-465-rootfs/usr/lib/gcc/powerpc-linux-gnu/4.6/include
|
||||
XENOMAI_INC_DIR=/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/include
|
||||
XENOMAI_LIB_DIR=/opt/powerpc-465-rootfs/usr/local/xenomai-2.6.2.1/lib
|
||||
RPATH=-Wl,-rpath-link,/opt/powerpc-465-rootfs/lib/powerpc-linux-gnu
|
||||
ROOTFS_DIR=/opt/powerpc-465-rootfs
|
||||
endif
|
||||
|
||||
#RTPMACINCLUDEDIR=/usr/local/dtlibs/rtpmac
|
||||
#LIBPPMACINCLUDEDIR=/usr/local/dtlibs/libppmac
|
||||
#THESE PATHS MUST BE ABSOLUTE !
|
||||
RTPMACINCLUDEDIR=/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/rtpmac/
|
||||
LIBPPMACINCLUDEDIR=/opt/eldk-4.2/PPMAC_rootfs-7-wheezy/opt/ppmac/libppmac/
|
||||
|
||||
export ARCH
|
||||
export CROSS_COMPILE
|
||||
|
||||
OBJS := ${patsubst %, %.o, $(MODULES)}
|
||||
CLEANMOD := ${patsubst %, .%*, $(MODULES)}
|
||||
PWD := $(shell if [ "$$PWD" != "" ]; then echo $$PWD; else pwd; fi)
|
||||
|
||||
|
||||
obj-m += usralgo.o
|
||||
usralgo-objs := usralgomain.o \
|
||||
usrcode.o
|
||||
|
||||
LDFLAGS := -nostdlib
|
||||
EXTRA_CFLAGS := -O2 -DCONFIG_460EX -D_GNU_SOURCE -D_REENTRANT -D__XENO__ -mhard-float -I$(RTPMACINCLUDEDIR) -I$(LIBPPMACINCLUDEDIR) -I$(XENOMAI_INC_DIR) -I$(XENOMAI_INC_DIR)/posix -I$(KSRC)/include/xenomai -I$(KSRC)/include/xenomai/posix -I$(INCLUDE) $(ADD_CFLAGS) --sysroot=$(ROOTFS_DIR)
|
||||
KBUILD_EXTRA_SYMBOLS := /usr/local/dtlibs/libppmac/Module.symvers
|
||||
|
||||
%.o: %.S
|
||||
$(CC) -s -D__KERNEL__ -x c -E $< -o $*.i
|
||||
$(AS) -mbooke -o $@ $*.i
|
||||
|
||||
#all::
|
||||
# cp -f /usr/local/usralgo/usralgomain.c $(PWD)
|
||||
all:
|
||||
$(MAKE) -C $(KSRC) SUBDIRS=$(PWD) modules
|
||||
#scp usralgo.ko root@$(PPMAC):/tmp
|
||||
#scp pp_proj.ini root@$(PPMAC):/var/ftp/usrflash/Project/Configuration/pp_proj.ini
|
||||
#scp pp_proj.ini root@$(PPMAC):/tmp/pp_proj.ini
|
||||
#ssh root@$(PPMAC) projpp
|
||||
|
||||
# $(MAKE) -C $(KSRC) SUBDIRS=$(PWD) modules V=1
|
||||
# mv -f usralgo.ko ../../bin/Debug/
|
||||
|
||||
|
||||
#modules:
|
||||
# @echo "$(CFLAGS)"
|
||||
|
||||
bclean::
|
||||
$(RM) *.o .*.o.d .*.o.cmd *.ko *.log
|
||||
$(RM) -R .tmp*
|
||||
$(RM) .runinfo
|
||||
rm -rf .runinfo .tmp* .*.o.d .*.o.cmd .*.cmd *.o *.ko *.mod.c *.i *.so Module.symvers modules.order
|
||||
|
||||
clean::
|
||||
$(RM) *.o .*.o.d .*.o.cmd *.ko
|
||||
$(RM) -R .tmp*
|
||||
$(RM) .runinfo
|
||||
rm -rf .runinfo .tmp* .*.o.d .*.o.cmd .*.cmd *.o *.ko *.mod.c *.i *.so Module.symvers modules.order
|
||||
|
||||
dbg:
|
||||
@echo PATH $(PATH)
|
||||
189
python/usr_code/pp_proj.h
Executable file
189
python/usr_code/pp_proj.h
Executable file
@@ -0,0 +1,189 @@
|
||||
|
||||
#ifndef _PP_PROJ_H_
|
||||
#define _PP_PROJ_H_
|
||||
//***********************************************************************************
|
||||
// C header for accessing PMAC Global, CSGlobal, Ptr vars
|
||||
// _PPScriptMode_ for Pmac Script like access global & csglobal
|
||||
// global Mypvar - access with "Mypvar"
|
||||
// global Myparray(32) - access with "Myparray(i)"
|
||||
// csglobal Myqvar - access with "Myqvar(i)" where "i" is Coord #
|
||||
// csglobal Myqarray(16) - access with "Myqvar(i,j)" where "j" is index
|
||||
// _EnumMode_ for Pmac enum data type checking on Set & Get global functions
|
||||
// Example
|
||||
// global Mypvar
|
||||
// csglobal Myqvar
|
||||
// "SetGlobalVar(Myqvar, data)" will give a compile error because its a csglobal var.
|
||||
// "SetCSGlobalVar(Mypvar, data)" will give a compile error because its a global var.
|
||||
//************************************************************************************
|
||||
|
||||
#ifdef _PPScriptMode_
|
||||
enum globalP {_globalP_=-1};
|
||||
enum globalParray {_globalParray_=-1};
|
||||
enum csglobalQ {_csglobalQ_=-1};
|
||||
enum csglobalQarray {_csglobalQarray_=-1};
|
||||
|
||||
enum ptrM {_ptrM_=-1};
|
||||
enum ptrMarray {_ptrMarray_=-1};
|
||||
void SetEnumGlobalVar(enum globalP var, double data)
|
||||
{
|
||||
pshm->P[var] = data;
|
||||
}
|
||||
|
||||
double GetEnumGlobalVar(enum globalP var)
|
||||
{
|
||||
return pshm->P[var];
|
||||
}
|
||||
|
||||
void SetEnumGlobalArrayVar(enum globalParray var, unsigned index, double data)
|
||||
{
|
||||
pshm->P[(var + index)%MAX_P] = data;
|
||||
}
|
||||
|
||||
double GetEnumGlobalArrayVar(enum globalParray var, unsigned index)
|
||||
{
|
||||
return pshm->P[(var + index)%MAX_P];
|
||||
}
|
||||
|
||||
void SetEnumCSGlobalVar(enum csglobalQ var, unsigned cs, double data)
|
||||
{
|
||||
pshm->Coord[cs % MAX_COORDS].Q[var] = data;
|
||||
}
|
||||
|
||||
double GetEnumCSGlobalVar(enum csglobalQ var, unsigned cs)
|
||||
{
|
||||
return pshm->Coord[cs % MAX_COORDS].Q[var];
|
||||
}
|
||||
|
||||
void SetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs, double data)
|
||||
{
|
||||
pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q] = data;
|
||||
}
|
||||
|
||||
double GetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs)
|
||||
{
|
||||
return pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q];
|
||||
}
|
||||
|
||||
void SetEnumPtrVar(enum ptrM var, double data)
|
||||
{
|
||||
im_write(pshm->Mdef + var, data, &pshm->Ldata);
|
||||
}
|
||||
|
||||
double GetEnumPtrVar(enum ptrM var)
|
||||
{
|
||||
return im_read(pshm->Mdef + var, &pshm->Ldata);
|
||||
}
|
||||
|
||||
void SetEnumPtrArrayVar(enum ptrMarray var, unsigned index, double data)
|
||||
{
|
||||
im_write(pshm->Mdef + ((var + index)%MAX_M), data, &pshm->Ldata);
|
||||
}
|
||||
|
||||
double GetEnumPtrArrayVar(enum ptrMarray var, unsigned index)
|
||||
{
|
||||
return im_read(pshm->Mdef + ((var + index)%MAX_M), &pshm->Ldata);
|
||||
}
|
||||
|
||||
#define SetGlobalVar(i, x) SetEnumGlobalVar(i, x)
|
||||
#define SetGlobalArrayVar(i, j, x) SetEnumGlobalArrayVar(i, j, x)
|
||||
#define GetGlobalVar(i) GetEnumGlobalVar(i)
|
||||
#define GetGlobalArrayVar(i, j) GetEnumGlobalArrayVar(i, j)
|
||||
|
||||
#define SetCSGlobalVar(i, j, x) SetEnumCSGlobalVar(i, j, x)
|
||||
#define SetCSGlobalArrayVar(i, j, k, x) SetEnumCSGlobalArrayVar(i, j, k, x)
|
||||
#define GetCSGlobalVar(i, j) GetEnumCSGlobalVar(i, j)
|
||||
#define GetCSGlobalArrayVar(i, j, k) GetEnumCSGlobalArrayVar(i, j, k)
|
||||
|
||||
#define SetPtrVar(i, x) SetEnumPtrVar(i, x)
|
||||
#define SetPtrArrayVar(i, j, x) SetEnumPtrArrayVar(i, j, x)
|
||||
#define GetPtrVar(i) GetEnumPtrVar(i)
|
||||
#define GetPtrArrayVar(i, j) GetEnumPtrArrayVar(i, j)
|
||||
// end of #ifdef _PPScriptMode_
|
||||
#else
|
||||
#ifdef _EnumMode_
|
||||
enum globalP {_globalP_=-1};
|
||||
enum globalParray {_globalParray_=-1};
|
||||
enum csglobalQ {_csglobalQ_=-1};
|
||||
enum csglobalQarray {_csglobalQarray_=-1};
|
||||
enum ptrM {_ptrM_=-1};
|
||||
enum ptrMarray {_ptrMarray_=-1};
|
||||
void SetEnumGlobalVar(enum globalP var, double data)
|
||||
{
|
||||
pshm->P[var] = data;
|
||||
}
|
||||
|
||||
double GetEnumGlobalVar(enum globalP var)
|
||||
{
|
||||
return pshm->P[var];
|
||||
}
|
||||
|
||||
void SetEnumGlobalArrayVar(enum globalParray var, unsigned index, double data)
|
||||
{
|
||||
pshm->P[(var + index)%MAX_P] = data;
|
||||
}
|
||||
|
||||
double GetEnumGlobalArrayVar(enum globalParray var, unsigned index)
|
||||
{
|
||||
return pshm->P[(var + index)%MAX_P];
|
||||
}
|
||||
|
||||
void SetEnumCSGlobalVar(enum csglobalQ var, unsigned cs, double data)
|
||||
{
|
||||
pshm->Coord[cs % MAX_COORDS].Q[var] = data;
|
||||
}
|
||||
|
||||
double GetEnumCSGlobalVar(enum csglobalQ var, unsigned cs)
|
||||
{
|
||||
return pshm->Coord[cs % MAX_COORDS].Q[var];
|
||||
}
|
||||
|
||||
void SetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs, double data)
|
||||
{
|
||||
pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q] = data;
|
||||
}
|
||||
|
||||
double GetEnumCSGlobalArrayVar(enum csglobalQarray var, unsigned index, unsigned cs)
|
||||
{
|
||||
return pshm->Coord[cs % MAX_COORDS].Q[(var + index)%MAX_Q];
|
||||
}
|
||||
|
||||
void SetEnumPtrVar(enum ptrM var, double data)
|
||||
{
|
||||
im_write(pshm->Mdef + var, data, &pshm->Ldata);
|
||||
}
|
||||
|
||||
double GetEnumPtrVar(enum ptrM var)
|
||||
{
|
||||
return im_read(pshm->Mdef + var, &pshm->Ldata);
|
||||
}
|
||||
|
||||
void SetEnumPtrArrayVar(enum ptrMarray var, unsigned index, double data)
|
||||
{
|
||||
im_write(pshm->Mdef + ((var + index)%MAX_M), data, &pshm->Ldata);
|
||||
}
|
||||
|
||||
double GetEnumPtrArrayVar(enum ptrMarray var, unsigned index)
|
||||
{
|
||||
return im_read(pshm->Mdef + ((var + index)%MAX_M), &pshm->Ldata);
|
||||
}
|
||||
|
||||
#define SetGlobalVar(i, x) SetEnumGlobalVar(i, x)
|
||||
#define SetGlobalArrayVar(i, j, x) SetEnumGlobalArrayVar(i, j, x)
|
||||
#define GetGlobalVar(i) GetEnumGlobalVar(i)
|
||||
#define GetGlobalArrayVar(i, j) GetEnumGlobalArrayVar(i, j)
|
||||
|
||||
#define SetCSGlobalVar(i, j, x) SetEnumCSGlobalVar(i, j, x)
|
||||
#define SetCSGlobalArrayVar(i, j, k, x) SetEnumCSGlobalArrayVar(i, j, k, x)
|
||||
#define GetCSGlobalVar(i, j) GetEnumCSGlobalVar(i, j)
|
||||
#define GetCSGlobalArrayVar(i, j, k) GetEnumCSGlobalArrayVar(i, j, k)
|
||||
|
||||
#define SetPtrVar(i, x) SetEnumPtrVar(i, x)
|
||||
#define SetPtrArrayVar(i, j, x) SetEnumPtrArrayVar(i, j, x)
|
||||
#define GetPtrVar(i) GetEnumPtrVar(i)
|
||||
#define GetPtrArrayVar(i, j) GetEnumPtrArrayVar(i, j)
|
||||
// end of #ifdef _EnumMode_
|
||||
#else
|
||||
// ***** Standard default mode *****
|
||||
#endif
|
||||
#endif
|
||||
#endif //_PP_PROJ_H_
|
||||
114
python/usr_code/usralgomain.c
Normal file
114
python/usr_code/usralgomain.c
Normal file
@@ -0,0 +1,114 @@
|
||||
//---------------------------------------------------------------------------
|
||||
// Project PowerPMAC Firmware
|
||||
// Delta Tau Data Systems, Inc.
|
||||
// Copyright 2007. All Rights Reserved.
|
||||
//
|
||||
// SUBSYSTEM: User Algo Driver
|
||||
// FILE: usralgo.c
|
||||
// AUTH OR: Henry Bausley
|
||||
//
|
||||
// OVERVIEW
|
||||
// ~~~~~~~~
|
||||
// This file is a device driver that exports functions that can be used by other
|
||||
// kernel mode modules. ie. RtPmac.rtl can call functions that are part of this
|
||||
// driver for use as user written servo, phase, plc rti, plc rt thread
|
||||
// No one should ever have to touch this module! Custom routines are added
|
||||
// as additional modules as whatever name the user wants.
|
||||
//
|
||||
// NOTES
|
||||
// ~~~~~
|
||||
// - The driver must be loaded
|
||||
// bash-3.0# insmod /opt/ppmac/usralgo/usralgo.ko
|
||||
//
|
||||
// - A node can be created on the file system the thing you fopen
|
||||
// but it is not required. Could be used by gpos if ioctl's were added
|
||||
// bash-3.0# mknod -m 666 /dev/usralgo c 240 0
|
||||
//
|
||||
//--------------------------------------------------------------------------------
|
||||
#include <linux/module.h>
|
||||
#include <linux/init.h>
|
||||
#include <linux/version.h>
|
||||
#include <linux/errno.h>
|
||||
#include <linux/fs.h>
|
||||
#include <linux/mm.h>
|
||||
#include <linux/interrupt.h>
|
||||
#include <linux/sched.h>
|
||||
#include <asm/uaccess.h>
|
||||
#include <asm/io.h>
|
||||
#include <linux/vmalloc.h>
|
||||
#include <linux/mman.h>
|
||||
#include <linux/slab.h>
|
||||
#include <RtGpShm.h>
|
||||
|
||||
|
||||
//#define KDBEUG 1
|
||||
|
||||
#ifdef KDBEUG
|
||||
#define PRINTK(arg) printk(arg)
|
||||
#else
|
||||
#define PRINTK(arg)
|
||||
#endif
|
||||
|
||||
#define USRALGO_MAJOR 240
|
||||
#define USRALGO_NAME "usralgo"
|
||||
|
||||
struct SHM *pshm; // Pointer to shared memory
|
||||
volatile unsigned *piom; // Pointer to I/O memory
|
||||
void *pushm; // Pointer to user memory
|
||||
|
||||
|
||||
// define which file operations are supported
|
||||
struct file_operations usralgo_fops =
|
||||
{
|
||||
.owner = THIS_MODULE,
|
||||
.llseek = NULL,
|
||||
.read = NULL,
|
||||
.write = NULL,
|
||||
// .readdir = NULL,
|
||||
.poll = NULL,
|
||||
#if LINUX_VERSION_CODE < KERNEL_VERSION(3,0,0)
|
||||
.ioctl = NULL,
|
||||
#else
|
||||
.unlocked_ioctl = NULL,
|
||||
#endif
|
||||
.mmap = NULL,
|
||||
.open = NULL,
|
||||
.flush = NULL,
|
||||
.release = NULL,
|
||||
.fsync = NULL,
|
||||
.fasync = NULL,
|
||||
.lock = NULL,
|
||||
};
|
||||
|
||||
//-----------------------------------------------------------------------
|
||||
// initialize module -- this what is called when you do insmod ./usralgo.ko
|
||||
//-----------------------------------------------------------------------
|
||||
static int __init usralgo_init_module (void)
|
||||
{
|
||||
int i;
|
||||
|
||||
PRINTK(KERN_INFO "initializing module usralgo\n");
|
||||
pshm = GetSharedMemPtr();
|
||||
pushm = GetUserBufferPtr();
|
||||
piom = GetIOMemPtr();
|
||||
|
||||
i = register_chrdev (USRALGO_MAJOR, USRALGO_NAME, &usralgo_fops);
|
||||
if (i != 0) return - EIO;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
//-----------------------------------------------------------------------
|
||||
// close and cleanup module
|
||||
//-------------------------
|
||||
static void __exit usralgo_cleanup_module (void)
|
||||
{
|
||||
PRINTK("cleaning up module usralgo\n");
|
||||
unregister_chrdev (USRALGO_MAJOR, USRALGO_NAME);
|
||||
}
|
||||
|
||||
module_init(usralgo_init_module);
|
||||
module_exit(usralgo_cleanup_module);
|
||||
MODULE_AUTHOR("www.deltatau.com");
|
||||
MODULE_LICENSE("GPL");
|
||||
MODULE_DESCRIPTION("Linux Device Driver for usralgo");
|
||||
117
python/usr_code/usrcode_template.c
Normal file
117
python/usr_code/usrcode_template.c
Normal file
@@ -0,0 +1,117 @@
|
||||
//---------------------------------------------------------------------------
|
||||
// Project PowerPMAC Firmware
|
||||
// Delta Tau Data Systems, Inc.
|
||||
// Copyright 2007. All Rights Reserved.
|
||||
//
|
||||
// SUBSYSTEM: User Servo Driver
|
||||
// FILE: usrcode.c
|
||||
// TEMPLATE AUTHOR: Henry Bausley
|
||||
//
|
||||
// OVERVIEW
|
||||
// ~~~~~~~~
|
||||
// This file is where exportable user code can be placed.
|
||||
// To make a function callable as a user servo do three steps
|
||||
//
|
||||
// 1.) Prototye the function user_func(void ,void );
|
||||
// 2.) Export the function EXPORT_SYMBOL(user_func);
|
||||
// 3.) Make sure useralgo.ko has been loaded with projpp.ini
|
||||
//
|
||||
//--------------------------------------------------------------------------------
|
||||
#include "usrcode.h"
|
||||
//----------------------------------------------------------------------------------
|
||||
// pp_proj.h is the C header for accessing PMAC Global, CSGlobal, Ptr vars
|
||||
// _PPScriptMode_ for Pmac Script like access global & csglobal
|
||||
// global Mypvar - access with "Mypvar"
|
||||
// global Myparray(32) - access with "Myparray(i)"
|
||||
// csglobal Myqvar - access with "Myqvar(i)" where "i" is Coord #
|
||||
// csglobal Myqarray(16) - access with "Myqvar(i,j)" where "j" is index
|
||||
// _EnumMode_ for Pmac enum data type checking on Set & Get global functions
|
||||
// Example
|
||||
// global Mypvar
|
||||
// csglobal Myqvar
|
||||
// "SetGlobalVar(Myqvar, data)" will give a compile error because its a csglobal var.
|
||||
// "SetCSGlobalVar(Mypvar, data)" will give a compile error because its a global var.
|
||||
//------------------------------------------------------------------------------------
|
||||
#define _PPScriptMode_ // uncomment for Pmac Script type access
|
||||
// #define _EnumMode_ // uncomment for Pmac enum data type checking on Set & Get global functions
|
||||
|
||||
#include "pp_proj.h"
|
||||
#ifdef __KERNEL__
|
||||
// Kernal mode can't have paths with spaces and long names
|
||||
//#include "../../PMACSC~1/GLOBAL~1/asharedwithcapp.pmh"
|
||||
#else
|
||||
//#include "../../PMAC Script Language/Global Includes/asharedwithcapp.pmh"
|
||||
#endif
|
||||
|
||||
extern struct SHM *pshm; // Pointer to shared memory
|
||||
extern volatile unsigned *piom; // Pointer to I/O memory
|
||||
extern void *pushm; // Pointer to user memory
|
||||
|
||||
<usr_code>
|
||||
|
||||
void user_phase_sample( struct MotorData *Mptr)
|
||||
{
|
||||
}
|
||||
|
||||
double usr_servo_sample(MotorData *Mptr)
|
||||
{
|
||||
pshm->P[2000]=pshm->P[2000]*.9999+abs(Mptr->PosError)*0.0001; //lowpass of Position error
|
||||
return pshm->ServoCtrl(Mptr);
|
||||
}
|
||||
|
||||
//double user_pid_ctrl( struct MotorData *Mptr)
|
||||
//{
|
||||
// //double *p;
|
||||
// //p = pushm;
|
||||
// //return 0;
|
||||
// double ctrl_out;
|
||||
// if (Mptr->ClosedLoop) {
|
||||
// // Compute PD terms
|
||||
// ctrl_out=Mptr->Servo.Kp*Mptr->PosError-Mptr->Servo.Kvfb*Mptr->ActVel;
|
||||
// Mptr->Servo.Integrator+=Mptr->PosError*Mptr->Servo.Ki; // I term
|
||||
// ctrl_out+=Mptr->Servo.Integrator;
|
||||
// // Combine
|
||||
// if (ctrl_out>2000)ctrl_out=2000;
|
||||
// if (ctrl_out<-2000)ctrl_out=-2000;
|
||||
// return ctrl_out;
|
||||
// }
|
||||
// else {
|
||||
// Mptr->Servo.Integrator=0.0;
|
||||
// return 0.0;
|
||||
// }
|
||||
//}
|
||||
|
||||
void CaptCompISR(void)
|
||||
{
|
||||
unsigned *pUnsigned = pushm;
|
||||
*pUnsigned = *pUnsigned + 1;
|
||||
}
|
||||
|
||||
double GetLocal(struct LocalData *Ldata,int m)
|
||||
{
|
||||
return *(Ldata->L + Ldata->Lindex + m);
|
||||
}
|
||||
|
||||
void SetLocal(struct LocalData *Ldata,int m,double value)
|
||||
{
|
||||
*(Ldata->L + Ldata->Lindex + m) = value;
|
||||
}
|
||||
|
||||
double *GetLocalPtr(struct LocalData *Ldata,int m)
|
||||
{
|
||||
return (Ldata->L + Ldata->Lindex + m);
|
||||
}
|
||||
|
||||
double CfromScript(double cfrom_type, double arg2, double arg3, double arg4, double arg5, double arg6, double arg7, struct LocalData *Ldata)
|
||||
{
|
||||
int icfrom_type = (int) cfrom_type;
|
||||
double *C, *D, *L, *R, rtn; // C, D, R - only needed if doing Kinmatics
|
||||
|
||||
C = GetCVarPtr(Ldata); // Only needed if doing Kinmatics
|
||||
D = GetDVarPtr(Ldata); // Only needed if doing Kinmatics
|
||||
L = GetLVarPtr(Ldata); // Only needed if using Ldata or Kinmatics
|
||||
R = GetRVarPtr(Ldata); // Only needed if doing Kinmatics
|
||||
rtn = -1.0;
|
||||
return rtn;
|
||||
}
|
||||
|
||||
134
python/usr_code/usrcode_template.h
Normal file
134
python/usr_code/usrcode_template.h
Normal file
@@ -0,0 +1,134 @@
|
||||
#ifdef __KERNEL__
|
||||
#include <linux/module.h>
|
||||
#else
|
||||
#define EXPORT_SYMBOL(x) // x
|
||||
#define KERN_ALERT
|
||||
#define printk printf
|
||||
#include <gplib.h>
|
||||
#endif
|
||||
#include <RtGpShm.h> // Global Rt/Gp Externals and structures
|
||||
#include <rtpmacapi.h>
|
||||
|
||||
int rtsprintf(char * buf, const char *fmt, ...);
|
||||
|
||||
<usr_header>
|
||||
|
||||
double usr_servo_sample(MotorData *Mptr);
|
||||
EXPORT_SYMBOL(usr_servo_sample);
|
||||
|
||||
void user_phase_sample( struct MotorData *Mptr);
|
||||
EXPORT_SYMBOL(user_phase_sample);
|
||||
|
||||
void CaptCompISR(void);
|
||||
EXPORT_SYMBOL(CaptCompISR);
|
||||
|
||||
double CfromScript(double arg1,double arg2,double arg3,double arg4,double arg5,double arg6,double arg7,struct LocalData *Ldata);
|
||||
EXPORT_SYMBOL(CfromScript);
|
||||
|
||||
//----------------------------------------
|
||||
// Required Kinematic define Names
|
||||
//----------------------------------------
|
||||
#define KinPosMotor L
|
||||
#define KinVelMotor R
|
||||
#define KinEnaAxisA 0x1
|
||||
#define KinPosAxisA C[0]
|
||||
#define KinVelAxisA C[32]
|
||||
#define KinEnaAxisB 0x2
|
||||
#define KinPosAxisB C[1]
|
||||
#define KinVelAxisB C[33]
|
||||
#define KinEnaAxisC 0x4
|
||||
#define KinPosAxisC C[2]
|
||||
#define KinVelAxisC C[34]
|
||||
#define KinEnaAxisU 0x8
|
||||
#define KinPosAxisU C[3]
|
||||
#define KinVelAxisU C[35]
|
||||
#define KinEnaAxisV 0x10
|
||||
#define KinPosAxisV C[4]
|
||||
#define KinVelAxisV C[36]
|
||||
#define KinEnaAxisW 0x20
|
||||
#define KinPosAxisW C[5]
|
||||
#define KinVelAxisW C[37]
|
||||
#define KinEnaAxisX 0x40
|
||||
#define KinPosAxisX C[6]
|
||||
#define KinVelAxisX C[38]
|
||||
#define KinEnaAxisY 0x80
|
||||
#define KinPosAxisY C[7]
|
||||
#define KinVelAxisY C[39]
|
||||
#define KinEnaAxisZ 0x100
|
||||
#define KinPosAxisZ C[8]
|
||||
#define KinVelAxisZ C[40]
|
||||
#define KinEnaAxisAA 0x200
|
||||
#define KinPosAxisAA C[9]
|
||||
#define KinVelAxisAA C[41]
|
||||
#define KinEnaAxisBB 0x400
|
||||
#define KinPosAxisBB C[10]
|
||||
#define KinVelAxisBB C[42]
|
||||
#define KinEnaAxisCC 0x800
|
||||
#define KinPosAxisCC C[11]
|
||||
#define KinVelAxisCC C[43]
|
||||
#define KinEnaAxisDD 0x1000
|
||||
#define KinPosAxisDD C[12]
|
||||
#define KinVelAxisDD C[44]
|
||||
#define KinEnaAxisEE 0x2000
|
||||
#define KinPosAxisEE C[13]
|
||||
#define KinVelAxisEE C[45]
|
||||
#define KinEnaAxisFF 0x4000
|
||||
#define KinPosAxisFF C[14]
|
||||
#define KinVelAxisFF C[46]
|
||||
#define KinEnaAxisGG 0x8000
|
||||
#define KinPosAxisGG C[15]
|
||||
#define KinVelAxisGG C[47]
|
||||
#define KinEnaAxisHH 0x10000
|
||||
#define KinPosAxisHH C[16]
|
||||
#define KinVelAxisHH C[48]
|
||||
#define KinEnaAxisLL 0x20000
|
||||
#define KinPosAxisLL C[17]
|
||||
#define KinVelAxisLL C[49]
|
||||
#define KinEnaAxisMM 0x40000
|
||||
#define KinPosAxisMM C[18]
|
||||
#define KinVelAxisMM C[50]
|
||||
#define KinEnaAxisNN 0x80000
|
||||
#define KinPosAxisNN C[19]
|
||||
#define KinVelAxisNN C[51]
|
||||
#define KinEnaAxisOO 0x100000
|
||||
#define KinPosAxisOO C[20]
|
||||
#define KinVelAxisOO C[52]
|
||||
#define KinEnaAxisPP 0x200000
|
||||
#define KinPosAxisPP C[21]
|
||||
#define KinVelAxisPP C[53]
|
||||
#define KinEnaAxisQQ 0x400000
|
||||
#define KinPosAxisQQ C[22]
|
||||
#define KinVelAxisQQ C[54]
|
||||
#define KinEnaAxisRR 0x800000
|
||||
#define KinPosAxisRR C[23]
|
||||
#define KinVelAxisRR C[55]
|
||||
#define KinEnaAxisSS 0x1000000
|
||||
#define KinPosAxisSS C[24]
|
||||
#define KinVelAxisSS C[56]
|
||||
#define KinEnaAxisTT 0x2000000
|
||||
#define KinPosAxisTT C[25]
|
||||
#define KinVelAxisTT C[57]
|
||||
#define KinEnaAxisUU 0x4000000
|
||||
#define KinPosAxisUU C[26]
|
||||
#define KinVelAxisUU C[58]
|
||||
#define KinEnaAxisVV 0x8000000
|
||||
#define KinPosAxisVV C[27]
|
||||
#define KinVelAxisVV C[59]
|
||||
#define KinEnaAxisWW 0x10000000
|
||||
#define KinPosAxisWW C[28]
|
||||
#define KinVelAxisWW C[60]
|
||||
#define KinEnaAxisXX 0x20000000
|
||||
#define KinPosAxisXX C[29]
|
||||
#define KinVelAxisXX C[61]
|
||||
#define KinEnaAxisYY 0x40000000
|
||||
#define KinPosAxisYY C[30]
|
||||
#define KinVelAxisYY C[62]
|
||||
#define KinEnaAxisZZ 0x80000000
|
||||
#define KinPosAxisZZ C[31]
|
||||
#define KinVelAxisZZ C[63]
|
||||
#define KinAxisUsed D[0]
|
||||
#define KinVelEna D[0]
|
||||
//----------------------------------------
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user