From: Walter Fetter Lages Date: Sun, 20 May 2018 06:08:22 +0000 (-0300) Subject: Change the Real-time in AIC to use the RTAI API. X-Git-Tag: v1.5.0^0 X-Git-Url: http://git.ece.ufrgs.br/?a=commitdiff_plain;h=a6e4097f85a395da47f7cf795077e318a5fd4356;p=aic.git Change the Real-time in AIC to use the RTAI API. Criado o programa EncoderRtRead para teste do encoder em tempo real utilizando as funcoes de ponto flutuante, ou seja, obtendo a leitura em radianos. Incluido suporte para ponto-flutuante. Implementadas em C as funcoes de interface que utilizam ponto flutuante. Criados os programas EncoderRtTest para teste do encoder em tempo real. Criado o programa MotorRtTest para teste do motor em tempo real, utilizando as funcoes de ponto flutante. Criado o programa PwmRtTest, para teste do PWM em tempo real em C. Incluidas no aicio.h consantes com a frequencia de referencia, frequencia de chaveamento, delay de turn-off e contagens maximas e minimas do PWM. Programas de tempo real modificados para utilizar a API do RTAI (Real Time Application Interface) para a TINI, ao inves de implementar diretamente todo o tratamento de tempo real. --- diff --git a/BrakeRtTest/BrakeRtTest.java b/BrakeRtTest/BrakeRtTest.java index 83373db..990f0ff 100644 --- a/BrakeRtTest/BrakeRtTest.java +++ b/BrakeRtTest/BrakeRtTest.java @@ -63,7 +63,7 @@ public class BrakeRtTest while(t <= tfms) { t=TINIOS.uptimeMillis()-t0; - System.out.print("t="+t+" ms"); + System.out.print("t="+t+"ms "); } System.out.println("...done."); diff --git a/BrakeRtTest/Makefile b/BrakeRtTest/Makefile index 08076f8..8244316 100644 --- a/BrakeRtTest/Makefile +++ b/BrakeRtTest/Makefile @@ -24,7 +24,7 @@ globals.mpp: globals.a51 ${ASMMACRO} ${ASMINC} -e- globals.a51 braketest.tlib: braketest.mpp globals.mpp ../lib/aicio.mpp.lib - cat braketest.mpp globals.mpp ../lib/aicio.mpp.lib > tmp.mpp + cat braketest.mpp globals.mpp ${TINIRTAIPATH}/lib/rtai.mpp.lib ../lib/aicio.mpp.lib > tmp.mpp echo end >> tmp.mpp ${ASM} ${ASMOPT} tmp.mpp mv tmp.tlib braketest.tlib diff --git a/BrakeRtTest/braketest.c b/BrakeRtTest/braketest.c index fdcdc2a..0531397 100644 --- a/BrakeRtTest/braketest.c +++ b/BrakeRtTest/braketest.c @@ -24,47 +24,38 @@ *******************************************************************************/ -#include +#include +#include + +#include #define BASE 0x800000 -void initialize(void) +extern volatile pthread_t th; + +void *braketest(void *arg) { - brake_initialize(BASE); + static int time=0; + + if(time) brake_apply(); else brake_release(); + time=~time; + + return NULL; } + /* public static native int startRealtime(int period) */ long Native_startRealtime(void) _JavaNative { - volatile long period = NatLib_LoadInt(0); + volatile long period=NatLib_LoadInt(0); + start_rt_timer(micro2count(period)); + pthread_create(&th,NULL,braketest,NULL); - _asm - - mov T2CON,#00h ; clear TR2 = disable timer 2 - anl IE,#0dfh ; clear ET2 = disable timer 2 interrupt - - mov dps,#1 - mov dptr,#braketest - mov a,#2bh ; timer 2 - call System_InstallInterrupt - mov dps,#00h - - anl CKCON,#0dfh ; clear T2M = timer 2 uses clock/12 - mov T2MOD,#00h ; clear DCEN = counts up only - -; TINI oscillator is 18432000 => counter=0c400h=100Hz - mov RCAP2L,#00h ; count reload LSB - mov TL2,#00h ; count LSB - mov RCAP2H,#0c4h ; count reload MSB - mov TH2,#0c4h ; count MSB - - orl IP,#20h ; set PT2 = set timer 2 interrupt to high priority - orl IE,#20h ; set ET2 = enable timer 2 interrupt + brake_initialize(BASE); - mov T2CON,#04h ; set TR2 = enable timer 2 + _asm + clr a _endasm; - - initialize(); return period; } @@ -72,27 +63,11 @@ long Native_startRealtime(void) _JavaNative /* public static native void stopRealtime() */ void Native_stopRealtime(void) _JavaNative { + pthread_cancel(th); + stop_rt_timer(); + _asm - mov T2CON,#00h ; clear TR2 = disable timer 2 - anl IE,#0dfh ; clear ET2 = disable timer 2 interrupt - anl IP,#0dfh ; clear PT2 = set timer 2 interrupt to normal priority - _endasm; -} - -static void int2ack(void) -{ - _asm - anl T2CON,#7fh ; clear TF2 = clears timer 2 overflow + clr a _endasm; } -static void braketest(void) interrupt -{ - static int time=0; - - int2ack(); - - if(time) brake_apply(); else brake_release(); - time=~time; -} - diff --git a/BrakeRtTest/globals.a51 b/BrakeRtTest/globals.a51 index ab84e84..6f467aa 100644 --- a/BrakeRtTest/globals.a51 +++ b/BrakeRtTest/globals.a51 @@ -26,4 +26,5 @@ braketest_time_1_1: db 0,0,0,0 +th: db 0,0,0,0 end diff --git a/Changes b/Changes index f17f430..76f0529 100644 --- a/Changes +++ b/Changes @@ -1,3 +1,29 @@ +2005.09.02 Criado o programa EncoderRtRead para teste do encoder em + tempo real utilizando as funcoes de ponto flutuante, ou + seja, obtendo a leitura em radianos. + +2005.08.31 Incluido suporte para ponto-flutuante. Implementadas em C as + funcoes de interface que utilizam ponto flutuante. + + Criados os programas EncoderRtTest para teste do encoder em + tempo real. + + Criado o programa MotorRtTest para teste do motor em tempo + real, utilizando as funcoes de ponto flutante. + +2005.08.30 Criado o programa PwmRtTest, para teste do PWM em tempo real + em C. + + Incluidas no aicio.h consantes com a frequencia de + referencia, frequencia de chaveamento, delay de turn-off e + contagens maximas e minimas do PWM. + +2005.08.13 Programas de tempo real modificados para utilizar a API do + RTAI (Real Time Application Interface) para a TINI, ao inves + de implementar diretamente todo o tratamento de tempo real. + + Criada versao 1.5.0 + 2005.08.12 Criado o programa DoorDaemon, para implementar a abertura de uma porta conectada no sinal de freio da AIC atraves da Internet, utilizando um protocolo CHAP com sha-1. diff --git a/CmdDaemon/CmdDaemon.java b/CmdDaemon/CmdDaemon.java deleted file mode 100644 index 75ef6bc..0000000 --- a/CmdDaemon/CmdDaemon.java +++ /dev/null @@ -1,192 +0,0 @@ -/****************************************************************************** - - Actuator Interface Card - Command Daemon - Copyright (C) 2005 Walter Fetter Lages - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - - You can also obtain a copy of the GNU General Public License - at . - -*******************************************************************************/ - -import br.ufrgs.eletro.AIC.*; -import com.dalsemi.comm.*; -import com.dalsemi.system.*; -import java.io.*; -import java.lang.*; - -class CmdDaemon extends Thread -{ - static final int BASE=0x800000; - - static AIC aic; - static Host host; - static boolean debug=false; - static boolean udp=false; - static boolean help=false; - - public static void main(String[] args) throws IOException,CanBusException - { - int i; - int hostarg=0; - - System.out.println("AIC Command Daemon"); - System.out.println("Copyright (C) 2005 Walter Fetter Lages .\n"); - - switch(args.length) - { - case 0: - { - help=true; - break; - } - case 1: - { - if(args[0].equals("-debug") || - args[0].equals("-UDP")) help=true; - break; - } - case 2: - { - hostarg=1; - if(args[0].equals("-debug")) debug=true; - else help=true; - break; - } - case 3: - { - hostarg=1; - if(args[0].equals("-UDP")) udp=true; - else help=true; - break; - } - case 4: - { - hostarg=2; - if(args[0].equals("-debug")) debug=true; - else help=true; - if(args[1].equals("-UDP")) udp=true; - else help=true; - break; - } - default: - { - help=true; - } - } - - - if(help) - { - System.out.println("Usage:\tjava CmdDaemon.tini [-debug] aic_id"); - System.out.println("\tor"); - System.out.println("\tjava CmdDaemon.tini [-debug] -UDP controller_host status_port"); - System.exit(1); - } - try - { - aic=new AIC(BASE,24.0,20e3,2000); - if(udp) host=new HostUDP(args[hostarg],Integer.parseInt(args[hostarg+1])); - else host=new HostCAN(Integer.parseInt(args[hostarg])); - - GetCommand getCommand=new GetCommand(); - getCommand.setPriority(Thread.MAX_PRIORITY); - getCommand.start(); - - if(debug) System.out.println("getCommand priority="+getCommand.getPriority()); - } - catch (NumberFormatException nfe) - { - System.out.println("Usage: java CmdDaemon.tini [-debug] [-UDP controller_host port] | aicid"); - System.exit(1); - } - } -} - - -class GetCommand extends CmdDaemon -{ - public void run() - { - try - { - aic.on(); - aic.brake.release(); - - for(int k=0;;k++) - { - if(debug) System.out.print("k="+k+"\t"); - - int cmd=host.command(); - - if(debug) System.out.print("Command="+ cmd +"\t"); - - switch(cmd) - { - case Host.MOTOR_ON: - { - if(debug) System.out.print("Motor on\t"); - aic.motor.on(); - break; - } - case Host.MOTOR_OFF: - { - if(debug) System.out.print("Motor off\t"); - aic.motor.off(); - break; - } - case Host.MOTOR_ACT: - { - if(debug) System.out.print("Voltage="+host.voltage()+"\t"); - aic.motor.set(host.voltage()); - break; - } - case Host.BRAKE_RELEASE: - { - if(debug) System.out.print("Brake release\t"); - aic.brake.release(); - break; - } - case Host.BRAKE_APPLY: - { - if(debug) System.out.print("Brake apply\t"); - aic.brake.apply(); - break; - } -/* case Host.RESET: - { - if(debug) System.out.print("Reset\t"); - aic.reset(); - break; - } -*/ - default: - { - if(debug) System.out.print("Invalid\t"); - break; - } - - } - if(debug) System.out.println(""); - } - } - catch (Exception all) - { - all.printStackTrace(); - } - } -} diff --git a/CmdDaemon/Makefile b/CmdDaemon/Makefile deleted file mode 100644 index c344464..0000000 --- a/CmdDaemon/Makefile +++ /dev/null @@ -1,33 +0,0 @@ -CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar:/opt/tini/bin/modules.jar:../lib/AIC.jar -APIDBPATH=-d /opt/tini/bin/tini.db -DEPPATH=-p /opt/tini/bin/modules.jar -DEPFILE=-x /opt/tini/bin/owapi_dep.txt -DEPNAME=-add CAN -LIBPATH=-path ../lib/AIC.jar -NATLIB=-n brake.tlib -n encoder.tlib -n index.tlib -n pwm.tlib -JAVAFLAGS=-O -target 1.1 -source 1.3 -TARGET= aic1 aic2 aic3 aic4 aic5 - -all: CmdDaemon.tini - -CmdDaemon.class: CmdDaemon.java ../lib/AIC.jar - javac ${JAVAFLAGS} ${CLASSPATH} CmdDaemon.java - -CmdDaemon.tini: CmdDaemon.class ../lib/AIC.jar - java ${CLASSPATH} BuildDependency ${DEPNAME} ${DEPFILE} ${DEPPATH} -f CmdDaemon.class ${APIDBPATH} -o CmdDaemon.tini ${LIBPATH} ${NATLIB} - -clean: - rm -f *.bak *~ *.class UPLOADED - -distclean: clean - rm -f CmdDaemon.tini - -upload: UPLOADED - -UPLOADED: CmdDaemon.tini - for i in ${TARGET}; do \ - ftp $$i < upload.ftp; \ - echo $$i uploaded; \ - done - - diff --git a/PIDDaemon/PIDDaemon.java b/EncoderRtRead/EncoderRtRead.java similarity index 67% rename from PIDDaemon/PIDDaemon.java rename to EncoderRtRead/EncoderRtRead.java index d669fcb..7a87673 100644 --- a/PIDDaemon/PIDDaemon.java +++ b/EncoderRtRead/EncoderRtRead.java @@ -1,9 +1,8 @@ /****************************************************************************** Actuator Interface Card - PID Daemon - User Interface Module - Copyright (C) 2004, 2005 Walter Fetter Lages + Encoder Real Time Test + Copyright (C) 2005 Walter Fetter Lages This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -25,19 +24,38 @@ *******************************************************************************/ import com.dalsemi.system.*; +import java.lang.Math.*; -public class PIDDaemon +class EncoderRtRead { public static native int startRealtime(int period); public static native void stopRealtime(); - public static native void setReference(int reference); - public static native int getOutput(); + public static native float encoderGet(); + static final int ST=100; + private static void delay(int ms) + { + long time=TINIOS.uptimeMillis(); + while(TINIOS.uptimeMillis() < time+ms); + } + + static void iter(long t) + { + long timeread=TINIOS.uptimeMillis(); + float disp=encoderGet(); + System.out.print("t="+t+" ms"); + System.out.println("\tEncoder read: " + disp); + delay(ST); + } + public static void main(String[] args) - { - System.out.println("PID Daemon"); + { + int i; + boolean run=true; + + System.out.println("AIC Encoder Real Time Test Program"); System.out.println("Copyright (C) Walter Fetter Lages, 2005.\n"); - + int tfms=0; if(args.length==1) { @@ -45,34 +63,34 @@ public class PIDDaemon } else { - System.out.println("Usage:\tPIDDaemon ms"); + System.out.println("Usage:\tEncoderRtRead ms"); System.exit(1); } System.out.print("Loading native library..."); - System.loadLibrary("pid.tlib"); + System.loadLibrary("encoderread.tlib"); System.out.println("done."); int us=10000; System.out.print("Starting real-time timer with "+us+" us period..."); int period=startRealtime(us); - System.out.println("done. Effective period: "+period+" us."); + System.out.println("done."); + System.out.println("Effective period: "+period+" us."); - System.out.print("Running PID for "+tfms+" ms..."); + System.out.println("Running EncoderRtRead for "+tfms+" ms..."); + long t0=TINIOS.uptimeMillis(); long t=0; while(t <= tfms) { t=TINIOS.uptimeMillis()-t0; - System.out.print("t="+t+" ms"); - int ref=(int)t/2; - setReference(ref); - System.out.println("\treference="+ref+"\toutput="+getOutput()); + iter(t); } System.out.println("...done."); System.out.print("Stoping real-time timer..."); stopRealtime(); System.out.println("done."); + } } diff --git a/EncoderRtRead/Makefile b/EncoderRtRead/Makefile new file mode 100644 index 0000000..fad82b1 --- /dev/null +++ b/EncoderRtRead/Makefile @@ -0,0 +1,43 @@ +CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar +APIDBPATH=-d /opt/tini/bin/tini.db +TRTAIPATH=${HOME}/src/tini/rtai +TMATHPATH=${HOME}/src/tini/tmath +LIBPATH=-PATH ${TINIRTAIPATH}/lib/rtai.jar +JAVAFLAGS=-O -target 1.1 -source 1.3 +ASMINC=-I/opt/tini/native/lib +ASMOPT=-f 1.15 -p 390 -l +ASMMACRO=/opt/tini/native/bin/Linux/macro +ASM=/opt/tini/native/bin/Linux/a390 +CC=sdcc +CFLAGS=-mTININative -S --use-accelerator +CINCLUDE=-I${TRTAIPATH}/include -I${TMATHPATH}/include -I../include +CMPFLAGS=${CFLAGS} ${CINCLUDE} + +all: EncoderRtRead.tini + +EncoderRtRead.class: EncoderRtRead.java ../lib/AIC.jar + javac ${JAVAFLAGS} ${CLASSPATH} EncoderRtRead.java + +encoderread.mpp: encoderread.a51 + ${ASMMACRO} ${ASMINC} -e- encoderread.a51 + +encoderread.tlib: encoderread.mpp ../lib/aicio.mpp.lib + cat encoderread.mpp ${TRTAIPATH}/lib/rtai.mpp.lib ../lib/aicio.mpp.lib > tmp.mpp + echo end >> tmp.mpp + ${ASM} ${ASMOPT} tmp.mpp + mv tmp.tlib encoderread.tlib + mv tmp.lst encoderread.lst + +encoderread.a51: encoderread.c + ${CC} ${CMPFLAGS} encoderread.c + +EncoderRtRead.tini: EncoderRtRead.class encoderread.tlib + java ${CLASSPATH} TINIConvertor -n encoderread.tlib -f EncoderRtRead.class ${APIDBPATH} -o EncoderRtRead.tini + + +clean: + rm -f *.bak *~ EncoderRtRead.class encoderread.tlib encoderread.mpp encoderread.lst encoderread.a51 globals.mpp tmp.mpp tmp.lst + +distclean: clean + rm -f EncoderRtRead.tini + diff --git a/trtai/PID/pid.c b/EncoderRtRead/encoderread.c similarity index 60% rename from trtai/PID/pid.c rename to EncoderRtRead/encoderread.c index d5fd00e..e68e268 100644 --- a/trtai/PID/pid.c +++ b/EncoderRtRead/encoderread.c @@ -1,8 +1,8 @@ /****************************************************************************** Actuator Interface Card - PID - Real Time Module + Encoder Real Time Test + Real Time Module Copyright (C) 2005 Walter Fetter Lages This program is free software; you can redistribute it and/or modify @@ -25,47 +25,63 @@ *******************************************************************************/ #include - #include -extern volatile long *output; -extern volatile pthread_t th; -extern volatile long *reference; +#include + +#define BASE 0x800000 + +volatile pthread_t th; +volatile float disp; -/* By now this is not actually a thread as it should be, - but a function that is called at each timer interrupt */ -void *count(void *arg) +void encodertest_globals(void) _naked { -// volatile long *output=(volatile long *)arg; -// for(;;) -// { -// rt_wait_period(); - *output=*reference; -// } + _asm +th: db 0,0,0,0 +disp: db 0,0,0,0 + _endasm; +} + +void *encoderread(void *arg) +{ + disp=encoder_read(); + return NULL; } -RTIME Native_startRealtime(void) _JavaNative +/* public static native int startRealtime(int period) */ +long Native_startRealtime(void) _JavaNative { - volatile long period = NatLib_LoadInt(0); - RTIME count=micro2count(period); - start_rt_timer(count); - pthread_create(&th,NULL,count,output); - return count2micro(count); + volatile long period=NatLib_LoadInt(0); + start_rt_timer(micro2count(period)); + pthread_create(&th,NULL,encoderread,NULL); + + encoder_initialize(BASE,2048); + + brake_release(); + + _asm + clr a + _endasm; + + return period; } +/* public static native void stopRealtime() */ void Native_stopRealtime(void) _JavaNative { + brake_apply(); + pthread_cancel(th); stop_rt_timer(); + + _asm + clr a + _endasm; } - -long Native_getOutput(void) _JavaNative +/* public static native float encoderGet() */ +float Native_encoderGet(void) _JavaNative { - return output; + return disp; } -void Native_setReference(void) _JavaNative -{ - *reference = NatLib_LoadInt(0); -} diff --git a/trtai/PID/PID.java b/EncoderRtTest/EncoderRtTest.java similarity index 69% rename from trtai/PID/PID.java rename to EncoderRtTest/EncoderRtTest.java index a0f5c33..ed3a5d0 100644 --- a/trtai/PID/PID.java +++ b/EncoderRtTest/EncoderRtTest.java @@ -1,8 +1,7 @@ /****************************************************************************** Actuator Interface Card - PID - User Interface Module + Encoder Real Time Test Copyright (C) 2005 Walter Fetter Lages This program is free software; you can redistribute it and/or modify @@ -25,19 +24,38 @@ *******************************************************************************/ import com.dalsemi.system.*; +import java.lang.Math.*; -public class PID +class EncoderRtTest { public static native int startRealtime(int period); public static native void stopRealtime(); - public static native void setReference(int reference); - public static native int getOutput(); + public static native int encoderGet(); + static final int ST=100; + private static void delay(int ms) + { + long time=TINIOS.uptimeMillis(); + while(TINIOS.uptimeMillis() < time+ms); + } + + static void iter(long t) + { + long timeread=TINIOS.uptimeMillis(); + int count=encoderGet(); + System.out.print("t="+t+" ms"); + System.out.println("\tEncoder count: " + count); + delay(ST); + } + public static void main(String[] args) - { - System.out.println("PID"); + { + int i; + boolean run=true; + + System.out.println("AIC Encoder Real Time Test Program"); System.out.println("Copyright (C) Walter Fetter Lages, 2005.\n"); - + int tfms=0; if(args.length==1) { @@ -45,34 +63,34 @@ public class PID } else { - System.out.println("Usage:\tPID ms"); + System.out.println("Usage:\tEncoderRtTest ms"); System.exit(1); } System.out.print("Loading native library..."); - System.loadLibrary("pid.tlib"); + System.loadLibrary("encodertest.tlib"); System.out.println("done."); int us=10000; System.out.print("Starting real-time timer with "+us+" us period..."); int period=startRealtime(us); - System.out.println("done. Effective period: "+period+" us."); + System.out.println("done."); + System.out.println("Effective period: "+period+" us."); - System.out.print("Running PID for "+tfms+" ms..."); + System.out.println("Running EncoderRtTest for "+tfms+" ms..."); + long t0=TINIOS.uptimeMillis(); long t=0; while(t <= tfms) { t=TINIOS.uptimeMillis()-t0; - System.out.print("t="+t+" ms"); - int ref=(int)t/2; - setReference(ref); - System.out.println("\treference="+ref+"\toutput="+getOutput()); + iter(t); } System.out.println("...done."); System.out.print("Stoping real-time timer..."); stopRealtime(); System.out.println("done."); + } } diff --git a/EncoderRtTest/Makefile b/EncoderRtTest/Makefile new file mode 100644 index 0000000..303f317 --- /dev/null +++ b/EncoderRtTest/Makefile @@ -0,0 +1,43 @@ +CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar +APIDBPATH=-d /opt/tini/bin/tini.db +TRTAIPATH=${HOME}/src/tini/rtai +TMATHPATH=${HOME}/src/tini/tmath +LIBPATH=-PATH ${TINIRTAIPATH}/lib/rtai.jar +JAVAFLAGS=-O -target 1.1 -source 1.3 +ASMINC=-I/opt/tini/native/lib +ASMOPT=-f 1.15 -p 390 -l +ASMMACRO=/opt/tini/native/bin/Linux/macro +ASM=/opt/tini/native/bin/Linux/a390 +CC=sdcc +CFLAGS=-mTININative -S --use-accelerator +CINCLUDE=-I${TRTAIPATH}/include -I${TMATHPATH}/include -I../include +CMPFLAGS=${CFLAGS} ${CINCLUDE} + +all: EncoderRtTest.tini + +EncoderRtTest.class: EncoderRtTest.java ../lib/AIC.jar + javac ${JAVAFLAGS} ${CLASSPATH} EncoderRtTest.java + +encodertest.mpp: encodertest.a51 + ${ASMMACRO} ${ASMINC} -e- encodertest.a51 + +encodertest.tlib: encodertest.mpp ../lib/aicio.mpp.lib + cat encodertest.mpp ${TRTAIPATH}/lib/rtai.mpp.lib ../lib/aicio.mpp.lib > tmp.mpp + echo end >> tmp.mpp + ${ASM} ${ASMOPT} tmp.mpp + mv tmp.tlib encodertest.tlib + mv tmp.lst encodertest.lst + +encodertest.a51: encodertest.c + ${CC} ${CMPFLAGS} encodertest.c + +EncoderRtTest.tini: EncoderRtTest.class encodertest.tlib + java ${CLASSPATH} TINIConvertor -n encodertest.tlib -f EncoderRtTest.class ${APIDBPATH} -o EncoderRtTest.tini + + +clean: + rm -f *.bak *~ EncoderRtTest.class encodertest.tlib encodertest.mpp encodertest.lst encodertest.a51 globals.mpp tmp.mpp tmp.lst + +distclean: clean + rm -f EncoderRtTest.tini + diff --git a/PIDDaemon/pid.c b/EncoderRtTest/encodertest.c similarity index 56% rename from PIDDaemon/pid.c rename to EncoderRtTest/encodertest.c index 2895194..683e466 100644 --- a/PIDDaemon/pid.c +++ b/EncoderRtTest/encodertest.c @@ -1,9 +1,9 @@ /****************************************************************************** Actuator Interface Card - PID Daemon - Real Time Module - Copyright (C) 2004, 2005 Walter Fetter Lages + Encoder Real Time Test + Real Time Module + Copyright (C) 2005 Walter Fetter Lages This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -25,47 +25,63 @@ *******************************************************************************/ #include - #include -extern volatile long *output; -extern volatile pthread_t th; -extern volatile long *reference; +#include + +#define BASE 0x800000 + +volatile pthread_t th; +volatile long count; -/* By now this is not actually a thread as it should be, - but a function that is called at each timer interrupt */ -void *count(void *arg) +void encodertest_globals(void) _naked { -// volatile long *output=(volatile long *)arg; -// for(;;) -// { -// rt_wait_period(); - *output=*reference; -// } + _asm +th: db 0,0,0,0 +count: db 0,0,0,0 + _endasm; +} + +void *encodertest(void *arg) +{ + count=encoder_get_count(); + return NULL; } -RTIME Native_startRealtime(void) _JavaNative +/* public static native int startRealtime(int period) */ +long Native_startRealtime(void) _JavaNative { - volatile long period = NatLib_LoadInt(0); - RTIME count=micro2count(period); - start_rt_timer(count); - pthread_create(&th,NULL,count,output); - return count2micro(count); + volatile long period=NatLib_LoadInt(0); + start_rt_timer(micro2count(period)); + pthread_create(&th,NULL,encodertest,NULL); + + encoder_initialize(BASE,2000); + + brake_release(); + + _asm + clr a + _endasm; + + return period; } +/* public static native void stopRealtime() */ void Native_stopRealtime(void) _JavaNative { + brake_apply(); + pthread_cancel(th); stop_rt_timer(); + + _asm + clr a + _endasm; } - -long Native_getOutput(void) _JavaNative +/* public static native int encoderGet() */ +long Native_encoderGet(void) _JavaNative { - return output; + return count; } -void Native_setReference(void) _JavaNative -{ - *reference = NatLib_LoadInt(0); -} diff --git a/IndexRtTest/Makefile b/IndexRtTest/Makefile index 31c4b17..46ea9c0 100644 --- a/IndexRtTest/Makefile +++ b/IndexRtTest/Makefile @@ -24,7 +24,7 @@ globals.mpp: globals.a51 ${ASMMACRO} ${ASMINC} -e- globals.a51 indextest.tlib: indextest.mpp globals.mpp ../lib/aicio.mpp.lib - cat indextest.mpp globals.mpp ../lib/aicio.mpp.lib > tmp.mpp + cat indextest.mpp globals.mpp ${TINIRTAIPATH}/lib/rtai.mpp.lib ../lib/aicio.mpp.lib > tmp.mpp echo end >> tmp.mpp ${ASM} ${ASMOPT} tmp.mpp mv tmp.tlib indextest.tlib diff --git a/IndexRtTest/globals.a51 b/IndexRtTest/globals.a51 index acdc2d9..e8fcbd4 100644 --- a/IndexRtTest/globals.a51 +++ b/IndexRtTest/globals.a51 @@ -26,4 +26,5 @@ index: db 0,0,0,0 +th: db 0,0,0,0 end diff --git a/IndexRtTest/indextest.c b/IndexRtTest/indextest.c index 8b7c916..a0a2e1e 100644 --- a/IndexRtTest/indextest.c +++ b/IndexRtTest/indextest.c @@ -23,50 +23,34 @@ at . *******************************************************************************/ +#include +#include #include #define BASE 0x800000 +extern volatile pthread_t th; extern volatile long index; -void initialize(void) +void *indextest(void *arg) { - index_initialize(BASE); + index=index_read(); + return NULL; } /* public static native int startRealtime(int period) */ long Native_startRealtime(void) _JavaNative { - volatile long period = NatLib_LoadInt(0); + volatile long period=NatLib_LoadInt(0); + start_rt_timer(micro2count(period)); + pthread_create(&th,NULL,indextest,NULL); - _asm - - mov T2CON,#00h ; clear TR2 = disable timer 2 - anl IE,#0dfh ; clear ET2 = disable timer 2 interrupt - - mov dps,#1 - mov dptr,#indextest - mov a,#2bh ; timer 2 - call System_InstallInterrupt - mov dps,#00h - - anl CKCON,#0dfh ; clear T2M = timer 2 uses clock/12 - mov T2MOD,#00h ; clear DCEN = counts up only - -; TINI oscillator is 18432000 => counter=0c400h=100Hz - mov RCAP2L,#00h ; count reload LSB - mov TL2,#00h ; count LSB - mov RCAP2H,#0c4h ; count reload MSB - mov TH2,#0c4h ; count MSB - - orl IP,#20h ; set PT2 = set timer 2 interrupt to high priority - orl IE,#20h ; set ET2 = enable timer 2 interrupt + index_initialize(BASE); - mov T2CON,#04h ; set TR2 = enable timer 2 + _asm + clr a _endasm; - - initialize(); return period; } @@ -74,27 +58,14 @@ long Native_startRealtime(void) _JavaNative /* public static native void stopRealtime() */ void Native_stopRealtime(void) _JavaNative { + pthread_cancel(th); + stop_rt_timer(); + _asm - mov T2CON,#00h ; clear TR2 = disable timer 2 - anl IE,#0dfh ; clear ET2 = disable timer 2 interrupt - anl IP,#0dfh ; clear PT2 = set timer 2 interrupt to normal priority - _endasm; -} - -static void int2ack(void) -{ - _asm - anl T2CON,#7fh ; clear TF2 = clears timer 2 overflow + clr a _endasm; } -static void indextest(void) interrupt -{ - int2ack(); - - index=index_read(); -} - /* public static native boolean getIndex() */ long Native_getIndex(void) _JavaNative { diff --git a/Makefile b/Makefile index e4fb3ac..60dcde9 100644 --- a/Makefile +++ b/Makefile @@ -2,17 +2,22 @@ PACKAGES= doc \ lib \ AICOff \ BrakeTest \ + BrakeRtTest \ CANTest \ EncoderTest \ + EncoderRtTest \ + EncoderRtRead \ IndexTest \ + IndexRtTest \ IODaemon \ JointTest \ MotorReset \ MotorTest \ + MotorRtTest \ + PwmRtTest \ PID \ StrobeTest \ DoorDaemon \ - CmdDaemon \ aicd \ braketst \ encodertst \ diff --git a/MotorRtTest/Makefile b/MotorRtTest/Makefile new file mode 100644 index 0000000..fd287a3 --- /dev/null +++ b/MotorRtTest/Makefile @@ -0,0 +1,44 @@ +CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar +APIDBPATH=-d /opt/tini/bin/tini.db +TRTAIPATH=${HOME}/src/tini/rtai +TMATHPATH=${HOME}/src/tini/tmath +LIBPATH=-PATH ${TRTAIPATH}/lib/rtai.jar +JAVAFLAGS=-O -target 1.1 -source 1.3 + +ASMINC=-I/opt/tini/native/lib +ASMOPT=-f 1.15 -p 390 -l +ASMMACRO=/opt/tini/native/bin/Linux/macro +ASM=/opt/tini/native/bin/Linux/a390 +CC=sdcc +CFLAGS=-mTININative -S --use-accelerator +CINCLUDE=-I${TRTAIPATH}/include -I${TMATHPATH}/include -I../include +CMPFLAGS=${CFLAGS} ${CINCLUDE} + +all: MotorRtTest.tini + +MotorRtTest.class: MotorRtTest.java ../lib/AIC.jar + javac ${JAVAFLAGS} ${CLASSPATH} MotorRtTest.java + +motortest.mpp: motortest.a51 + ${ASMMACRO} ${ASMINC} -e- motortest.a51 + +motortest.tlib: motortest.mpp ../lib/aicio.mpp.lib + cat motortest.mpp ${TRTAIPATH}/lib/rtai.mpp.lib ../lib/aicio.mpp.lib > tmp.mpp + echo end >> tmp.mpp + ${ASM} ${ASMOPT} tmp.mpp + mv tmp.tlib motortest.tlib + mv tmp.lst motortest.lst + +motortest.a51: motortest.c + ${CC} ${CMPFLAGS} motortest.c + +MotorRtTest.tini: MotorRtTest.class motortest.tlib + java ${CLASSPATH} TINIConvertor -n motortest.tlib -f MotorRtTest.class ${APIDBPATH} -o MotorRtTest.tini + + +clean: + rm -f *.bak *~ MotorRtTest.class motortest.tlib motortest.mpp motortest.lst motortest.a51 globals.mpp tmp.mpp tmp.lst + +distclean: clean + rm -f MotorRtTest.tini + diff --git a/MotorRtTest/MotorRtTest.java b/MotorRtTest/MotorRtTest.java new file mode 100644 index 0000000..f44596e --- /dev/null +++ b/MotorRtTest/MotorRtTest.java @@ -0,0 +1,97 @@ +/****************************************************************************** + + Actuator Interface Card + Motor Real Time Test + User Interface Module + Copyright (C) 2005 Walter Fetter Lages + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +import com.dalsemi.system.*; +import java.lang.Math.*; + +class MotorRtTest +{ + public static native int startRealtime(int period); + public static native void stopRealtime(); + public static native void motorSet(float voltage); + static final int ST=100; + + private static void delay(int ms) + { + long time=TINIOS.uptimeMillis(); + while(TINIOS.uptimeMillis() < time+ms); + } + + static void iter(int i) + { + motorSet(i); + System.out.println("Motor Voltage: " + i); + delay(ST); + } + + public static void main(String[] args) + { + int i; + boolean run=true; + + System.out.println("AIC Motor Real Time Test Program"); + System.out.println("Copyright (C) Walter Fetter Lages, 2005.\n"); + + int tfms=0; + if(args.length==1) + { + tfms=Integer.parseInt(args[0]); + } + else + { + System.out.println("Usage:\tMotorRtTest ms"); + System.exit(1); + } + + System.out.print("Loading native library..."); + System.loadLibrary("motortest.tlib"); + System.out.println("done."); + + int us=10000; + System.out.print("Starting real-time timer with "+us+" us period..."); + int period=startRealtime(us); + System.out.println("done."); + System.out.println("Effective period: "+period+" us."); + + System.out.println("Running MotorRtTest for "+tfms+" ms..."); + + long t0=TINIOS.uptimeMillis(); + long t=0; + while(t <= tfms) + { + t=TINIOS.uptimeMillis()-t0; + System.out.print("t="+t+" ms"); + for(i=0;(i <= 24) && run;i++) iter(i); + for(i=24;(i >= -24) && run;i--) iter(i); + for(i=-24;(i <= 0) && run;i++) iter(i); + } + System.out.println("...done."); + + System.out.print("Stoping real-time timer..."); + stopRealtime(); + System.out.println("done."); + } +} diff --git a/trtai/PID/globals.a51 b/MotorRtTest/globals.a51 similarity index 95% rename from trtai/PID/globals.a51 rename to MotorRtTest/globals.a51 index bf5b08d..bb77d90 100644 --- a/trtai/PID/globals.a51 +++ b/MotorRtTest/globals.a51 @@ -1,7 +1,7 @@ ;****************************************************************************** ; ; Actuator Interface Card -; PID +; Motor Real time Test ; Real Time Module Global Variables ; Copyright (C) 2005 Walter Fetter Lages ; @@ -25,7 +25,6 @@ ;*******************************************************************************/ -output: db 0,0,0,0 th: db 0,0 -reference: db 0,0,0,0 +voltage: db 0,0,0,0 end diff --git a/MotorRtTest/motortest.c b/MotorRtTest/motortest.c new file mode 100644 index 0000000..2e0eb81 --- /dev/null +++ b/MotorRtTest/motortest.c @@ -0,0 +1,117 @@ +/****************************************************************************** + + Actuator Interface Card + Motor Real Time Test + Real Time Module + Copyright (C) 2005 Walter Fetter Lages + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ +#include +#include + +#include + +#define BASE 0x800000 + +volatile pthread_t th; +volatile float voltage; + +void motortest_globals(void) _naked +{ + _asm +th: db 0,0,0,0 +voltage: db 0,0,0,0 + _endasm; +} + +void *motortest(void *arg) +{ +// motor_set(voltage); + return NULL; +} + +/* public static native int startRealtime(int period) */ +long Native_startRealtime(void) _JavaNative +{ + volatile long period; + + aic_initialize(BASE,24.0,20e3,2000); + + period=NatLib_LoadInt(0); + start_rt_timer(micro2count(period)); + pthread_create(&th,NULL,motortest,NULL); + + aic_on(); + brake_release(); + + _asm + clr a + _endasm; + + return period; +} + +/* public static native void stopRealtime() */ +void Native_stopRealtime(void) _JavaNative +{ + brake_apply(); + aic_off(); + + pthread_cancel(th); + stop_rt_timer(); + + aic_finalize(); + + _asm + clr a + _endasm; +} + +/* public static native void motorSet(float voltage) */ +void Native_motorSet(void) _JavaNative +{ + /* + volatile long v =NatLib_LoadInt(0); + voltage=*((float *) &v); + */ + + _asm + clr a + lcall Natlib_LoadPrimitive ; get parameter in R3:R2:R1:R0 + mov dptr,#voltage + mov a,r0 + movx @dptr,a + inc dptr + mov a,r1 + movx @dptr,a + inc dptr + mov a,r2 + movx @dptr,a + inc dptr + mov a,r3 + movx @dptr,a + _endasm; + + motor_set(voltage); + + _asm + clr a + _endasm; +} diff --git a/PID/Makefile b/PID/Makefile index 3d26167..97c4442 100644 --- a/PID/Makefile +++ b/PID/Makefile @@ -1,7 +1,8 @@ CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar APIDBPATH=-d /opt/tini/bin/tini.db -TINIRTAIPATH=${HOME}/src/tini/rtai -LIBPATH=-PATH ${TINIRTAIPATH}/lib/rtai.jar +TRTAIPATH=${HOME}/src/tini/rtai +TMATHPATH=${HOME}/src/tini/tmath +LIBPATH=-PATH ${TRTAIPATH}/lib/rtai.jar JAVAFLAGS=-O -target 1.1 -source 1.3 ASMINC=-I/opt/tini/native/lib ASMOPT=-f 1.15 -p 390 -l @@ -9,7 +10,7 @@ ASMMACRO=/opt/tini/native/bin/Linux/macro ASM=/opt/tini/native/bin/Linux/a390 CC=sdcc CFLAGS=-mTININative -S --use-accelerator -CINCLUDE=-I${TINIRTAIPATH}/include -I../include +CINCLUDE=-I${TRTAIPATH}/include -I${TMATHPATH}/include -I../include CMPFLAGS=${CFLAGS} ${CINCLUDE} all: PID.tini @@ -20,11 +21,9 @@ PID.class: PID.java pid.mpp: pid.a51 ${ASMMACRO} ${ASMINC} -e- pid.a51 -globals.mpp: globals.a51 - ${ASMMACRO} ${ASMINC} globals.a51 - -pid.tlib: pid.mpp globals.mpp ../lib/aicio.mpp.lib - cat pid.mpp ../lib/aicio.mpp.lib globals.mpp > tmp.mpp +pid.tlib: pid.mpp ${TRTAIPATH}/lib/rtai.mpp.lib ../lib/aicio.mpp.lib + cat pid.mpp ${TRTAIPATH}/lib/rtai.mpp.lib ../lib/aicio.mpp.lib > tmp.mpp + echo end >> tmp.mpp ${ASM} ${ASMOPT} tmp.mpp mv tmp.tlib pid.tlib mv tmp.lst pid.lst diff --git a/PID/PID.java b/PID/PID.java index 4a78bc5..3357d33 100644 --- a/PID/PID.java +++ b/PID/PID.java @@ -56,20 +56,18 @@ public class PID int us=10000; System.out.print("Starting real-time timer with "+us+" us period..."); int period=startRealtime(us); - System.out.println("done."); - System.out.println("Effective period: "+period+" us."); + System.out.println("done. Effective period: "+period+" us."); System.out.println("Running PID for "+tfms+" ms..."); long t0=TINIOS.uptimeMillis(); long t=0; - while(t <= tfms || true) + while(t <= tfms) { t=TINIOS.uptimeMillis()-t0; System.out.print("t="+t+" ms"); - float ref=((float)t)/2; -// setReference(ref); - System.out.print("\treference="+ref); - System.out.println("\toutput="+getOutput()); + int ref=(int)t/2; + setReference(ref); + System.out.println("\treference="+ref+"\toutput="+getOutput()); } System.out.println("...done."); diff --git a/PID/globals.a51 b/PID/globals.a51 index 4bae99a..bf5b08d 100644 --- a/PID/globals.a51 +++ b/PID/globals.a51 @@ -26,7 +26,6 @@ output: db 0,0,0,0 -;th: db 0,0 +th: db 0,0 reference: db 0,0,0,0 -controller_time_1_1: db 0,0,0,0 end diff --git a/PID/pid.c b/PID/pid.c index f2ee2ef..6ce6cfa 100644 --- a/PID/pid.c +++ b/PID/pid.c @@ -1,7 +1,7 @@ /****************************************************************************** Actuator Interface Card - PID + PID Real Time Module Copyright (C) 2005 Walter Fetter Lages @@ -24,102 +24,129 @@ *******************************************************************************/ -#include +#include +#include + +#include + +#include #define BASE 0x800000 -extern volatile float output; -extern volatile float reference; +#define Kp 20.0 +#define Ki 5.0 +#define Kd 10.0 + +#define MIN_U -24.0 +#define MAX_U 24.0 + +static pthread_t th; +static float u; +static float y; +static float ref; +static float e1; +static float e2; + +static void pid_globals(void) _naked +{ + _asm +th: db 0,0,0,0 +u: db 0,0,0,0 +y: db 0,0,0,0 +ref: db 0,0,0,0 +e1: db 0,0,0,0 +e2: db 0,0,0,0 + _endasm; +} + -void initialize(void) +/* By now this is not actually a thread as it should be, + but a function that is called at each timer interrupt */ +void *pid(void *arg) { - brake_initialize(BASE); + float e; + +// volatile long *y=(volatile long *)arg; +// for(;;) +// { +// rt_wait_period(); + + y+=encoder_read(); + + e=ref-y; + + u+=Kp*(e-e1)+Ki*e+Kd*(e-2*e1+e2); + if(u < MIN_U) u=MIN_U; + if(u > MAX_U) u=MAX_U; + + motor_set(u); + + e2=e1; + e1=e; +// } + return NULL; } /* public static native int startRealtime(int period) */ long Native_startRealtime(void) _JavaNative { - volatile long period = NatLib_LoadInt(0); + volatile long period; + RTIME count; - _asm - - mov T2CON,#00h ; clear TR2 = disable timer 2 - anl IE,#0dfh ; clear ET2 = disable timer 2 interrupt - - mov dps,#1 - mov dptr,#controller - mov a,#2bh ; timer 2 - call System_InstallInterrupt - mov dps,#00h + aic_initialize(BASE,24.0,20e3,2000); - anl CKCON,#0dfh ; clear T2M = timer 2 uses clock/12 - mov T2MOD,#00h ; clear DCEN = counts up only + period = NatLib_LoadInt(0); + count=micro2count(period); + start_rt_timer(count); + pthread_create(&th,NULL,pid,&y); + period=count2micro(count); + + aic_on(); + brake_release(); -; TINI oscillator is 18432000 => counter=0c400h=100Hz - mov RCAP2L,#00h ; count reload LSB - mov TL2,#00h ; count LSB - mov RCAP2H,#0c4h ; count reload MSB - mov TH2,#0c4h ; count MSB - - orl IP,#20h ; set PT2 = set timer 2 interrupt to high priority - orl IE,#20h ; set ET2 = enable timer 2 interrupt - - mov T2CON,#04h ; set TR2 = enable timer 2 + _asm + clr a _endasm; - initialize(); - return period; } /* public static native void stopRealtime() */ void Native_stopRealtime(void) _JavaNative { + motor_set(0.0); + brake_apply(); + + pthread_cancel(th); + stop_rt_timer(); + + aic_off(); + + aic_finalize(); + _asm - mov T2CON,#00h ; clear TR2 = disable timer 2 - anl IE,#0dfh ; clear ET2 = disable timer 2 interrupt - anl IP,#0dfh ; clear PT2 = set timer 2 interrupt to normal priority - _endasm; -} - -static void int2ack(void) -{ - _asm - anl T2CON,#7fh ; clear TF2 = clears timer 2 overflow + clr a _endasm; } -static void controller(void) interrupt -{ - static int time=0; - - int2ack(); - - if(time) brake_apply(); else brake_release(); - time=~time; - - output=reference; -} - /* public static native float getOutput() */ float Native_getOutput(void) _JavaNative { - return output; + return y; } /* public static native void setReference(float reference) */ void Native_setReference(void) _JavaNative { - -/* + /* volatile long ref =NatLib_LoadInt(0); - reference=*((float *) &ref); -*/ + ref=*((float *) &ref); + */ _asm clr a lcall Natlib_LoadPrimitive ; get parameter in R3:R2:R1:R0 - mov dptr,#reference + mov dptr,#ref mov a,r0 movx @dptr,a inc dptr @@ -131,5 +158,6 @@ void Native_setReference(void) _JavaNative inc dptr mov a,r3 movx @dptr,a + clr a _endasm; } diff --git a/PIDDaemon/Makefile b/PIDDaemon/Makefile deleted file mode 100644 index 590bf53..0000000 --- a/PIDDaemon/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar -APIDBPATH=-d /opt/tini/bin/tini.db -TINIRTAIPATH=${HOME}/src/tini/rtai -LIBPATH=-PATH ${TINIRTAIPATH}/lib/rtai.jar -JAVAFLAGS=-O -target 1.1 -source 1.3 -ASMINC=-I/opt/tini/native/lib -ASMOPT=-f 1.14 -p 390 -l -ASMMACRO=/opt/tini/native/bin/Linux/macro -ASM=/opt/tini/native/bin/Linux/a390 -CC=sdcc -CFLAGS=-mTININative -S --use-accelerator -CINCLUDE=-I${TINIRTAIPATH}/include -CMPFLAGS=${CFLAGS} ${CINCLUDE} - -all: PIDDaemon.tini - -PIDDaemon.class: PIDDaemon.java - javac ${CLASSPATH} ${JAVAFLAGS} PIDDaemon.java - -pid.mpp: pid.a51 - ${ASMMACRO} ${ASMINC} -e- pid.a51 - -globals.mpp: globals.a51 - ${ASMMACRO} ${ASMINC} globals.a51 - -pid.tlib: pid.mpp globals.mpp ${TINIRTAIPATH}/lib/globals.mpp - cat pid.mpp ${TINIRTAIPATH}/lib/globals.mpp globals.a51 > tmp.mpp - ${ASM} ${ASMOPT} tmp.mpp - mv tmp.tlib pid.tlib - mv tmp.lst pid.lst - -pid.a51: pid.c - ${CC} ${CMPFLAGS} pid.c - -PIDDaemon.tini: PIDDaemon.class pid.tlib - java ${CLASSPATH} TINIConvertor -n pid.tlib -f PIDDaemon.class ${APIDBPATH} -o PIDDaemon.tini - - -clean: - rm -f *.bak *~ PIDDaemon.class pid.tlib pid.mpp pid.lst pid.a51 globals.mpp tmp.mpp tmp.lst - -distclean: clean - rm -f PIDDaemon.tini - diff --git a/PIDDaemon/pid1.c b/PIDDaemon/pid1.c deleted file mode 100644 index 40019df..0000000 --- a/PIDDaemon/pid1.c +++ /dev/null @@ -1,85 +0,0 @@ -/****************************************************************************** - - Actuator Interface Card - PID Daemon - Real Time Module - Copyright (C) 2004, 2005 Walter Fetter Lages - - This program is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 2 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program; if not, write to the Free Software - Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. - - You can also obtain a copy of the GNU General Public License - at . - -*******************************************************************************/ - -#define RTIME long - -RTIME Native_start_rt_timer(void) _JavaNative -{ - volatile long period = NatLib_LoadInt(0); - - _asm - - mov T2CON,#00h ; clear TR2 = disable timer 2 - anl IE,#0dfh ; clear ET2 = disable timer 2 interrupt - - mov dps,#1 - mov dptr,#count - mov a,#2bh ; timer 2 - call System_InstallInterrupt - mov dps,#00h - - anl CKCON,#0dfh ; clear T2M = timer 2 uses clock/12 - mov T2MOD,#00h ; clear DCEN = counts up only - -; TINI oscillator is 18432000 => counter=0c400h=100Hz - mov RCAP2L,#00h ; count reload LSB - mov TL2,#00h ; count LSB - mov RCAP2H,#0c4h ; count reload MSB - mov TH2,#0c4h ; count MSB - - orl IP,#20h ; set PT2 = set timer 2 interrupt to high priority - orl IE,#20h ; set ET2 = enable timer 2 interrupt - - mov T2CON,#04h ; set TR2 = enable timer 2 - _endasm; - - return period; -} - -void Native_stop_rt_timer(void) _JavaNative -{ - _asm - mov T2CON,#00h ; clear TR2 = disable timer 2 - anl IE,#0dfh ; clear ET2 = disable timer 2 interrupt - anl IP,#0dfh ; clear PT2 = set timer 2 interrupt to normal priority - _endasm; -} - -static void int2ack(void) -{ - _asm - anl T2CON,#7fh ; clear TF2 = clears timer 2 overflow - _endasm; -} - -extern volatile long counter; - -static void count(void) interrupt -{ - int2ack(); - - counter++; -} diff --git a/PwmRtTest/Makefile b/PwmRtTest/Makefile new file mode 100644 index 0000000..23f2cf9 --- /dev/null +++ b/PwmRtTest/Makefile @@ -0,0 +1,43 @@ +CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar +APIDBPATH=-d /opt/tini/bin/tini.db +TRTAIPATH=${HOME}/src/tini/rtai +TMATHPATH=${HOME}/src/tini/tmath +LIBPATH=-PATH ${TINIRTAIPATH}/lib/rtai.jar +JAVAFLAGS=-O -target 1.1 -source 1.3 +ASMINC=-I/opt/tini/native/lib +ASMOPT=-f 1.15 -p 390 -l +ASMMACRO=/opt/tini/native/bin/Linux/macro +ASM=/opt/tini/native/bin/Linux/a390 +CC=sdcc +CFLAGS=-mTININative -S --use-accelerator +CINCLUDE=-I${TRTAIPATH}/include -I${TMATHPATH}/include -I../include +CMPFLAGS=${CFLAGS} ${CINCLUDE} + +all: PwmRtTest.tini + +PwmRtTest.class: PwmRtTest.java ../lib/AIC.jar + javac ${JAVAFLAGS} ${CLASSPATH} PwmRtTest.java + +pwmtest.mpp: pwmtest.a51 + ${ASMMACRO} ${ASMINC} -e- pwmtest.a51 + +pwmtest.tlib: pwmtest.mpp ../lib/aicio.mpp.lib + cat pwmtest.mpp ${TRTAIPATH}/lib/rtai.mpp.lib ../lib/aicio.mpp.lib > tmp.mpp + echo end >> tmp.mpp + ${ASM} ${ASMOPT} tmp.mpp + mv tmp.tlib pwmtest.tlib + mv tmp.lst pwmtest.lst + +pwmtest.a51: pwmtest.c ../include/aicio.h + ${CC} ${CMPFLAGS} pwmtest.c + +PwmRtTest.tini: PwmRtTest.class pwmtest.tlib + java ${CLASSPATH} TINIConvertor -n pwmtest.tlib -f PwmRtTest.class ${APIDBPATH} -o PwmRtTest.tini + + +clean: + rm -f *.bak *~ PwmRtTest.class pwmtest.tlib pwmtest.mpp pwmtest.lst pwmtest.a51 globals.mpp tmp.mpp tmp.lst + +distclean: clean + rm -f PwmRtTest.tini + diff --git a/PwmRtTest/PwmRtTest.java b/PwmRtTest/PwmRtTest.java new file mode 100644 index 0000000..ec697b3 --- /dev/null +++ b/PwmRtTest/PwmRtTest.java @@ -0,0 +1,103 @@ +/****************************************************************************** + + Actuator Interface Card + PWM Real Time Test + User Interface Module + Copyright (C) 2005 Walter Fetter Lages + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +import com.dalsemi.system.*; +import java.lang.Math.*; + +class PwmRtTest +{ + public static native int startRealtime(int period); + public static native void stopRealtime(); + public static native void pwmSet(int duty); + static final int ST=100; + + private static void delay(int ms) + { + long time=TINIOS.uptimeMillis(); + while(TINIOS.uptimeMillis() < time+ms); + } + + static void iter(int i) + { + double volt2duty=0.5/24.0; + int dutycycle=((int)(i*volt2duty*1000000))+500000; + pwmSet(dutycycle); + System.out.println("PWM Duty Cicle: " + dutycycle); + delay(ST); + } + + public static void main(String[] args) + { + int i; + boolean run=true; + + System.out.println("AIC PWM Real Time Test Program"); + System.out.println("Copyright (C) Walter Fetter Lages, 2005.\n"); + + int tfms=0; + if(args.length==1) + { + tfms=Integer.parseInt(args[0]); + } + else + { + System.out.println("Usage:\tPwmRtTest ms"); + System.exit(1); + } + + System.out.print("Loading native library..."); + System.loadLibrary("pwmtest.tlib"); + System.out.println("done."); + + int us=10000; + System.out.print("Starting real-time timer with "+us+" us period..."); + int period=startRealtime(us); + System.out.println("done."); + System.out.println("Effective period: "+period+" us."); + + System.out.println("Running PwmRtTest for "+tfms+" ms..."); + + long t0=TINIOS.uptimeMillis(); + long t=0; + while(t <= tfms) + { + System.out.println("t="+t+" ms"); + for(i=0;(i <= 24) && run;i++) iter(i); + t=TINIOS.uptimeMillis()-t0; + if(t > tfms) break; + for(i=24;(i >= -24) && run;i--) iter(i); + t=TINIOS.uptimeMillis()-t0; + if(t > tfms) break; + for(i=-24;(i <= 0) && run;i++) iter(i); + t=TINIOS.uptimeMillis()-t0; + } + System.out.println("...done."); + + System.out.print("Stoping real-time timer..."); + stopRealtime(); + System.out.println("done."); + } +} diff --git a/PwmRtTest/pwmtest.c b/PwmRtTest/pwmtest.c new file mode 100644 index 0000000..32d7389 --- /dev/null +++ b/PwmRtTest/pwmtest.c @@ -0,0 +1,99 @@ +/****************************************************************************** + + Actuator Interface Card + PWM Real Time Test + Real Time Module + Copyright (C) 2005 Walter Fetter Lages + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ +#include +#include + +#include + +#define BASE 0x800000 + +volatile pthread_t th; +volatile unsigned long duty; + +void pwmtest_globals(void) _naked +{ + _asm +th: db 0,0,0,0 +duty: db 0,0,0,0 + _endasm; +} + +void *pwmtest(void *arg) +{ + pwm_set_duty(duty); + return NULL; +} + +/* public static native int startRealtime(int period) */ +long Native_startRealtime(void) _JavaNative +{ + unsigned int count; + volatile long period; + + count=(unsigned int)(REF_FREQ/20e3) & 0xffff; + pwm_initialize(BASE,MIN_COUNT,count); + brake_initialize(BASE); + + period=NatLib_LoadInt(0); + start_rt_timer(micro2count(period)); + pthread_create(&th,NULL,pwmtest,NULL); + + pwm_on(); + brake_release(); + + _asm + clr a + _endasm; + + return period; +} + +/* public static native void stopRealtime() */ +void Native_stopRealtime(void) _JavaNative +{ + brake_apply(); + pwm_off(); + + pthread_cancel(th); + stop_rt_timer(); + + pwm_finalize(); + + _asm + clr a + _endasm; +} + +/* public static native void pwmSet(unsigned int duty) */ +void Native_pwmSet(void) _JavaNative +{ + duty=NatLib_LoadInt(0); + + _asm + clr a + _endasm; +} + diff --git a/TODO b/TODO index 515a669..e2b7e62 100644 --- a/TODO +++ b/TODO @@ -1,3 +1,9 @@ +- Reorganizar a estrutura de diretorios. + +- Reunir todas as configuracoes dos Makefiles em um unico arquivo. + +- Implementar todas os metodos da classe AIC em C. + - Utilizar RCCI/RCCL? - Verificar a maxima taxa possivel com CAN. Em Java T=14ms. diff --git a/include/aicio.h b/include/aicio.h index c5a186e..be79e86 100644 --- a/include/aicio.h +++ b/include/aicio.h @@ -36,6 +36,26 @@ @{ */ +/** Reference frequency = 10MHz +*/ +#define REF_FREQ 10e6 + +/** Default Switching Frequency = 20KHz +*/ +#define SW_FREQ 20e3 + +/** H-bridge turn-off delay +*/ +#define TURNOFF_DELAY 600e-9 + +/** PWM minimal count value +*/ +#define MIN_COUNT (unsigned int)(2*TURNOFF_DELAY*REF_FREQ) + +/** PWM maximal count value +*/ +#define MAX_COUNT (unsigned int)(REF_FREQ/SW_FREQ) + /** Initializes brake * @param base Base address for on board devices * @since AIC-1.4.2 @@ -82,7 +102,7 @@ extern void pwm_set_freq(float frequency); * @return PWM frequency in Hz * @since AIC-1.4.2 */ -//extern double pwm_get_freq(void); +extern float pwm_get_freq(void); /** PWM initialization * @param base Base address for on board devices @@ -103,7 +123,7 @@ extern void pwm_finalize(void); * @return the count value programmed to the PWM timer * @since AIC-1.4.2 */ -extern unsigned long pwm_set_duty(float dutycycle); +extern unsigned long pwm_set_duty_float(float dutycycle); /** Sets the duty-cycle of PWM * @param dutycycle to be used by PWM as a int from 0 to 1000000 @@ -122,7 +142,124 @@ extern void pwm_on(void); */ extern void pwm_off(void); +/** Initializes the motor driver +* @param base Base address for on board devices +* @param voltage Motor Voltage +* @param freq PWM frequency +* @since AIC-1.5.0 +*/ +extern void motor_initialize(unsigned long base,float voltage,float freq); + +/** Finalizes the motor driver +* @since AIC-1.5.0 +*/ +extern void motor_finalize(void); + +/** Turns the motor driver on +* @since AIC-1.5.0 +*/ +extern void motor_on(void); + +/** Turns the motor driver off +* @since AIC-1.5.0 +*/ +extern void motor_off(void); + +/** Sets the voltage to be applied by the motor driver +* @param voltage to be applied by the motor driver +* @return the duty-cycle of the associated PWM +* @since AIC-1.5.0 +*/ +extern float motor_set(float voltage); + +/** Clears the counter on the quadrature decoder chip +* @since AIC-1.5.0 +*/ +extern encoder_clear(void); + +/** Initializes Encoder, clears the counter on the quadrature decoder chip +* @param base Base address for on board devices +* @param pulses Number of pulses per revolution of the encoder, considering the +* quadrature decoder +* @since AIC-1.5.0 +*/ +extern void encoder_initialize(unsigned long base,long pulses); + +/** Finalizes Encoder +* @since AIC-1.5.0 +*/ +extern void encoder_finalize(void); + +/** Gets the count on the quadrature decoder chip +* @return the number of pulses (in 2-complement) since the last time +* the counter was cleared +* @since AIC-1.5.0 +*/ +extern long encoder_get_count(void); + +/** Gets the count and clears the counter on the quadrature decoder chip +* @return the number of pulses (in 2-complement) since the last time +* the counter was cleared +* @since AIC-1.5.0 +*/ +extern long encoder_get_count_and_clear(void); + +/** Reads the counter on the quadrature decoder chip and converts the motion +* to radians +* @return the displacement in radians since the last time +* the encoder was cleared +* @since AIC-1.5.0 +*/ +extern float encoder_read(void); + +/** Reads the counter on the quadrature decoder chip, converts the motion +* to radians and clears the counter +* @return the displacement in radians since the last time +* the encoder was cleared +* @since AIC-1.5.0 +*/ +extern float encoder_read_and_Clear(void); + +/** Turns an AIC on +* @since AIC-1.5.0 +*/ +extern void aic_on(void); + +/** Turns an AIC off +* @since AIC-1.5.0 +*/ +extern void aic_off(void); + +/** Initializes an AIC +* @param base Base address for on board devices +* @param vm Motor Voltage +* @param freq PWM frequency +* @param np Pulses per encoder revolution +* @since AIC-1.5.0 +*/ +extern void aic_initialize(unsigned long base,float vm,float freq,long np); + +/** Finalizes an AIC +* @since AIC-1.5.0 +*/ +extern void aic_finalize(void); + /** @} */ + +/* Unfortunately the TINI Native target does not support a linker, so all +functions are inlined */ +#ifndef _MOTOR_C +#include "../lib/aicio/motor.c" +#endif + +#ifndef _ENCODER_C +#include "../lib/aicio/encoder.c" +#endif + +#ifndef _AIC_C +#include "../lib/aicio/aic.c" +#endif + #endif diff --git a/include/aicmacro.inc b/include/aicmacro.inc index a6be3d3..6ecb9f0 100644 --- a/include/aicmacro.inc +++ b/include/aicmacro.inc @@ -27,7 +27,7 @@ ; parameters: addr?=address to store data into ; inputs: dpx:dph:dhl=data to be stored ; destroy: a -; outputs: dpx1:dph1:dpl1addr? +; outputs: dpx1:dph1:dpl1=addr? STORE24 macro param addr? mov a,dpx orl DPS,#01h diff --git a/lib/Makefile b/lib/Makefile index a24cb12..5702eae 100644 --- a/lib/Makefile +++ b/lib/Makefile @@ -1,3 +1,4 @@ + CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar:/opt/tini/bin/modules.jar:. APIDBPATH=-d /opt/tini/bin/tini.db PKGPATH=br/ufrgs/eletro/AIC @@ -76,44 +77,57 @@ ${ASMINC}/ds80c390.inc: ${NATINC}/ds80c390.inc ${ASMINC}/apiequ.inc: ${NATINC}/apiequ.inc ln -s ${NATINC}/apiequ.inc ${ASMINC}/apiequ.inc -brake.mpp: brake.a51 ${TINIINC} ${ASMINC}/aicpld.inc - ${ASMMACRO} -I${ASMINC} -e- brake.a51 +brake.tlib: brake.mpp + ${ASM} ${ASMOPT} brake.mpp -brake.tlib: tlib/brake.mpp brake.mpp - cat tlib/brake.mpp brake.mpp > brake.tmp.mpp - echo end >> brake.tmp.mpp - ${ASM} ${ASMOPT} brake.tmp.mpp +brake.mpp: tlib/brake.mpp aicio/brake.mpp + cat tlib/brake.mpp aicio/brake.mpp > brake.mpp + echo end >> brake.mpp tlib/brake.mpp: tlib/brake.a51 $(MAKE) -C tlib brake.mpp - -index.mpp: index.a51 ${TINIINC} ${ASMINC}/aicpld.inc - ${ASMMACRO} -I${ASMINC} -e- index.a51 -index.tlib: tlib/index.mpp index.mpp - cat tlib/index.mpp index.mpp > index.tmp.mpp - echo end >> index.tmp.mpp - ${ASM} ${ASMOPT} index.tmp.mpp +aicio/brake.mpp: aicio/brake.a51 + $(MAKE) -C aicio brake.mpp + +index.tlib: index.mpp + ${ASM} ${ASMOPT} index.mpp + +index.mpp: tlib/index.mpp aicio/index.mpp + cat tlib/index.mpp aicio/index.mpp > index.mpp + echo end >> index.mpp tlib/index.mpp: tlib/index.a51 $(MAKE) -C tlib index.mpp -encoder.mpp: encoder.a51 ${TINIINC} ${ASMINC}/aicpld.inc - ${ASMMACRO} -I${ASMINC} encoder.a51 +aicio/index.mpp: aicio/index.a51 + $(MAKE) -C aicio index.mpp encoder.tlib: encoder.mpp ${ASM} ${ASMOPT} encoder.mpp + +encoder.mpp: tlib/encoder.mpp aicio/encoder.mpp + cat tlib/encoder.mpp aicio/encoder.mpp > encoder.mpp + echo end >> encoder.mpp -pwm.mpp: pwm.a51 ${TINIINC} ${ASMINC}/aicpld.inc - ${ASMMACRO} -I${ASMINC} -e- pwm.a51 - -pwm.tlib: pwm.mpp tlib/pwm.mpp - cat tlib/pwm.mpp pwm.mpp > pwm.tmp.mpp - echo end >> pwm.tmp.mpp - ${ASM} ${ASMOPT} pwm.tmp.mpp +tlib/encoder.mpp: tlib/encoder.a51 + $(MAKE) -C tlib encoder.mpp + +aicio/encoder.mpp: aicio/encoder.a51 + $(MAKE) -C aicio encoder.mpp + +pwm.tlib: pwm.mpp + ${ASM} ${ASMOPT} pwm.mpp + +pwm.mpp: tlib/pwm.mpp aicio/pwm.mpp + cat tlib/pwm.mpp aicio/pwm.mpp > pwm.mpp + echo end >> pwm.mpp tlib/pwm.mpp: tlib/pwm.a51 $(MAKE) -C tlib pwm.mpp + +aicio/pwm.mpp: aicio/pwm.a51 + $(MAKE) -C aicio pwm.mpp AIC.jar: ${PKGPATH}/PLD.class\ ${PKGPATH}/CtrlRegister.class\ @@ -153,8 +167,11 @@ aichostcan.o: aichostcan.cpp ../include/aichostcan.h ../include/aicnet.h libaic.a: aic.o aicudp.o aiccan.o aicsim.o aichost.o aichostudp.o aichostcan.o ar -crvs libaic.a aic.o aicudp.o aiccan.o aicsim.o aichost.o aichostudp.o aichostcan.o -aicio.mpp.lib: brake.mpp index.mpp - cat brake.mpp index.mpp > aicio.mpp.lib +aicio.mpp.lib: aicio/aicio.mpp.lib + cp aicio/aicio.mpp.lib . + +aicio/aicio.mpp.lib: aicio/brake.a51 aicio/index.a51 aicio/pwm.a51 aicio/encoder.a51 + $(MAKE) -C aicio install: install -m 0644 libaic.a ${LIBINST} diff --git a/lib/aicio/Makefile b/lib/aicio/Makefile new file mode 100644 index 0000000..5e22640 --- /dev/null +++ b/lib/aicio/Makefile @@ -0,0 +1,75 @@ +CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar:/opt/tini/bin/modules.jar:. +APIDBPATH=-d /opt/tini/bin/tini.db +PKGPATH=br/ufrgs/eletro/AIC +JAVAFLAGS=-O -target 1.1 -source 1.3 + +NATINC=/opt/tini/native/lib +ASMINC=../../include +TINIINC=${ASMINC}/tini.inc ${ASMINC}/tinimacro.inc ${ASMINC}/ds80c390.inc ${ASMINC}/apiequ.inc +ASMOPT=-f 1.15 -p 390 -l +ASMMACRO=/opt/tini/native/bin/Linux/macro +ASM=/opt/tini/native/bin/Linux/a390 + +#CFLAGS = -O2 -Wall -march=i486 +CFLAGS=-mTININative -S --use-accelerator +#CINCLUDE = -I. -I${HOME}/include +CINCLUDE=-I${TINIRTAIPATH}/include -I../../include + +CPPFLAGS = +CPPINCLUDE = -I${HOME}/include/cpp -I/usr/local/include/cpp -I../../include +LIB = ${HOME}/lib + +LIBINST=/usr/local/lib +INCLUDEINST=/usr/local/include + +INCLUDE=${CINCLUDE} ${CPPINCLUDE} +FLAGS= ${CFLAGS} ${CPPFLAGS} -DDEBUG +LIBDIR=${CLIBDIR} ${CPPLIBDIR} +LIBS=${CLIBS} ${CPPLIBS} + +#CMP= g++ +CMP= sdcc +CMPFLAGS= ${FLAGS} ${INCLUDE} +LDFLAGS= ${LIBDIR} ${LIBS} + +all: aicio.mpp.lib + +${ASMINC}/tini.inc: ${NATINC}/tini.inc + ln -s ${NATINC}/tini.inc ${ASMINC}/tini.inc + +${ASMINC}/tinimacro.inc: ${NATINC}/tinimacro.inc + ln -s ${NATINC}/tinimacro.inc ${ASMINC}/tinimacro.inc + +${ASMINC}/ds80c390.inc: ${NATINC}/ds80c390.inc + ln -s ${NATINC}/ds80c390.inc ${ASMINC}/ds80c390.inc + +${ASMINC}/apiequ.inc: ${NATINC}/apiequ.inc + ln -s ${NATINC}/apiequ.inc ${ASMINC}/apiequ.inc + +brake.mpp: brake.a51 ${TINIINC} ${ASMINC}/aicpld.inc + ${ASMMACRO} -I${ASMINC} -e- brake.a51 + +index.mpp: index.a51 ${TINIINC} ${ASMINC}/aicpld.inc + ${ASMMACRO} -I${ASMINC} -e- index.a51 + +encoder.mpp: encoder.a51 ${TINIINC} ${ASMINC}/aicpld.inc + ${ASMMACRO} -I${ASMINC} -e- encoder.a51 + +pwm.mpp: pwm.a51 ${TINIINC} ${ASMINC}/aicpld.inc + ${ASMMACRO} -I${ASMINC} -e- pwm.a51 + +motor.mpp: motor.a51 ${TINIINC} + ${ASMMACRO} -I${ASMINC} -e- motor.a51 + +aicio.mpp.lib: brake.mpp index.mpp pwm.mpp encoder.mpp motor.mpp + cat brake.mpp index.mpp pwm.mpp encoder.mpp motor.mpp > aicio.mpp.lib + +install: + +clean: + rm -rf *.o *~ *.bak *.class ../include/{*.bak,*~} br *.tlib *.mpp ${TINIINC} *.lst + +distclean: clean + rm -rf aicio.mpp.lib + +uninstall: diff --git a/lib/aicio/aic.c b/lib/aicio/aic.c new file mode 100644 index 0000000..2110aea --- /dev/null +++ b/lib/aicio/aic.c @@ -0,0 +1,56 @@ +/****************************************************************************** + + Actuator Interface Card + Real Time Interface + Copyright (C) 2005 Walter Fetter Lages + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include + +#define _AIC_C +#include + +void aic_on(void) +{ + motor_on(); + encoder_clear(); +} + +void aic_off(void) +{ + motor_set(0); + motor_off(); + encoder_clear(); +} + +void aic_initialize(unsigned long base,float vm,float freq,long np) +{ + motor_initialize(base,vm,freq); + encoder_initialize(base,np); + brake_initialize(base); + index_initialize(base); +} + +void aic_finalize(void) +{ + aic_off(); +} + diff --git a/lib/aicio/brake.a51 b/lib/aicio/brake.a51 new file mode 100644 index 0000000..3f77038 --- /dev/null +++ b/lib/aicio/brake.a51 @@ -0,0 +1,88 @@ +;****************************************************************************** +; +; Actuator Interface Card +; Brake Functions +; Copyright (C) 2005 Walter Fetter Lages +; +; This program is free software; you can redistribute it and/or modify +; it under the terms of the GNU General Public License as published by +; the Free Software Foundation; either version 2 of the License, or +; (at your option) any later version. +; +; This program is distributed in the hope that it will be useful, +; but WITHOUT ANY WARRANTY; without even the implied warranty of +; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +; GNU General Public License for more details. +; +; You should have received a copy of the GNU General Public License +; along with this program; if not, write to the Free Software +; Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +; +; You can also obtain a copy of the GNU General Public License +; at . +; +;******************************************************************************* + +$include(tini.inc) +$include(ds80c390.inc) +$include(tinimacro.inc) + +$include(aicpld.inc) +$include(aicmacro.inc) + +; data saved in big-endian mode +; Brake base address +brake_base: db 0,0,0 + +; Initializes Brake +; void brake_initialize(unsigned long base); +brake_initialize: + mov r0,dpl ; parameter in b:dpx:dph:dpl + mov r1,dph ; BUG IN SDCC MANUAL! + mov r2,dpx + mov r3,b + + mov dptr,#CONTROL ; compute brake address + lcall Add_Dptr1_24 ; dptr=dptr+R2:R1:R0 + + STORE24 #brake_base ; store brake address + + clr a ; success + ret + +; Applies the electromagnetic brakes +; void brake_apply(void) +brake_apply: + LOAD24 #brake_base ; get brake address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + mov a,#BRAKEAPPLY ; apply brake + movx @dptr,a + EXIT_PCE_SPACE_AND_POP_STATE + + + RESTORE_STRETCH r0 + + clr a ; success + ret + +; Releases the electromagnetic brake +; void brake_release(void) +brake_release: + LOAD24 #brake_base ; get brake address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + mov a,#BRAKERELEASE ; release brake + movx @dptr,a + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + clr a ; success + ret + + end diff --git a/lib/aicio/encoder.a51 b/lib/aicio/encoder.a51 new file mode 100644 index 0000000..ebec810 --- /dev/null +++ b/lib/aicio/encoder.a51 @@ -0,0 +1,199 @@ +;****************************************************************************** +; +; Actuator Interface Card +; Encoder Functions +; Copyright (C) 2004, 2005 Walter Fetter Lages +; +; This program is free software; you can redistribute it and/or modify +; it under the terms of the GNU General Public License as published by +; the Free Software Foundation; either version 2 of the License, or +; (at your option) any later version. +; +; This program is distributed in the hope that it will be useful, +; but WITHOUT ANY WARRANTY; without even the implied warranty of +; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +; GNU General Public License for more details. +; +; You should have received a copy of the GNU General Public License +; along with this program; if not, write to the Free Software +; Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +; +; You can also obtain a copy of the GNU General Public License +; at . +; +;******************************************************************************* + +$include(tini.inc) +$include(ds80c390.inc) +$include(tinimacro.inc) + +$include(aicpld.inc) +$include(aicmacro.inc) + +_bpx EQU 01Eh ; _bpx (frame pointer) mapped to R8_B3:R7_B3 +_ap EQU 01Dh ; _ap mapped to R6_B3 + +; data saved in big-endian mode +; Encoder base address +encoder_base: db 0,0,0 +encoder_pulses: db 0,0,0,0 + +; Initializes Encoder +; void encoder_initialize(unsigned long base,long pulses); +encoder_initialize: + push _bpx ; SDCC style preface code + push _bpx+1 + mov _bpx,sp + mov _bpx+1,esp + anl _bpx+1,#3 + + mov r0,dpl ; parameter in b:dpx:dph:dpl + mov r1,dph ; BUG IN SDCC MANUAL! + mov r2,dpx + mov r3,b + + mov dptr,#ENCODER ; compute encoder address + lcall Add_Dptr1_24 ; dptr=dptr+R2:R1:R0 + + STORE24 #encoder_base ; store encoder address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + clr a ; clear encoder + movx @dptr,a + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + mov a,_bpx + clr c + subb a,#8 ; second parameter is base pointer-6 + mov b,a ; point dpx:dptr to it + mov a,_bpx+1 + subb a,#0 + mov dpx,#40h + mov dph,a + mov dpl,b + + mov dps,#21h ; store in encoder_pulses + mov dptr,#encoder_pulses + movx a,@dptr + movx @dptr,a + inc dptr + inc dptr + movx a,@dptr + movx @dptr,a + mov dps,#0 + + mov sp,_bpx ; SDCC style epilogue code + mov esp,_bpx+1 + pop _bpx+1 + pop _bpx + + clr a ; sucess + ret + +; Finalizes Encoder +; void encoder_finalize(void) +encoder_finalize: + + ; fall-through to encoder_clear: + +; Clears the counter on the quadrature decoder chip +; void clear(void) +encoder_clear: + LOAD24 #encoder_base ; get encoder address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + clr a + movx @dptr,a ; clear counter + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + clr a ; success + ret + +; Gets the count on the quadrature decoder chip +; return the number of pulses (in 2-complement) since the last time +; the counter was cleared +; long encoder_get_count(void) +encoder_get_count: + LOAD24 #encoder_base ; get encoder address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + movx a,@dptr ; get count high byte + mov r1,a ; form count value in R3:R2:R1:R0 + + rlc a ; sign extension to 32 bits + clr a ; r1 > 0 extend with 0s + jnc encoder_posextend + cpl a ; r1 < 0 extend with 1s +encoder_posextend: + mov r2,a ; form count value in R3:R2:R1:R0 + mov r3,a + + inc dptr + movx a,@dptr ; get count low byte + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + mov r0,a ; form count value in R3:R2:R1:R0 + + mov dpl,a ; return count in b:dpx:dph:dpl + mov dph,r1 + + clr a ; sucess + + mov dpx,r2 + mov b,r3 + + ret + + +; Gets the count and clearsthe counter on the quadrature decoder chip +; return the number of pulses (in 2-complement) since the last time +; the counter was cleared +; long encoder_get_count_and_clear() +encoder_get_count_and_clear: + LOAD24 #encoder_base ; get encoder address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + movx a,@dptr ; get count high byte + mov r1,a ; form count value in R3:R2:R1:R0 + + rlc a ; sign extension to 32 bits + clr a ; r1 > 0 extend with 0s + jnc encoder_posextend1 + cpl a ; r1 < 0 extend with 1s +encoder_posextend1: + mov r2,a ; form count value in R3:R2:R1:R0 + mov r3,a + + inc dptr + movx a,@dptr ; get count low byte + movx @dptr,a ; clear counter + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + mov r0,a ; form count value in R3:R2:R1:R0 + + mov dpl,a ; return count in b:dpx:dph:dpl + mov dph,r1 + + clr a ; sucess + + mov dpx,r2 + mov b,r3 + + end + diff --git a/lib/aicio/encoder.c b/lib/aicio/encoder.c new file mode 100644 index 0000000..d2d8737 --- /dev/null +++ b/lib/aicio/encoder.c @@ -0,0 +1,41 @@ +/****************************************************************************** + + Actuator Interface Card + Encoder Real Time Interface + Copyright (C) 2005 Walter Fetter Lages + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include + +#define _ENCODER_C +#include + +extern long encoder_pulses; + +float encoder_read(void) +{ + return ((2*M_PI/encoder_pulses)*encoder_get_count()); +} + +float encoder_read_and_Clear(void) +{ + return ((2*M_PI/encoder_pulses)*encoder_get_count_and_clear()); +} diff --git a/lib/aicio/index.a51 b/lib/aicio/index.a51 new file mode 100644 index 0000000..bff12f8 --- /dev/null +++ b/lib/aicio/index.a51 @@ -0,0 +1,77 @@ +;****************************************************************************** +; +; Actuator Interface Card +; Index Function +; Copyright (C) 2005 Walter Fetter Lages +; +; This program is free software; you can redistribute it and/or modify +; it under the terms of the GNU General Public License as published by +; the Free Software Foundation; either version 2 of the License, or +; (at your option) any later version. +; +; This program is distributed in the hope that it will be useful, +; but WITHOUT ANY WARRANTY; without even the implied warranty of +; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +; GNU General Public License for more details. +; +; You should have received a copy of the GNU General Public License +; along with this program; if not, write to the Free Software +; Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +; +; You can also obtain a copy of the GNU General Public License +; at . +; +;******************************************************************************* + +$include(tini.inc) +$include(ds80c390.inc) +$include(tinimacro.inc) + +$include(aicpld.inc) +$include(aicmacro.inc) + +; data saved in big-endian mode +; Index base address +index_base: db 0,0,0 + +; Initializes Index +; void index_initialize(unsigned long base); +index_initialize: + mov r0,dpl ; parameter in b:dpx:dph:dpl + mov r1,dph ; BUG IN SDCC MANUAL! + mov r2,dpx + mov r3,b + + mov dptr,#CONTROL ; compute index address + lcall Add_Dptr1_24 ; dptr=dptr+R2:R1:R0 + + STORE24 #index_base ; store index address + + clr a ; success + ret + +; Reads the sync-switch +; returns the sync-switch state +; unsigned long index_read(void) +index_read: + LOAD24 #index_base ; get index address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + movx a,@dptr ; read index + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + anl a,#01h + mov dpl,a ; return value in b:dpx:dph:dpl + clr a ; BUG IN SDCC MANUAL! + mov dph,a + mov dpx,a + mov b,a + + ret + + end + diff --git a/PIDDaemon/globals.a51 b/lib/aicio/motor.a51 similarity index 84% rename from PIDDaemon/globals.a51 rename to lib/aicio/motor.a51 index 27ccad3..29e3830 100644 --- a/PIDDaemon/globals.a51 +++ b/lib/aicio/motor.a51 @@ -1,9 +1,9 @@ ;****************************************************************************** ; ; Actuator Interface Card -; PID Daemon -; Real Time Module Global Variables -; Copyright (C) 2004, 2005 Walter Fetter Lages +; Motor Real Time Interface +; Global Variables +; Copyright (C) 2005 Walter Fetter Lages ; ; This program is free software; you can redistribute it and/or modify ; it under the terms of the GNU General Public License as published by @@ -25,7 +25,5 @@ ;*******************************************************************************/ -output: db 0,0,0,0 -th: db 0,0 -reference: db 0,0,0,0 +motor_volt2duty: db 0,0,0,0 end diff --git a/lib/aicio/motor.c b/lib/aicio/motor.c new file mode 100644 index 0000000..6627991 --- /dev/null +++ b/lib/aicio/motor.c @@ -0,0 +1,63 @@ +/****************************************************************************** + + Actuator Interface Card + Motor Real Time Interface + Copyright (C) 2005 Walter Fetter Lages + + This program is free software; you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation; either version 2 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program; if not, write to the Free Software + Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. + + You can also obtain a copy of the GNU General Public License + at . + +*******************************************************************************/ + +#include + +#define _MOTOR_C +#include + +static float motor_volt2duty; + +void motor_initialize(unsigned long baseadd,float voltage,float freq) +{ + unsigned int count; + motor_volt2duty=0.5/voltage; + count=(unsigned int)(REF_FREQ/freq) & 0xffff; + pwm_initialize(baseadd,MIN_COUNT,count); + pwm_off(); +} + +void motor_finalize(void) +{ + pwm_off(); +} + +void motor_on(void) +{ + pwm_on(); +} + +void motor_off(void) +{ + pwm_off(); +} + +float motor_set(float voltage) +{ + unsigned long dutycycle=((unsigned long)(voltage*motor_volt2duty*1000000))+500000; + pwm_set_duty(dutycycle); + return dutycycle; +} + diff --git a/lib/aicio/pwm.a51 b/lib/aicio/pwm.a51 new file mode 100644 index 0000000..7524a3c --- /dev/null +++ b/lib/aicio/pwm.a51 @@ -0,0 +1,422 @@ +;****************************************************************************** +; +; Actuator Interface Card +; PWM Functions +; Copyright (C) 2004, 2005 Walter Fetter Lages +; +; This program is free software; you can redistribute it and/or modify +; it under the terms of the GNU General Public License as published by +; the Free Software Foundation; either version 2 of the License, or +; (at your option) any later version. +; +; This program is distributed in the hope that it will be useful, +; but WITHOUT ANY WARRANTY; without even the implied warranty of +; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +; GNU General Public License for more details. +; +; You should have received a copy of the GNU General Public License +; along with this program; if not, write to the Free Software +; Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. +; +; You can also obtain a copy of the GNU General Public License +; at . +; +;******************************************************************************* + +$include(tini.inc) +$include(ds80c390.inc) +$include(tinimacro.inc) + +$include(i8254.inc) +$include(aicpld.inc) +$include(aicmacro.inc) + +_bpx EQU 01Eh ; _bpx (frame pointer) mapped to R8_B3:R7_B3 +_ap EQU 01Dh ; _ap mapped to R6_B3 + +; data saved in big-endian mode +; PWM base address +pwm_base: db 0,0,0 + +; PWM frequency in cycles of 10MHz +pwm_max_count: db 0,0 + +; minimal value to load in PWM +pwm_min_count: db 0,0 + +; Control Register base address +pwm_ctrl_base: db 0,0,0 + +; Sets the frequency of PWM in cycles of 10MHz +; void pwm_set_count(unsigned int count) +pwm_set_count: + mov r0,dpl ; parameter in dph:dpl + mov r1,dph ; BUG IN SDCC MANUAL! + + LOAD24 #pwm_base ; get PWM address + + mov a,#CTRL_8254 ; compute control register address + ADD_DPTR_0_A + + mov b,r0 ; save parameter LSB + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + mov a,#RATE_BIN_8254 ; set timer 0 mode + movx @dptr,a + + mov a,#ONE_SHOT_BIN_8254 ; set timer 1 mode + orl a,#40h ; select timer 1 + movx @dptr,a + +; program timer 0 with count + + mov a,DPS ; point dptr to timer 0 + orl DPS,#0c0h ; avoids extern memory access + inc dptr ; actualy decrement + inc dptr ; due to DPS value + inc dptr + mov DPS,a + + mov a,b ; program timer 0 lsB + movx @dptr,a + mov r2,a ; save count lsB + + mov a,r1 ; program timer 0 msB + movx @dptr,a + + EXIT_PCE_SPACE_AND_POP_STATE + + inc DPS ; save count + mov dptr,#pwm_max_count + movx @dptr,a ; store count msB + inc dptr + mov a,r2 ; store count lsB + movx @dptr,a + inc DPS + +; program timer 1 with count/2 + +pwm_duty50: inc dptr ; point dptr to timer 1 + + mov a,r1 ; compute count/2 + clr c + rrc a + mov r1,a + mov a,b + rrc a + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + movx @dptr,a ; program timer 1 lsB + mov a,r1 ; program timer 1 msB + movx @dptr,a + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + clr a ; success + ret + +; Initializes PWM +; void pwm_initialize(unsigned long base,unsigned int min_count,unsigned int count); +pwm_initialize: + push _bpx ; SDCC style preface code + push _bpx+1 + mov _bpx,sp + mov _bpx+1,esp + anl _bpx+1,#3 + + + mov r0,dpl ; parameter in b:dpx:dph:dpl + mov r1,dph ; BUG IN SDCC MANUAL! + mov r2,dpx + mov r3,b + + push R0_B0 + push R1_B0 + push R2_B0 + + mov dptr,#CONTROL ; compute Control address + lcall Add_Dptr1_24 ; dptr=dptr+R2:R1:R0 + + STORE24 #pwm_ctrl_base ; store Control register address + + pop R2_B0 + pop R1_B0 + pop R0_B0 + + mov dptr,#PWM ; compute PWM address + lcall Add_Dptr1_24 ; dptr=dptr+R2:R1:R0 + + STORE24 #pwm_base ; store PWM address + + mov a,_bpx + clr c + subb a,#06 ; second parameter is base pointer-6 + mov b,a ; point dpx:dptr to it + mov a,_bpx+1 + subb a,#0 + mov dpx,#40h + mov dph,a + mov dpl,b + + movx a,@dptr ; min_count LSB + mov b,a + inc dptr + movx a,@dptr ; min_count MSB + + mov dptr,#pwm_min_count ; stores it + movx @dptr,a + inc dptr + mov a,b + movx @dptr,a + + mov a,_bpx + clr c + subb a,#8 ; third parameter is base pointer-8 + mov b,a ; point dpx:dptr to it + mov a,_bpx+1 + subb a,#0 + mov dpx,#040h + mov dph,a + mov dpl,b + + movx a,@dptr ; store in dph:dpl + inc dptr + mov b,a + movx a,@dptr + mov dph,a + mov dpl,b + + mov sp,_bpx ; SDCC style epilogue code + mov esp,_bpx+1 + pop _bpx+1 + pop _bpx + ; to fall over max count should be in dph:dpl + ljmp pwm_set_count ; fall over to native_setCount + + +; Finalizes PWM +; void finalize(void) +pwm_finalize: + LOAD24 #pwm_base ; get pwm address + + inc DPS ; get count msB + mov dptr,#pwm_max_count + movx a,@dptr + mov r1,a + inc dptr ; get count lsB + movx a,@dptr + mov b,a + inc DPS + + SET_STRETCH STRETCH + + ljmp pwm_duty50 ; fall over to native pwm_set_count + + +; Sets the duty-cycle of PWM +; unsigned long pwm_set_duty(unsigned long dutycycle) +pwm_set_duty: + mov r0,dpl ; parameter in b:dpx:dph:dpl + mov r1,dph ; BUG IN SDCC MANUAL! + mov r2,dpx + mov r3,b + + mov a,r3 ; ensures R3:R2:R1:R0 >= 0 + jnb ACC.7,pwm_parmpos + + clr a ; forces R3:R2:R1:R0=0 + mov r3,a + mov r2,a + mov r1,a + mov r0,a + ljmp pwm_parmok + +pwm_parmpos: cjne r3,#00,pwm_parmabove ; ensures R3:R2:R1:R0 <= 1000000 + clr c + mov a,r0 ; 1000000=0f4240h + subb a,#40h + mov a,r1 + subb a,#42h + mov a,r2 + subb a,#0fh + jc pwm_parmok + +pwm_parmabove: mov r3,#00h ; forces R3:R2:R1:R0=1000000 + mov r2,#0fh + mov r1,#42h + mov r0,#40h + +pwm_parmok: + + ; ljmp pwm_countok ; debug + + + TINIOS_ENTER_CRITICAL_SECTION + + mov MCNT1,#10h ; MST=0,MOF=0,SCB=0,CLM=1 + mov MA,r0 ; R3:R2:R1:R0 >> 4 using + mov MA,r1 ; math acelerator + mov MA,r2 + mov MA,r3 + mov MCNT0,#0a4h ;/LSHIFT=1,CSE=0,SCE=1,MAS40=4 +pwm_waitshift: mov a,MCNT1 + jb ACC.7,pwm_waitshift ; wait MST cleared + + mov a,MA ; the result is 16 bits + mov a,MA ; discard upper 16 bits + mov r1,MA + mov r0,MA + + mov MCNT1,#00h ; R1:R0 * max_count using + mov MB,r0 ; math accelarator + mov MB,r1 + mov dptr,#pwm_max_count ; load max_count in R1_B1:R0:B1 + movx a,@dptr + mov R1_B1,a + inc dptr + movx a,@dptr + mov R0_B1,a + mov MA,a + mov a,R1_B1 + mov MA,a +pwm_waitmult: mov a,MCNT1 + jb ACC.7,pwm_waitmult ; wait MST cleared + + mov r3,MA + mov r2,MA + mov r1,MA + mov r0,MA + + mov MCNT1,#00h ;R1:R0*max_count/(1000000/16) using + mov MA,r0 ; math accelarator + mov MA,r1 + mov MA,r2 + mov MA,r3 + mov MB,#24h ; 1000000/16=F424h + mov MB,#0f4h +pwm_waitdiv: mov a,MCNT1 + jb ACC.7,pwm_waitdiv ; wait MST cleared + + mov a,MA ; The result should be 16 bits + mov a,MA ; upper 16 bits discarded + mov r1,MA + mov r0,MA + + TINIOS_EXIT_CRITICAL_SECTION + + clr c ; computes count + mov a,R0_B1 + subb a,r0 + mov r0,a + mov a,R1_B1 + subb a,r1 + mov r1,a + + mov dptr,#pwm_min_count ; check if count < min_count + movx a,@dptr + mov r3,a + inc dptr + movx a,@dptr + mov r2,a + + clr c + mov a,r0 + subb a,r2 + mov a,r1 + subb a,r3 + jnc pwm_countabove + mov r0,R2_B0 + mov r1,R3_B0 + ljmp pwm_countok + + +pwm_countabove: clr c ; check if count > max_count-min_count + mov a,R0_B1 ; computes max_count - min_count + subb a,r2 + mov r2,a + mov a,R1_B1 + subb a,r3 + mov r3,a + + clr c + mov a,r0 + subb a,r2 + mov a,r1 + subb a,r3 + jc pwm_countok + mov r0,R2_B0 + mov r1,R3_B0 + +pwm_countok: + ; mov r0,#250 ; debug + ; mov r1,#0 + + LOAD24 #pwm_base ; get PWM address + + mov a,#TIMER1_8254 ; compute timer 1 address + ADD_DPTR_0_A + + mov b,r0 ; save count LSB + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + mov a,b + movx @dptr,a ; program timer 1 lsB + mov a,r1 ; program timer 1 msB + movx @dptr,a + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 ; restore original stretch value + + mov dpl,b ; return count in b:dbx:dph:dpl + mov dph,r1 + + clr a + + mov dpx,a + mov b,a + + ret + + + +; Turns the PWM on +; void pwm_on(void) +pwm_on: + LOAD24 #pwm_ctrl_base ; get control register address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + mov a,#PWMENABLE ; enable PWM + movx @dptr,a + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + clr a ; success + ret + +; Turns the PWM off +; void pwm_off(void) +pwm_off: + LOAD24 #pwm_ctrl_base ; get control register address + + SET_STRETCH STRETCH + + ENTER_PCE_SPACE_AND_PUSH_STATE #0 + mov a,#PWMDISABLE ; disable PWM + movx @dptr,a + EXIT_PCE_SPACE_AND_POP_STATE + + RESTORE_STRETCH r0 + + clr a ; success + ret + + end + diff --git a/lib/pwm.a51 b/lib/pwm.a51 index 1c551fa..7370e5b 100644 --- a/lib/pwm.a51 +++ b/lib/pwm.a51 @@ -31,8 +31,6 @@ $include(i8254.inc) $include(aicpld.inc) $include(aicmacro.inc) -_bpx EQU 01Eh ; _bpx (frame pointer) mapped to R8_B3:R7_B3 -_ap EQU 01Dh ; _ap mapped to R6_B3 ; data saved in big-endian mode ; PWM base address @@ -121,17 +119,8 @@ duty50: inc dptr ; point dptr to timer 1 ; Initializes PWM ; void pwm_initialize(unsigned long base,unsigned int min_count,unsigned int count); pwm_initialize: - push _bpx ; SDCC style preface code - push _bpx+1 - mov _bpx,sp - mov _bpx+1,esp - anl _bpx+1,#3 - - - mov r0,dpl ; parameter in b:dpx:dph:dpl - mov r1,dph ; BUG IN SDCC MANUAL! - mov r2,dpx - mov r3,b + clr a + lcall Natlib_LoadPrimitive ; get first parameter in R3:R2:R1:R0 push R0_B0 push R1_B0 @@ -151,49 +140,18 @@ pwm_initialize: STORE24 #pwm_base ; store PWM address - mov a,_bpx - clr c - subb a,#06 ; second parameter is base pointer-6 - mov b,a ; point dpx:dptr to it - mov a,_bpx+1 - subb a,#0 - mov dpx,#40h - mov dph,a - mov dpl,b - - movx a,@dptr ; min_count LSB - mov b,a + mov a,#1h + lcall Natlib_LoadPrimitive ; get second parameter in R3:R2:R1:R0 + mov dptr,#pwm_min_count ; stores it + mov a,r1 + movx @dptr,a inc dptr - movx a,@dptr ; min_count MSB + mov a,r0 + movx @dptr,a + + mov a,#2h + lcall Natlib_LoadPrimitive ; get third parameter in R3:R2:R1:R0 - mov dptr,#pwm_min_count ; stores it - movx @dptr,a - inc dptr - mov a,b - movx @dptr,a - - mov a,_bpx - clr c - subb a,#8 ; third parameter is base pointer-8 - mov b,a ; point dpx:dptr to it - mov a,_bpx+1 - subb a,#0 - mov dpx,#040h - mov dph,a - mov dpl,b - - movx a,@dptr ; store in dph:dpl - inc dptr - mov b,a - movx a,@dptr - mov dph,a - mov dpl,b - - mov sp,_bpx ; SDCC style epilogue code - mov esp,_bpx+1 - pop _bpx+1 - pop _bpx - ; to fall over max count should be in dph:dpl ljmp pwm_set_count ; fall over to native_setCount diff --git a/lib/tlib/Makefile b/lib/tlib/Makefile index 7de5da2..89fb511 100644 --- a/lib/tlib/Makefile +++ b/lib/tlib/Makefile @@ -48,7 +48,7 @@ index.mpp: index.a51 ${TINIINC} ${ASMMACRO} -I${ASMINC} -e- index.a51 encoder.mpp: encoder.a51 ${TINIINC} - ${ASMMACRO} -I${ASMINC} encoder.a51 + ${ASMMACRO} -I${ASMINC} -e- encoder.a51 pwm.mpp: pwm.a51 ${TINIINC} ${ASMMACRO} -I${ASMINC} -e- pwm.a51 diff --git a/lib/tlib/encoder.a51 b/lib/tlib/encoder.a51 index c9cc67d..3c93d80 100644 --- a/lib/tlib/encoder.a51 +++ b/lib/tlib/encoder.a51 @@ -2,7 +2,7 @@ ; ; Actuator Interface Card ; Encoder Native Methods -; Copyright (C) 2004 Walter Fetter Lages +; Copyright (C) 2004, 2005 Walter Fetter Lages ; ; This program is free software; you can redistribute it and/or modify ; it under the terms of the GNU General Public License as published by @@ -40,10 +40,6 @@ lib_init: clr a ; success ret -; data saved in big-endian mode -; Encoder base address -base: db 0,0,0 - ; Initializes Encoder ; private static native void initialize(int base); native_initialize: @@ -51,42 +47,30 @@ native_initialize: lcall Natlib_LoadPrimitive ; get parameter in R3:R2:R1:R0 - mov dptr,#ENCODER ; compute encoder address - lcall Add_Dptr1_24 ; dptr=dptr+R2:R1:R0 - - STORE24 #base ; store encoder address - - SET_STRETCH STRETCH + mov b,r3 ; SDCC parameter in b:dpx:dph:dpl + mov dpx,r2 ; BUG IN SDCC MANUAL! + mov dph,r1 + mov dpl,r0 - ENTER_PCE_SPACE_AND_PUSH_STATE #0 - clr a ; clear encoder - movx @dptr,a - EXIT_PCE_SPACE_AND_POP_STATE - RESTORE_STRETCH r0 + lcall encoder_initialize clr a ; sucess ret + ; Finalizes Encoder ; public native void finalize() native_finalize: + lcall encoder_finalize - ; fall-through to native_clear: + clr a ; success + ret ; Clears the counter on the quadrature decoder chip ; public void clear() -native_clear: - LOAD24 #base ; get encoder address - - SET_STRETCH STRETCH - - ENTER_PCE_SPACE_AND_PUSH_STATE #0 - clr a - movx @dptr,a ; clear counter - EXIT_PCE_SPACE_AND_POP_STATE - - RESTORE_STRETCH r0 +native_clear: + lcall encoder_clear clr a ; success ret @@ -96,63 +80,30 @@ native_clear: ; the counter was cleared ; public static native int getCount() native_getCount: - LOAD24 #base ; get encoder address - - SET_STRETCH STRETCH - - ENTER_PCE_SPACE_AND_PUSH_STATE #0 - movx a,@dptr ; get count high byte - mov r1,a ; return value in R3:R2:R1:R0 + lcall encoder_get_count ; SDCC returns value in b:dpx:dph:dpl - rlc a ; sign extension to 32 bits - clr a ; r1 > 0 extend with 0s - jnc posextend - cpl a ; r1 < 0 extend with 1s -posextend: mov r2,a ; return value in R3:R2:R1:R0 - mov r3,a + mov r0,dpl ; java native return value in R3:R2:R1:R0 + mov r1,dph + mov r2,dpx + mov r3,b - inc dptr - movx a,@dptr ; get count low byte - EXIT_PCE_SPACE_AND_POP_STATE - - RESTORE_STRETCH r0 - - mov r0,a ; return value in R3:R2:R1:R0 - - clr a ; success + clr a ; sucess ret -; Gets the count and clearsthe counter on the quadrature decoder chip +; Gets the count and clears the counter on the quadrature decoder chip ; return the number of pulses (in 2-complement) since the last time ; the counter was cleared ; public static native int getCountAndClear() native_getCountAndClear: - LOAD24 #base ; get encoder address - - SET_STRETCH STRETCH - - ENTER_PCE_SPACE_AND_PUSH_STATE #0 - movx a,@dptr ; get count high byte - mov r1,a ; return value in R3:R2:R1:R0 + lcall encoder_get_count_and_clear ; SDCC returns value in b:dpx:dph:dpl - rlc a ; sign extension to 32 bits - clr a ; r1 > 0 extend with 0s - jnc posextend1 - cpl a ; r1 < 0 extend with 1s -posextend1: mov r2,a ; return value in R3:R2:R1:R0 - mov r3,a + mov r0,dpl ; java native return value in R3:R2:R1:R0 + mov r1,dph + mov r2,dpx + mov r3,b - inc dptr - movx a,@dptr ; get count low byte - movx @dptr,a ; clear counter - EXIT_PCE_SPACE_AND_POP_STATE - - RESTORE_STRETCH r0 - - mov r0,a ; return value in R3:R2:R1:R0 - - clr a ; success + clr a ; sucess ret end diff --git a/motortst/motortst.cpp b/motortst/motortst.cpp index ff4da51..42a5071 100644 --- a/motortst/motortst.cpp +++ b/motortst/motortst.cpp @@ -73,19 +73,19 @@ int main(int argc,char *argv[]) aic->motor.on(); aic->brake.release(); - for(i=0;(i <= 12) && run;i++) + for(i=0;(i <= 24) && run;i++) { if(kbhit()) run=0; iter(aic,i); } while(run) { - for(i=12;(i > -12) && run;i--) + for(i=24;(i > -24) && run;i--) { if(kbhit()) run=0; iter(aic,i); } - for(i=-12;(i <= 12) && run;i++) + for(i=-24;(i <= 24) && run;i++) { if(kbhit()) run=0; iter(aic,i); diff --git a/trtai/PID/Makefile b/trtai/PID/Makefile deleted file mode 100644 index d917cc9..0000000 --- a/trtai/PID/Makefile +++ /dev/null @@ -1,44 +0,0 @@ -CLASSPATH=-classpath /opt/tini/bin/tini.jar:/opt/tini/bin/tiniclasses.jar -APIDBPATH=-d /opt/tini/bin/tini.db -TINIRTAIPATH=${HOME}/src/tini/rtai -LIBPATH=-PATH ${TINIRTAIPATH}/lib/rtai.jar -JAVAFLAGS=-O -target 1.1 -source 1.3 -ASMINC=-I/opt/tini/native/lib -ASMOPT=-f 1.14 -p 390 -l -ASMMACRO=/opt/tini/native/bin/Linux/macro -ASM=/opt/tini/native/bin/Linux/a390 -CC=sdcc -CFLAGS=-mTININative -S --use-accelerator -CINCLUDE=-I${TINIRTAIPATH}/include -CMPFLAGS=${CFLAGS} ${CINCLUDE} - -all: PID.tini - -PID.class: PID.java - javac ${CLASSPATH} ${JAVAFLAGS} PID.java - -pid.mpp: pid.a51 - ${ASMMACRO} ${ASMINC} -e- pid.a51 - -globals.mpp: globals.a51 - ${ASMMACRO} ${ASMINC} globals.a51 - -pid.tlib: pid.mpp globals.mpp ${TINIRTAIPATH}/lib/globals.mpp - cat pid.mpp ${TINIRTAIPATH}/lib/globals.mpp globals.a51 > tmp.mpp - ${ASM} ${ASMOPT} tmp.mpp - mv tmp.tlib pid.tlib - mv tmp.lst pid.lst - -pid.a51: pid.c - ${CC} ${CMPFLAGS} pid.c - -PID.tini: PID.class pid.tlib - java ${CLASSPATH} TINIConvertor -n pid.tlib -f PID.class ${APIDBPATH} -o PID.tini - - -clean: - rm -f *.bak *~ PID.class pid.tlib pid.mpp pid.lst pid.a51 globals.mpp tmp.mpp tmp.lst - -distclean: clean - rm -f PID.tini -