while(t <= tfms)
{
t=TINIOS.uptimeMillis()-t0;
- System.out.print("t="+t+" ms");
+ System.out.print("t="+t+"ms ");
}
System.out.println("...done.");
${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
*******************************************************************************/
-#include<aicio.h>
+#include <rtai_sched.h>
+#include <pthread.h>
+
+#include <aicio.h>
#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;
}
/* 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;
-}
-
braketest_time_1_1: db 0,0,0,0
+th: db 0,0,0,0
end
+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.
+++ /dev/null
-/******************************************************************************
-
- Actuator Interface Card
- Command Daemon
- Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
-
- 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 <http://www.gnu.org/licenses>.
-
-*******************************************************************************/
-
-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 <w.fetter@ieee.org>.\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();
- }
- }
-}
+++ /dev/null
-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
-
-
/******************************************************************************
Actuator Interface Card
- PID Daemon
- User Interface Module
- Copyright (C) 2004, 2005 Walter Fetter Lages <w.fetter@ieee.org>
+ Encoder Real Time Test
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
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
*******************************************************************************/
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)
{
}
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.");
+
}
}
--- /dev/null
+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
+
/******************************************************************************
Actuator Interface Card
- PID
- Real Time Module
+ Encoder Real Time Test
+ Real Time Module
Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software; you can redistribute it and/or modify
*******************************************************************************/
#include <rtai_sched.h>
-
#include <pthread.h>
-extern volatile long *output;
-extern volatile pthread_t th;
-extern volatile long *reference;
+#include <aicio.h>
+
+#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);
-}
/******************************************************************************
Actuator Interface Card
- PID
- User Interface Module
+ Encoder Real Time Test
Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
This program is free software; you can redistribute it and/or modify
*******************************************************************************/
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)
{
}
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.");
+
}
}
--- /dev/null
+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
+
/******************************************************************************
Actuator Interface Card
- PID Daemon
- Real Time Module
- Copyright (C) 2004, 2005 Walter Fetter Lages <w.fetter@ieee.org>
+ Encoder Real Time Test
+ Real Time Module
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
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
*******************************************************************************/
#include <rtai_sched.h>
-
#include <pthread.h>
-extern volatile long *output;
-extern volatile pthread_t th;
-extern volatile long *reference;
+#include <aicio.h>
+
+#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);
-}
${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
index: db 0,0,0,0
+th: db 0,0,0,0
end
at <http://www.gnu.org/licenses>.
*******************************************************************************/
+#include <rtai_sched.h>
+#include <pthread.h>
#include<aicio.h>
#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;
}
/* 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
{
lib \
AICOff \
BrakeTest \
+ BrakeRtTest \
CANTest \
EncoderTest \
+ EncoderRtTest \
+ EncoderRtRead \
IndexTest \
+ IndexRtTest \
IODaemon \
JointTest \
MotorReset \
MotorTest \
+ MotorRtTest \
+ PwmRtTest \
PID \
StrobeTest \
DoorDaemon \
- CmdDaemon \
aicd \
braketst \
encodertst \
--- /dev/null
+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
+
--- /dev/null
+/******************************************************************************
+
+ Actuator Interface Card
+ Motor Real Time Test
+ User Interface Module
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+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.");
+ }
+}
;******************************************************************************
;
; Actuator Interface Card
-; PID
+; Motor Real time Test
; Real Time Module Global Variables
; Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
;
;*******************************************************************************/
-output: db 0,0,0,0
th: db 0,0
-reference: db 0,0,0,0
+voltage: db 0,0,0,0
end
--- /dev/null
+/******************************************************************************
+
+ Actuator Interface Card
+ Motor Real Time Test
+ Real Time Module
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+#include <rtai_sched.h>
+#include <pthread.h>
+
+#include <aicio.h>
+
+#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;
+}
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
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
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
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.");
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
/******************************************************************************
Actuator Interface Card
- PID
+ PID
Real Time Module
Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
*******************************************************************************/
-#include<aicio.h>
+#include <rtai_sched.h>
+#include <pthread.h>
+
+#include <tmath.h>
+
+#include <aicio.h>
#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
inc dptr
mov a,r3
movx @dptr,a
+ clr a
_endasm;
}
+++ /dev/null
-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
-
+++ /dev/null
-/******************************************************************************
-
- Actuator Interface Card
- PID Daemon
- Real Time Module
- Copyright (C) 2004, 2005 Walter Fetter Lages <w.fetter@ieee.org>
-
- 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 <http://www.gnu.org/licenses>.
-
-*******************************************************************************/
-
-#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++;
-}
--- /dev/null
+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
+
--- /dev/null
+/******************************************************************************
+
+ Actuator Interface Card
+ PWM Real Time Test
+ User Interface Module
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+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.");
+ }
+}
--- /dev/null
+/******************************************************************************
+
+ Actuator Interface Card
+ PWM Real Time Test
+ Real Time Module
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+#include <rtai_sched.h>
+#include <pthread.h>
+
+#include <aicio.h>
+
+#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;
+}
+
+- 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.
@{
*/
+/** 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
* @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
* @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
*/
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
; 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
+
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
${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\
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}
--- /dev/null
+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:
--- /dev/null
+/******************************************************************************
+
+ Actuator Interface Card
+ Real Time Interface
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include <tmath.h>
+
+#define _AIC_C
+#include <aicio.h>
+
+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();
+}
+
--- /dev/null
+;******************************************************************************
+;
+; Actuator Interface Card
+; Brake Functions
+; Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+;
+; 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 <http://www.gnu.org/licenses>.
+;
+;*******************************************************************************
+
+$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
--- /dev/null
+;******************************************************************************
+;
+; Actuator Interface Card
+; Encoder Functions
+; Copyright (C) 2004, 2005 Walter Fetter Lages <w.fetter@ieee.org>
+;
+; 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 <http://www.gnu.org/licenses>.
+;
+;*******************************************************************************
+
+$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
+
--- /dev/null
+/******************************************************************************
+
+ Actuator Interface Card
+ Encoder Real Time Interface
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include <tmath.h>
+
+#define _ENCODER_C
+#include <aicio.h>
+
+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());
+}
--- /dev/null
+;******************************************************************************
+;
+; Actuator Interface Card
+; Index Function
+; Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+;
+; 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 <http://www.gnu.org/licenses>.
+;
+;*******************************************************************************
+
+$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
+
;******************************************************************************
;
; Actuator Interface Card
-; PID Daemon
-; Real Time Module Global Variables
-; Copyright (C) 2004, 2005 Walter Fetter Lages <w.fetter@ieee.org>
+; Motor Real Time Interface
+; Global Variables
+; Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
;
; 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
;*******************************************************************************/
-output: db 0,0,0,0
-th: db 0,0
-reference: db 0,0,0,0
+motor_volt2duty: db 0,0,0,0
end
--- /dev/null
+/******************************************************************************
+
+ Actuator Interface Card
+ Motor Real Time Interface
+ Copyright (C) 2005 Walter Fetter Lages <w.fetter@ieee.org>
+
+ 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 <http://www.gnu.org/licenses>.
+
+*******************************************************************************/
+
+#include <tmath.h>
+
+#define _MOTOR_C
+#include <aicio.h>
+
+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;
+}
+
--- /dev/null
+;******************************************************************************
+;
+; Actuator Interface Card
+; PWM Functions
+; Copyright (C) 2004, 2005 Walter Fetter Lages <w.fetter@ieee.org>
+;
+; 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 <http://www.gnu.org/licenses>.
+;
+;*******************************************************************************
+
+$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
+
$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
; 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
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
${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
;
; Actuator Interface Card
; Encoder Native Methods
-; Copyright (C) 2004 Walter Fetter Lages <w.fetter@ieee.org>
+; Copyright (C) 2004, 2005 Walter Fetter Lages <w.fetter@ieee.org>
;
; 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
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:
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
; 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
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);
+++ /dev/null
-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
-
projects / aic.git / commitdiff