├── .gitignore ├── README.md ├── as ├── Makefile ├── README.md ├── avr.inc └── smc3.sx ├── atmega ├── Makefile ├── avr.inc ├── inc │ └── m328Pdef.inc └── smc3.asm ├── chan ├── smc │ ├── avr.inc │ ├── smc.asm │ └── smc2.asm └── smc3 │ ├── AVR.INC │ ├── SMC3.ASM │ ├── SMC3.TXT │ ├── SMC3A.ASM │ └── SMC3A.TXT └── cport ├── Makefile ├── cli.c ├── cli.h ├── control.c ├── control.h ├── csmc3.h ├── csmc3.ino ├── eeprom.c ├── eeprom.h ├── encoder.c ├── encoder.h ├── motion-planning.c ├── motion-planning.h ├── mul.c ├── mul.h ├── not_ready ├── display.c └── display.h ├── uart.cpp └── uart.h /.gitignore: -------------------------------------------------------------------------------- 1 | # backup files 2 | *~ 3 | *.bak 4 | # developer notes and scratchpad directory 5 | notes/ 6 | # quality control files 7 | *.bin 8 | *.disass 9 | # assember generated files 10 | build*/ 11 | *.cof 12 | *.hex 13 | *.lst 14 | *.map 15 | *.obj 16 | *.d 17 | *.i 18 | *.o 19 | *.s 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | This is a port of the very sophisticated [DC Servomotor Controller 2 | SMC3](http://elm-chan.org/works/smc/report_e.html) by Elm Chan for the 3 | ATmega328 as used with the popular Arduino Uno and Arduino Nano boards. 4 | 5 | It includes the results of a detailed analysis of the inner workings of the 6 | source code and (hopefully) more beginner-friendly explanation of the 7 | meaning of the control parameter and how to choose sensible values for the 8 | desired application. 9 | 10 | This analysis includes a full translation of the assembler code into C code. 11 | This was started as a tool to understand the inner workings of SMC3 but it 12 | is turning into a full project of its own right now. 13 | 14 | The program logic is as close to the carefully optimized assembler version 15 | as possible, but it is much easier to read than the highly optimized 16 | hand-crafted assembler masterpiece by Elm Chan. 17 | 18 | It relies on the Arduino environment for the UART part and some 19 | initializations, but the resulting code behaves identical to the original 20 | version. The main point is to show the internal logic and to be a more 21 | accessible base for modifications. The price for this convienence is a much 22 | bigger binary size: 4.8kB instead of 1.8kB. 23 | 24 | 25 | 26 | 27 | ## Features of SMC3 28 | 29 | - Originally written for the (now obsolete) ATtiny2313 microcontroller and 30 | now ported to the ATmega328. 31 | - accepts serial command via UART 32 | - Direct Access to Position Command Register 33 | - G0/G1 Motion Generation 34 | - Three Control Modes 35 | - Absolute Positioning Mode 36 | - Constant Speed Mode 37 | - Constant Torque Mode 38 | - Software Decoding of Quadrature Encoder Input 39 | - Count rate over 100,000 counts/sec in quadrature decoding 40 | - No External Component Required 41 | - Two PWM Outputs for H-bridge Driver 42 | - Supports bootstrap type FET driver (minimum duty ratio 15/16) 43 | - Three Diagnostic Outputs 44 | - Ready, Torque Limit and Servo Error 45 | - On-the-Fly Servo Tuning 46 | 47 | 48 | 49 | The version *SMC3A* is optimized to be a drop-in replacement for stepper motor 50 | drivers: (but not ported yet). Instead of the UART interface it supports: 51 | 52 | - Pulse/Dir Command Input with Pulse Multiplyer 53 | - Pulse rate over 100,000 pulses/sec 54 | - Gear ratio from 1/256 to 255 in 1/256 step 55 | 56 | 57 | Differences of this version to the original: 58 | 59 | - ported to ATmega328, should be easy to adopt for other ATmega CPU 60 | - optional decay function for the integral sum to avoid unnecessary motor 61 | current after the target position is reached. 62 | - no support for the proprietary serial position display on the SPI pins. 63 | 64 | 65 | 66 | ## Compiling 67 | 68 | Install avra (The syntax is not compatible with avr-as): 69 | 70 | sudo apt install avra 71 | 72 | compile: 73 | 74 | make 75 | 76 | upload/flash to an Arduino Uno using avrdude: 77 | 78 | make upload 79 | 80 | Or more in detail: 81 | 82 | avra -I inc smc3.asm 83 | avrdude -c arduino -p m328p -U flash:w:smc3.hex -P /dev/ttyUSB0 84 | 85 | The avra package on Debian-based systems (Ubuntu, Mint etc) is very, very 86 | old and misses the required CPU definitions for the ATmega328. That's why m328Pdef.inc 87 | is included in this repository. 88 | 89 | To compile the original files (they were written for the orignal AVR 90 | assembler, but [avra](http://avra.sourceforge.net/) is almost 100% 91 | compatible): 92 | 93 | avra -I /usr/share/avra smc3.asm 94 | 95 | 96 | 97 | ## Pin connections 98 | 99 | Signal |Pin ATtiny2313 |Pin m328 |Arduino Uno pin number 100 | ------ |-------- |----------- |--------- 101 | Enc_A |PD5/T1 |PD5/T1 |5 102 | Enc_B |PD4/T0 |PD4/T0 |4 103 | Enc_Z |PD3/INT1 |PD3/INT1 |3 104 | nSTEP |PD2/INT0 |PD2/INT0 |2 105 | DIR |PD6 |PD6/OC0A |6 106 | TxD |PD1/TXD |PD1/TXD |1 107 | RxD |PD0/RXD |PD0/RXD |0 108 | SCK (Clk) |PB7/SCK/USCK | PB5/SCK |13 109 | MISO (SegData) |PB6/MISO/DO | PB3/MOSI |11 (intentionally swapped) 110 | MOSI (DigitClk) |PB5/MOSI/DI | PB4/MISO |12 (intentionally swapped) 111 | Mot_n |PB4/OC1B | PB2/OC1B |10 112 | Mot_p |PB3/OC1A | PB1/OC1A |9 113 | LED_Ready |PB2/OC0A |PC2 |16 (flexible) 114 | LED_TorqueLimit |PB1 |PC3 |17 (flexible) 115 | LED_ServoError |PB0 |PB0 |8 (flexible) 116 | RESn |PA2/RES |PC6/RES | 117 | Xtal2 |PA1/X2 | | 118 | Xtal1 |PA0/X1 | | 119 | 120 | The three LED connections can be easily reconfigured in the source code. All 121 | other connections are _very hard_-coded and would require major changes. 122 | 123 | 124 | 125 | ## Usage 126 | 127 | Connect the position encoder and the motor driver, start a serial terminal 128 | with 38400N1. Check if the motor runs on a 50% PWM signal: 129 | 130 | SMC type 3 131 | 132 | %m0 133 | %s128 134 | 135 | Turn off the motor: 136 | 137 | %s0 138 | 139 | The higher modes require the definition of some motor control parameters. 140 | These values depend on the motor, the supply voltage, the encoder and the 141 | masses involved. 142 | 143 | Follow [this guide in the project 144 | wiki](https://github.com/tenbaht/servo-motor-controller/wiki/Meaning-of-the-parameters) 145 | on how to measure and calculate them. Once you have a set of numbers define 146 | and save it: 147 | 148 | %p0 20 149 | %p1 256 150 | %p2 2560 151 | %p3 13 152 | %p4 240 153 | %p5 9 154 | %p6 4480 155 | %p7 16 156 | %w0 157 | 158 | These values are stored in EEPROM and can be recalled any time by 159 | 160 | %r0 161 | 162 | Now let's move! But be careful: This can physically crash your machine if 163 | the speed and torque limits or the feedback values are incorrectly set! 164 | 165 | Move to position 1000 following the speed ramp defined by P6 166 | and P7: 167 | 168 | g0 1000 169 | 170 | Move to position 100 at a constant speed of 10: 171 | 172 | g1 100 10 173 | 174 | Jump to 500 at maximum speed (P0) and acceleration (only limited by the 175 | current/torque limit P4): 176 | 177 | j 500 178 | 179 | 180 | ## Connection to the real world 181 | 182 | Positions are in encoder counts and speeds are in encoder counts per 183 | millisecond. A linear encoder strip from an old DeskJet printer with 150 lpi 184 | (lines per inch) gives 600 encoder counts per inch = 42.3um/count. 185 | 186 | - Position 1000 is at 42.3mm 187 | - Speed 10 cnt/ms is 10000 cnt/s = 423mm/s. 188 | 189 | For rotational encoders it all scales to rounds and rpm or rps instead of 190 | distance and distance/time. 191 | 192 | 193 | 194 | ## Position display 195 | 196 | The original source supports an optional 7-segment display with 8 digits to 197 | show the current position. The supported serial protocol (see [Chan Elm's 198 | page](http://elm-chan.org/docs/avr/avrisp.html) for details) is unfortunatly 199 | not compatible with the cheap serial LED modules you can find on Aliexpress. 200 | 201 | Since this is not an off-the-shelf standard display anyway I didn't bother 202 | to port that code (yet?) and swopped these two serial pins for the Arduino 203 | pin definition in a way that the builtin SPI could be used. 204 | 205 | 206 | 207 | ## CPU 208 | 209 | The AT90S2313 got replaced by the ATtiny2313 a long time ago. Both are very 210 | similar, only some register names changed slightly and the interrupt table 211 | got a little bigger. See Atmel/Microchip AVR091 for details on migration. 212 | 213 | Today I would choose one of the more modern CPUs in the 40 or 50 cent class 214 | like ATtiny404, ATtiny44 or ATtiny45. 4kB flash should even allow for C code 215 | and the hardware multiplier would help as well. 216 | 217 | Using an ATmega328 is ridiculous overkill, but that's what I have laying 218 | around in my drawer... 219 | 220 | 221 | 222 | 223 | ## Files in this repository 224 | 225 | _atmega/_: the ATmega port of SMC3 226 | 227 | _cport/_: A translation of the the assembler code into valid C code. The 228 | program logic is as close to the carefully optimized assembler version as 229 | possible. It compiles and the resulting code behaves identical to the 230 | original version. The main point is to show the internal logic and to be a 231 | more accessible base for modifications. The price for this convienence is a 232 | much bigger binary size: 4.8kB instead of 1.8kB. Carefully crafted assembler 233 | code can do real magic. 234 | 235 | The original archives are unpacked for reference into chan/smc and chan/smc3. 236 | 237 | _chan/smc/smc.asm_: v0.2 of the motor controller (AT90S2313) 238 | 239 | _chan/smc/smc2.asm_: v0.2 of the motor controller with position display on a 240 | 7-segment display connected via SPI to the ISP connector. (AT90S2313) 241 | 242 | _chan/smc3/SMC3.ASM_: v0.3, ATtiny2313. Supports serial interface, but not the 243 | stepper-like dir/step input and no LED display. 'E' command not implemented, 244 | echo always on. 245 | 246 | _chan/smc3/SMC3A.ASM_: v0.3a, ATtiny2313. Supports the stepper-like dir/step 247 | interface, but no serial interface and no LED display. 248 | 249 | 250 | 251 | ## Further reading 252 | 253 | - [Internal operations of SCM3](https://github.com/tenbaht/servo-motor-controller/wiki) 254 | in the project wiki 255 | - [ATtiny2313 data sheet](http://ww1.microchip.com/downloads/en/DeviceDoc/doc8246.pdf) 256 | - AVR091 Replacing AT90S2313 by ATtiny2313.pdf 257 | - [AVR Assember user guide](http://ww1.microchip.com/downloads/en/devicedoc/40001917a.pdf) 258 | (was known as Atmel doc1022 before the merger with Microchip) 259 | - Theory of sensorless speed control of brushed DC motors: [AB-026 : 260 | Sensorless Speed Stabiliser for a DC 261 | Motor](https://www.precisionmicrodrives.com/content/ab-026-sensorless-speed-stabiliser-for-a-dc-motor/) 262 | by Precision Microdrives. 263 | 264 | 265 | ## Licence 266 | 267 | The original work was released under the [Creative Commons Attribution 3.0 268 | Unported](https://creativecommons.org/licenses/by/3.0/) (CC-BY 3.0) by Elm 269 | Chan. 270 | -------------------------------------------------------------------------------- /as/Makefile: -------------------------------------------------------------------------------- 1 | MCU=atmega328 2 | FORMAT=ihex 3 | OBJCOPY=avr-objcopy 4 | 5 | .PHONY: all clean upload eeprom list disass 6 | .SUFFIXES: .elf .hex .eep.hex .lss .sym 7 | 8 | all: smc3.hex smc3.eep.hex 9 | 10 | # hex file for use with avrdude 11 | .elf.hex: 12 | $(OBJCOPY) -O $(FORMAT) -R .eeprom $< $@ 13 | 14 | # eeprom content for avrdude. eeprom upload is not supported by the 15 | # Arduino bootloader, it requires an ISP connection. 16 | .elf.eep.hex: 17 | -$(OBJCOPY) -j .eeprom --set-section-flags=.eeprom="alloc,load" \ 18 | --change-section-lma .eeprom=0 -O $(FORMAT) $< $@ 19 | 20 | # link 21 | smc3.elf: smc3.o 22 | avr-gcc -mmcu=$(MCU) -nostartfiles -nostdlib -nodefaultlibs $^ -o $@ 23 | 24 | 25 | # compile 26 | smc3.o: smc3.sx avr.inc 27 | avr-gcc -mmcu=$(MCU) -Wa,-I/usr/lib/avr/include -c $< 28 | 29 | # debug helpers: 30 | # - a readable assembler listing to stdout 31 | list: smc3.o 32 | avr-objdump -dt smc3.o 33 | 34 | # - a disassembly from the hex file (not the obj file) 35 | # This is very useful for bytewise comparision with the avra output. 36 | disass: smc3.hex 37 | avr-objdump -j .sec1 -m avr5 -d smc3.hex > smc3.disass 38 | 39 | 40 | # upload flash only 41 | upload: smc3.hex 42 | avrdude -c arduino -p m328p -U flash:w:$< 43 | 44 | eeprom: smc3.eep.hex 45 | avrdude -c arduino -p m328p -U eeprom:w:$< 46 | 47 | 48 | clean: 49 | rm -rf *~ *.bak *.o *.hex build 50 | -------------------------------------------------------------------------------- /as/README.md: -------------------------------------------------------------------------------- 1 | # Compiling smc3 using avr-as 2 | 3 | smc3 was written for the original AVR assembler (compatible to avra). 4 | Unfortunatly, the syntax differs slightly from avr-as. 5 | 6 | The main advantage of avr-as over avra is the possibility to generate 7 | linkable .o object files that can be mixed with C files. So I thought 8 | converting the code for avr-as would be a useful intermediate step on the 9 | way to embedd smc3 in other C-based projects. 10 | 11 | Unfortunatly, it turns out that the syntax differences are big enough to 12 | make the conversion of source code a non-trivial task and it took way longer 13 | than expected. 14 | 15 | I summed up my findings about [migrating an avra project to 16 | avr-as](https://tenbaht.github.io/posts/migrating-from-avra-to-avr-as/) and 17 | hope that can be useful to someone. 18 | 19 | 20 | 21 | ## Compile 22 | 23 | make 24 | 25 | Or more in detail: 26 | 27 | avr-gcc -mmcu=atmega328 -Wa,-I/usr/lib/avr/include -c smc3.sx 28 | avr-gcc -mmcu=atmega328 -nostartfiles -nostdlib -nodefaultlibs smc3.o -o smc3.elf 29 | avr-objcopy -O ihex -R .eeprom smc3.elf smc3.hex 30 | avr-objcopy -j .eeprom --set-section-flags=.eeprom="alloc,load" \ 31 | --change-section-lma .eeprom=0 -O ihex smc3.elf smc3.eep.hex 32 | 33 | 34 | ## Quality control 35 | 36 | The goal was to convert the source code in a way that it results in exactly 37 | the same binary. For checking on this it was neccessary to normalize the 38 | output files of avra and as into a common format that can be diff'ed 39 | efficiently. 40 | 41 | 42 | avra saves the interrupt vectors separately in individual pseudo-sections: 43 | 44 | ``` 45 | $ objdump -h smc3.hex 46 | 47 | smc3.hex: file format ihex 48 | 49 | Sections: 50 | Idx Name Size VMA LMA File off Algn 51 | 0 .sec1 00000004 00000000 00000000 00000011 2**0 52 | CONTENTS, ALLOC, LOAD 53 | 1 .sec2 00000004 00000038 00000038 00000026 2**0 54 | CONTENTS, ALLOC, LOAD 55 | 2 .sec3 00000004 00000048 00000048 0000003b 2**0 56 | CONTENTS, ALLOC, LOAD 57 | 3 .sec4 00000718 00000068 00000068 00000050 2**0 58 | CONTENTS, ALLOC, LOAD 59 | ``` 60 | 61 | This can be normalized by converting to a binary image and back: 62 | 63 | objcopy -I ihex -O binary smc3.hex smc3.bin 64 | objcopy -O ihex -I binary smc3.bin normal-avra.hex 65 | 66 | This can be used to produce a normalized disassembler listing: 67 | 68 | avr-objdump -j .sec1 -m avr5 -d -I ihex normal-avra.hex > avra.disass 69 | 70 | A similar listing can be produced for the as source. Both are normalized and 71 | can be compared using the usual text file comparision tools: 72 | 73 | make disass 74 | diffuse smc3.disass ../atmega/avra.disass 75 | 76 | 77 | 78 | ## Modifing the avra source for use with avr-as 79 | 80 | These quick notes summarize the steps to initially convert the source code. 81 | 82 | 83 | 84 | ### Modifications to the main file smc3.asm 85 | 86 | Remove `.equ`, only keep `var = val`: 87 | 88 | sed -i -e "s/^\.equ[ \t]*//" smc3.asm 89 | 90 | Replace `.def name = val` by `#define name val`: 91 | 92 | sed -i -e "/^\.def/ s/=[ \t]*//" -e "s/\.def/#define/" smc3.asm 93 | 94 | Make sure all comments in `#define` lines are C comments: 95 | 96 | sed -i -e "/^#define/ s#;#//#" 97 | 98 | Optional: replace `.ifdef` by `#ifdef`. This requires hand-editing later. 99 | Sometimes one is better than the other in a particular situation: 100 | 101 | sed -i -e "s/\.if/#if/" -e "s/\.endif/#endif/" -e "s/\.el/#el/" smc3.asm 102 | 103 | replace the section specifier: 104 | 105 | .eseg .section eeprom 106 | .dseg .section bss 107 | .cseg .text 108 | .byte .fill 109 | 110 | The easiest is using sed again: 111 | 112 | sed -i -e "s/\.eseg/\.section eeprom/" \ 113 | -e "s/\.dseg/\.section bss/" \ 114 | -e "s/\.cseg/\.text/" smc3.asm 115 | 116 | The remaining changes can't be automated. 117 | 118 | 119 | 120 | #### Relocate negative values in a different segment 121 | 122 | Requires brackets around the variable name to separate it from the minus 123 | sign. Otherwise the assembler wouldn't choose the required relocation type 124 | R_AVR_LO8_LDI_NEG. 125 | 126 | subiw YL, -Parms ; works only for avra 127 | subiw YL, -(Parms) ; works for both 128 | 129 | Strange, isn't it? Is this an assembler bug? 130 | 131 | 132 | #### Pointer into flash 133 | 134 | Since avr-as calculates flash addresses in bytes rather than word, the 135 | pointer values don't need to be multiplied by two anymore to access the stored 136 | strings. 137 | 138 | 139 | #### Program-counter relative calculations 140 | 141 | Replace all PC-relative branch specifiers by labels. 142 | [Here](https://tenbaht.github.io/posts/migrating-from-avra-to-avr-as/#program-counter) 143 | is why. 144 | 145 | 146 | 147 | ### Modifications to avr.inc 148 | 149 | Massive. And most of it can't be automated. And the result needs to be 150 | included with `#include`, because it contains `#define`s that has to be 151 | evaluated at preprocessing time. 152 | 153 | Don't change the .if/.endif, because this time they need to evaluated at 154 | assembly time. 155 | 156 | At least the .equ and .def replacements work the same way as before: 157 | 158 | sed -e "s/^\.equ[ \t]*//" -e "/^\.def/ s/=[ \t]*//" -e "s/\.def/#define/" ../atmega/avr.inc > avr.inc 159 | 160 | Add some compatibility definitions: 161 | 162 | #define low(X) lo8(X) 163 | #define high(X) hi8(X) 164 | #define RAMTOP RAMSTART 165 | #define EEPROMEND E2END 166 | #define __FLASH_SIZE__ FLASHEND 167 | #define INT_VECTORS_SIZE _VECTORS_SIZE 168 | 169 | #define A AL 170 | #define B BL 171 | #define C CL 172 | #define D DL 173 | #define T0 T0L 174 | #define T2 T2L 175 | #define T4 T4L 176 | 177 | There might be weird problems with nested expressions like _low(low(val))_. 178 | 179 | Adopting the syntax of macro parameters from @0/@0L/@0H to \par0/\par0/\par0+1 180 | like this is more complex, see 181 | [here](view-source:https://tenbaht.github.io/posts/migrating-from-avra-to-avr-as/#macros-and-preprocessor-constants) 182 | 183 | .macro ldiw par0,par1 184 | ldi \par0,low(\par1) 185 | ldi \par0+1,high(\par1) 186 | .endm 187 | 188 | This was a try to convert from @0/@0L to `\par0\()L` and `\par0\()H`: 189 | 190 | sed -e 's/@\([0-9]\)\([HL]\)/@par\1\\\(\)\2/g' -e 's/@\([0-9]\)/@par\1/g' avr.inc 191 | 192 | That's quite a regular expression, isn't it? But it doesn't help, the L/H 193 | syntax doesn't work anyway. (Again, it would be required at preprocessing 194 | time) 195 | 196 | It would be possible to use C macros. They would require braches around the 197 | arguments, but it could work. The preprocessor outputs the whole macro 198 | content as one line, but '$' can be used to [mark the intentend line 199 | breaks](https://sourceware.org/binutils/docs-2.21/as/AVR_002dChars.html ) 200 | for the assembler. 201 | -------------------------------------------------------------------------------- /as/avr.inc: -------------------------------------------------------------------------------- 1 | #define low(X) lo8(X) 2 | #define high(X) hi8(X) 3 | 4 | ;------------------------------------------------; 5 | ; Constants 6 | 7 | ;RAMTOP = RAMSTART ; SRAM top address 8 | #define RAMTOP RAMSTART 9 | #define EEPROMEND E2END 10 | #define __FLASH_SIZE__ FLASHEND 11 | #define INT_VECTORS_SIZE _VECTORS_SIZE 12 | 13 | bit0 = 0b00000001 14 | bit1 = 0b00000010 15 | bit2 = 0b00000100 16 | bit3 = 0b00001000 17 | bit4 = 0b00010000 18 | bit5 = 0b00100000 19 | bit6 = 0b01000000 20 | bit7 = 0b10000000 21 | 22 | ; this can't be done using assembler assigns 23 | #define r0 0 24 | #define r1 1 25 | #define r2 2 26 | #define r3 3 27 | #define r4 4 28 | #define r5 5 29 | #define r6 6 30 | #define r7 7 31 | #define r8 8 32 | #define r9 9 33 | #define r10 10 34 | #define r11 11 35 | #define r12 12 36 | #define r13 13 37 | #define r14 14 38 | #define r15 15 39 | #define r16 16 40 | #define r17 17 41 | #define r18 18 42 | #define r19 19 43 | #define r20 20 44 | #define r21 21 45 | #define r22 22 46 | #define r23 23 47 | #define r24 24 48 | #define r25 25 49 | #define r26 26 50 | #define r27 27 51 | #define r28 28 52 | #define r29 29 53 | #define r30 30 54 | #define r31 31 55 | 56 | ; these could use assembler assigns 57 | #define T0L r0 58 | #define T0H r1 59 | #define T2L r2 60 | #define T2H r3 61 | #define T4L r4 62 | #define T4H r5 63 | #define T6L r6 64 | #define T6H r7 65 | #define T8L r8 66 | #define T8H r9 67 | #define T10L r10 68 | #define T10H r11 69 | #define T12L r12 70 | #define T12H r13 71 | #define T14L r14 72 | #define T14H r15 73 | 74 | 75 | #define AL r16 76 | #define AH r17 77 | #define BL r18 78 | #define BH r19 79 | #define CL r20 80 | #define CH r21 81 | #define DL r22 82 | #define DH r23 83 | #define EL r24 84 | #define EH r25 85 | 86 | #define A AL 87 | #define B BL 88 | #define C CL 89 | #define D DL 90 | //#define X XL 91 | //#define Y YL 92 | //#define Z ZL 93 | #define T0 T0L 94 | #define T2 T2L 95 | #define T4 T4L 96 | 97 | ;------------------------------------------------; 98 | ; Push/Pop register pair 99 | ; 100 | ; pushw Z 101 | 102 | .macro pushw reg 103 | push \reg+1 104 | push \reg 105 | .endm 106 | 107 | .macro popw reg 108 | pop \reg 109 | pop \reg+1 110 | .endm 111 | 112 | 113 | ;------------------------------------------------; 114 | ; Load/store word from/to direct memory/immediate 115 | ; 116 | ; ldsw Z,mem 117 | ; ldiw Z,imm 118 | 119 | .macro xldiw par0,par1 120 | ldi \par0,16 121 | ldi r17,16 122 | .endm 123 | 124 | .macro ldiw reg,val 125 | ldi \reg,lo8(\val) 126 | ldi \reg+1,hi8(\val) 127 | .endm 128 | 129 | .macro ldsw par0,par1 ; 130 | lds \par0,\par1 131 | lds \par0+1,\par1+1 132 | .endm 133 | 134 | .macro stsw par0,par1 135 | sts \par0+1,\par1\()H 136 | sts \par0,\par1\()L 137 | .endm 138 | 139 | .macro lddw reg,val 140 | ldd \reg,\val 141 | ldd \reg+1,\val+1 142 | .endm 143 | 144 | .macro stdw disp,reg 145 | std \disp+1,\reg+1 146 | std \disp,\reg 147 | .endm 148 | 149 | .macro ldw par0,par1 150 | ld \par0\()L,\par1 151 | ld \par0\()H,\par1 152 | .endm 153 | 154 | .macro stw par0,par1 155 | st \par0,\par1\()L 156 | st \par0,\par1\()H 157 | .endm 158 | 159 | .macro inw par0,par1 160 | .if \par0\()L < 0x40 161 | in \par0\()L,\par1\()L 162 | in \par0\()H,\par1\()H 163 | .else 164 | lds \par0\()L,\par1\()L 165 | lds \par0\()H,\par1\()L 166 | .endif 167 | .endm 168 | 169 | .macro outw port,reg 170 | .if \port < 0x40 171 | out \port+1,\reg+1 172 | out \port,\reg 173 | .else 174 | sts \port+1,\reg+1 175 | sts \port,\reg 176 | .endif 177 | .endm 178 | 179 | 180 | ;------------------------------------------------; 181 | ; Store immediate into indirect memory via r16 182 | ; 183 | ; sti Z,imm 184 | ; stdi Z+d,imm 185 | 186 | .macro sti par0,par1 187 | ldi r16,\par1 188 | st \par0,r16 189 | .endm 190 | 191 | .macro stdi par0,par1 192 | ldi r16,\par1 193 | std \par0,r16 194 | .endm 195 | 196 | .macro muli par0,par1 197 | ldi r16,\par1 198 | mul \par0,r16 199 | .endm 200 | 201 | 202 | ;------------------------------------------------; 203 | ; add/sub/subc/cp/cpc/lsl/lsr/rol/ror to register pair 204 | ; 205 | 206 | ; this does not work for addresses, avr-as can't relocate the -\val. 207 | .macro addiw reg,val 208 | subi \reg,lo8(-(\val)) ; the brackets are required by gas 209 | sbci \reg+1,hi8(-(\val)) ; to be able to relocate the value 210 | .endm 211 | 212 | .macro subiw reg,val ; 213 | subi \reg,lo8(\val) 214 | sbci \reg+1,hi8(\val) 215 | .endm 216 | 217 | .macro addw par0,par1 ; 218 | add \par0,\par1 219 | adc \par0+1,\par1+1 220 | .endm 221 | 222 | .macro adcw par0,par1 223 | adc \par0\()L,\par1\()L 224 | adc \par0\()H,\par1\()H 225 | .endm 226 | 227 | .macro subw par0,par1 ; 228 | sub \par0,\par1 229 | sbc \par0+1,\par1+1 230 | .endm 231 | 232 | .macro sbcw par0,par1 233 | sbc \par0\()L,\par1\()L 234 | sbc \par0\()H,\par1\()H 235 | .endm 236 | 237 | .macro cpw par0,par1 ; 238 | cp \par0,\par1 239 | cpc \par0+1,\par1+1 240 | .endm 241 | 242 | .macro cpcw par0,par1 243 | cpc \par0\()L,\par1\()L 244 | cpc \par0\()H,\par1\()H 245 | .endm 246 | 247 | .macro cpiw par0,par1 248 | cpi \par0\()L,low(\par1) 249 | ldi r16,high(\par1) 250 | cpc \par0\()H,r16 251 | .endm 252 | 253 | .macro andw par0,par1 254 | and \par0\()L,\par1\()L 255 | and \par0\()H,\par1\()H 256 | .endm 257 | 258 | .macro andiw par0,par1 259 | andi \par0\()L,low(\par1) 260 | andi \par0\()H,high(\par1) 261 | .endm 262 | 263 | .macro orw par0,par1 264 | or \par0\()L,\par1\()L 265 | or \par0\()H,\par1\()H 266 | .endm 267 | 268 | .macro oriw par0,par1 269 | ori \par0\()L,low(\par1) 270 | ori \par0\()H,high(\par1) 271 | .endm 272 | 273 | .macro lslw par0 ; 274 | lsl \par0 275 | rol \par0+1 276 | .endm 277 | 278 | .macro lsrw par0 279 | lsr \par0\()H 280 | ror \par0\()L 281 | .endm 282 | 283 | .macro asrw par0 ; 284 | asr \par0+1 285 | ror \par0 286 | .endm 287 | 288 | .macro rolw par0 ; 289 | rol \par0 290 | rol \par0+1 291 | .endm 292 | 293 | .macro rorw par0 ; 294 | ror \par0+1 295 | ror \par0 296 | .endm 297 | 298 | .macro clrw par0 ; 299 | clr \par0 300 | clr \par0+1 301 | .endm 302 | 303 | .macro comw par0 304 | com \par0\()L 305 | com \par0\()H 306 | .endm 307 | 308 | .macro negw par0 ; 309 | com \par0+1 310 | neg \par0 311 | brne end\@ 312 | inc \par0+1 313 | end\@: 314 | .endm 315 | 316 | .macro movew par0,par1 317 | mov \par0\()L, \par1\()L 318 | mov \par0\()H, \par1\()H 319 | .endm 320 | 321 | .macro lpmw par0,par1 322 | lpm \par0\()L, \par1 323 | lpm \par0\()H, \par1 324 | .endm 325 | 326 | 327 | ;------------------------------------------------; 328 | ; Store immediate into direct memory via r16 329 | ; 330 | ; stsi var,imm 331 | 332 | .macro stsi par0,par1 333 | ldi r16,\par1 334 | sts \par0,r16 335 | .endm 336 | 337 | 338 | ;------------------------------------------------; 339 | ; Output port immediate via r16 340 | ; 341 | ; outi port,var 342 | 343 | .macro outi port,var 344 | .if \port < 0x40 345 | ldi r16,\var 346 | out \port,r16 347 | .else 348 | ldi r16,\var 349 | sts \port,r16 350 | .endif 351 | .endm 352 | 353 | 354 | ;------------------------------------------------; 355 | ; Add immediate to register 356 | 357 | .macro addi par0,par1 358 | subi \par0,-(\par1) 359 | .endm 360 | 361 | 362 | ;------------------------------------------------; 363 | ; Long branch 364 | 365 | .macro rjne par0 366 | breq end\@ 367 | rjmp \par0 368 | end\@: 369 | .endm 370 | 371 | .macro rjeq par0 372 | brne end\@ 373 | rjmp \par0 374 | end\@: 375 | .endm 376 | 377 | #if 0 378 | .macro rjcc par0 379 | brcs PC+2 380 | rjmp \par0 381 | .endm 382 | 383 | .macro rjcs par0 384 | brcc PC+2 385 | rjmp \par0 386 | .endm 387 | 388 | .macro rjtc par0 389 | brts PC+2 390 | rjmp \par0 391 | .endm 392 | 393 | .macro rjts par0 394 | brtc PC+2 395 | rjmp \par0 396 | .endm 397 | 398 | .macro rjge par0 399 | brlt PC+2 400 | rjmp \par0 401 | .endm 402 | 403 | .macro rjlt par0 404 | brge PC+2 405 | rjmp \par0 406 | .endm 407 | 408 | 409 | .macro retcc 410 | brcs PC+2 411 | ret 412 | .endm 413 | 414 | .macro retcs 415 | brcc PC+2 416 | ret 417 | .endm 418 | 419 | .macro reteq 420 | brne PC+2 421 | ret 422 | .endm 423 | 424 | .macro retne 425 | breq PC+2 426 | ret 427 | .endm 428 | #endif 429 | ;------------------------------------------------; 430 | ; Move single bit between two registers 431 | ; 432 | ; bmov dstreg,dstbit,srcreg.srcbit 433 | 434 | .macro movb par0,par1,par2,par3 435 | bst \par2,\par3 436 | bld \par0,\par1 437 | .endm 438 | 439 | 440 | -------------------------------------------------------------------------------- /as/smc3.sx: -------------------------------------------------------------------------------- 1 | ;----------------------------------------------------------------------------; 2 | ; AVR SERVOMOTOR CONTROLLER R0.3 (C)ChaN, 2004 ; 3 | ;----------------------------------------------------------------------------; 4 | ; 5 | 6 | .nolist 7 | #define __SFR_OFFSET 0 8 | #include "avr/io.h" 9 | ;.include "m328Pdef.inc" 10 | #include "avr.inc" 11 | .list 12 | 13 | .global main 14 | 15 | SYSCLK = 16000000 ;System clock 16 | BPS = 38400 ;UART bps 17 | TL_TIME = 1500 ;Error timer (tError(ms)=TL_TIME/3) 18 | 19 | #define USE_DECAY // let the I value decay after reaching a steady state 20 | 21 | #define _0 r15 //;Permanent zero register 22 | #define _PvEnc r14 //;Previous encoder signal A/B 23 | #define _PvDir r13 //;Previous direction 24 | #define _PosX r12 //;Current position 25 | #define _PosH r11 //; 26 | #define _PosL r10 //;/ 27 | 28 | #define _CtDiv r9 //;1/83 divider 29 | 30 | #define _Flags r25 //; 1kHz|Sat.F|Sat.R| | | | 31 | 32 | ; avr-as workarounds 33 | #define _Pos _PosL 34 | 35 | ;----------------------------------------------------------; 36 | ; compatibility definitions for devices with different register names 37 | 38 | ; eeprom: bigger devices have 16 bit offset register 39 | #ifndef EEAR 40 | EEAR = EEARL 41 | #endif 42 | 43 | ; uart: use UART0 44 | #ifndef UDR 45 | UBRRL = UBRR0L 46 | UBRRH = UBRR0H 47 | UCSRA = UCSR0A 48 | UCSRB = UCSR0B 49 | UDR = UDR0 50 | UDRE = UDRE0 51 | RXCIE = RXCIE0 52 | #endif 53 | 54 | ; timer 55 | #ifndef TIMSK 56 | TIMSK = TIMSK0 57 | #endif 58 | 59 | ; workarounds for avr-as 60 | UBRR = UBRRL 61 | 62 | 63 | ;----------------------------------------------------------; 64 | ; define LED pin mappings for different devices 65 | 66 | #if defined(__AVR_ATtiny2313__) || defined(__AVR_ATtiny2313A__) 67 | #define LED_ERROR PORTB,0 68 | #define LED_TORQUE PORTB,1 69 | #define LED_READY PORTB,2 70 | 71 | #elif defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) 72 | #define LED_ERROR PORTB,0 73 | #define LED_TORQUE PORTC,3 74 | #define LED_READY PORTC,2 75 | 76 | #else 77 | .error "no LED pin definition found for this device" 78 | 79 | #endif 80 | 81 | 82 | .macro led_on port,bit 83 | sbi \port,\bit ; LED on 84 | .endm 85 | 86 | .macro led_off port,bit 87 | cbi \port,\bit ; LED off 88 | .endm 89 | 90 | 91 | 92 | ;----------------------------------------------------------; 93 | ; EEPROM Area 94 | 95 | .section .eeprom 96 | ; Memory bank 0 : Yasukawa Electric UGTMEM-A1SA51 97 | .word 300, 0x0500, 0x0300, 0x00c0, 240, 0x0340, 0x2600, 128 98 | 99 | ; Memory bank 1 : Yasukawa Electric UGTMEM-A1SA51 100 | .word 300, 0x0500, 0x0300, 0x00c0, 240, 0x0340, 0x2600, 128 101 | 102 | ; Memory bank 2 : Tamagawa Seiki TS1410N1 103 | .word 300, 0x0800, 0x0300, 0x0060, 180, 0x0550, 0x1a00, 32 104 | 105 | ; Memory bank 3 : Matsushita Electric MCN-14EAEC (6V, 40p/r) 106 | .word 200, 0x0800, 0x0a00, 0x0400, 200, 0x0840, 0x1400, 64 107 | 108 | N_PARM = 8 ; Number of parameter words per bank. 109 | 110 | 111 | 112 | ;----------------------------------------------------------; 113 | ; Data memory area 114 | 115 | .section .bss 116 | 117 | ; Servo / G command parameters 118 | Parms: 119 | LimSpd: .fill 2 ;P0,Velocity limit Integer 120 | GaSpd: .fill 2 ;P1,Velocity feedback gain 8.8 fixed point 121 | GaTqP: .fill 2 ;P2,Proportional gain 8.8 fixed point 122 | GaTqI: .fill 2 ;P3,Integral gain 8.8 fixed point 123 | LimTrq: .fill 2 ;P4,Torque limit Integer 124 | GaEG: .fill 2 ;P5,EG feedback gain 8.8 fixed point 125 | MvSpd: .fill 2 ;P6,G0 velocity Integer 126 | MvAcc: .fill 2 ;P7,G0 acceleration Integer 127 | 128 | ; Command/Servo registers 129 | CtPos: .fill 3 ;Position g/j mode 3 130 | CtSub: .fill 2 ;Sub command s mode 0/1/2 131 | PvInt: .fill 2 ;Integration register 132 | PvPos: .fill 2 ;velocity detection register 133 | OvTmr: .fill 2 ;Torque limit timer 134 | 135 | Mode: .fill 1 ;Servo Mode m 136 | 137 | ; Display buffer 138 | Disp: .fill 1+8 ;Display buffer, pointer 139 | 140 | 141 | ; Displacements referrd from RAMTOP 142 | iLimSpd = LimSpd-Parms 143 | iGaSpd = GaSpd-Parms 144 | iGaTqP = GaTqP-Parms 145 | iGaTqI = GaTqI-Parms 146 | iLimTrq = LimTrq-Parms 147 | iGaEG = GaEG-Parms 148 | iMvSpd = MvSpd-Parms 149 | iMvAcc = MvAcc-Parms 150 | iCtPos = CtPos-Parms 151 | iCtSub = CtSub-Parms 152 | iPvInt = PvInt-Parms 153 | iPvPos = PvPos-Parms 154 | iOvTmr = OvTmr-Parms 155 | iMode = Mode-Parms 156 | 157 | ; Host command 158 | RxBuf: .fill 2+16 ; Serial receive buffer (Rp, Wp, Buff[16]) 159 | LineBuf:.fill 20 ; Command line input buffer 160 | 161 | 162 | 163 | ;----------------------------------------------------------; 164 | ; Program code 165 | 166 | .text 167 | .org 0 168 | 169 | #if defined(__AVR_ATtiny2313__) || defined(__AVR_ATtiny2313A__) 170 | ; Interrupt Vectors (ATtiny2313) 171 | rjmp reset ;Reset 172 | rjmp 0 ;INT0 173 | rjmp 0 ;INT1 174 | rjmp 0 ;TC1 CAPT 175 | rjmp 0 ;TC1 COMPA 176 | rjmp 0 ;TC1 overflow 177 | rjmp 0 ;TC0 overflow 178 | rjmp rxint ;USART0 Rx ready 179 | rjmp 0 ;USART0 Tx UDRE 180 | rjmp 0 ;USART0 Tx empty 181 | rjmp 0 ;Analog comparator 182 | rjmp 0 ;PCINT 183 | rjmp 0 ;TC1 COMPB 184 | rjmp background ;TC0 COMPA 185 | ; rjmp 0 ;TC0 COMPB 186 | ; rjmp 0 ;USI START 187 | ; rjmp 0 ;USI OVF 188 | ; rjmp 0 ;EEPROM 189 | ; rjmp 0 ;WDT 190 | 191 | 192 | #elif __FLASH_SIZE__ <= 0x1000 193 | ; Interrupt Vectors (for all AVR with up to 4kB flash, use rjmp) 194 | rjmp reset ;Reset 195 | .org OC0Aaddr 196 | rjmp background ;TC0 COMPA 197 | .org URXCaddr 198 | .org 199 | rjmp rxint ;USART0 Rx ready 200 | .org INT_VECTORS_SIZE 201 | ; leave the rest of the table empty 202 | 203 | #else 204 | ; Interrupt Vectors (for all AVR with more than 4kB flash, use jmp) 205 | jmp reset ;Reset 206 | ;.org OC0Aaddr 207 | .org TIMER0_COMPA_vect_num * 4; 208 | jmp background ;TC0 COMPA 209 | ;.org URXCaddr 210 | .org USART_RX_vect_num * 4; 211 | jmp rxint ;USART0 Rx ready 212 | .org INT_VECTORS_SIZE 213 | ; leave the rest of the table empty 214 | 215 | #endif 216 | 217 | 218 | reset: 219 | outi SPL, low(RAMEND) ;Stask ptr 220 | clr _0 ;Clear RAM 221 | ldiw YL, RAMTOP ; 222 | b001: st Y+, _0 ; 223 | cpi YL, low(RAMTOP+128) ; 224 | brne b001 ;/ 225 | 226 | outi PORTD, 0b01111111 ;Initialize PORTD 227 | outi DDRD, 0b00000010 ;/ 228 | 229 | #if defined(__AVR_ATtiny2313__) || defined(__AVR_ATtiny2313A__) || \ 230 | defined(__AVR_AT90S2313__) 231 | outi PORTB, 0b10000000 ;Initialize PORTB 232 | outi DDRB, 0b11111111 ;/ 233 | #elif defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) 234 | outi PORTB, 0b00100000 ;Initialize PORTB 235 | outi DDRB, 0b11111111 ;/ 236 | 237 | out PORTC, _0 ;Initialize PORTC 238 | outi DDRC, 0b00001100 ;/ 239 | #endif 240 | 241 | ldiw A, SYSCLK/16/BPS-1 ;UART 242 | outw UBRR, A ; 243 | outi UCSRB, 0b10011000 ;/ 244 | 245 | ldiw A, 128 ;TC1: 8bit PWM mode 246 | outw OCR1A, A ; 247 | outw OCR1B, A ; 248 | outi TCCR1A, 0b10100001 ; 249 | outi TCCR1B, 0b00000001 ;/ 250 | 251 | outi OCR0A, 2 ;TC0: 83kHz interval timer 252 | outi TCCR0A, 0b00000010 ; 253 | outi TCCR0B, 0b00000011 ; 254 | outi TIMSK, (1< Position command reg. 457 | lddw T4L, Z+0 ;T6L:T4 = start position 458 | ldd T6L, Z+2 ;/ 459 | mov T0H, T4L ;r3:r0 = commanded position 460 | mov T2L, T4H ; 461 | mov T2H, T6L ;/ 462 | rcall get_val ;sub command 463 | rjeq cmd_err ;/ 464 | cpi AL, 0 ;G0? 465 | breq dg_0 ;/ 466 | cpi AL, 1 ;G1? 467 | breq dg_1 ;/ 468 | rjmp cmd_err 469 | 470 | ; rectangular velocity profile 471 | dg_1: rcall get_val ;BL:AL = target position 472 | rjeq cmd_err ;/ 473 | pushw A 474 | push BL 475 | rcall get_val ;CH:BH = velocity 476 | mov BH, AL ; 477 | mov CL, AH ; 478 | mov CH, _0 ;/ 479 | pop BL 480 | popw A 481 | rjeq cmd_err ;/ 482 | cpw A, T4 ;T = direction 483 | cpc BL, T6L ; 484 | clt ; 485 | brge b09 ; 486 | set ;/ 487 | b09: clr r0 ; 488 | b010: rcall dg_add ;---Constant velocity loop 489 | brlt b010 ; 490 | rjmp dg_end ;/ 491 | 492 | ; Trapezoidal velocity profile 493 | dg_0: rcall get_val ;BL:A = target posision 494 | rjeq cmd_err ;/ 495 | cpw A, T4 ;T = direction 496 | cpc BL, T6L ; 497 | clt ; 498 | brge b011 ; 499 | set ;/ 500 | b011: clr r0 ; 501 | clr BH ;CL:BH = start velocity 502 | clrw C ;/ 503 | 504 | dg_ul: lds DL, MvAcc+0 ;---Up ramp loop 505 | add BH, DL ;Increase velocity 506 | lds DL, MvAcc+1 ; 507 | adc CL, DL ; 508 | adc CH, _0 ;/ 509 | rcall dg_add 510 | brge dg_end 511 | movw DL, AL ;Check current position has passed half of distance. 512 | mov EL, BL ;If passed, enter to down ramp. 513 | sub DL, T4L ; 514 | sbc DH, T4H ; 515 | sbc EL, T6L ; 516 | asr EL ; 517 | rorw D ; 518 | addw D, T4 ; 519 | adc EL, T6L ; 520 | brts b40 ; 521 | cp T0H, DL ; 522 | cpc T2L, DH ; 523 | cpc T2H, EL ; 524 | brge dg_de ; 525 | rjmp b41 ; 526 | b40: cp DL, T0H ; 527 | cpc DH, T2L ; 528 | cpc EL, T2H ; 529 | brge dg_de ;/ 530 | b41: ldsw D, MvSpd ;Has current velocity reached P6? 531 | cp BH, DL ;If reached, enter constant velocity mode. 532 | cpc CL, DH ; 533 | cpc CH, _0 ; 534 | brlo dg_ul ;/ 535 | 536 | movw DL, T4L ;Calculate down ramp point 537 | mov EL, T6L ; 538 | sub DL, T0H ; 539 | sbc DH, T2L ; 540 | sbc EL, T2H ; 541 | addw D, A ; 542 | adc EL, BL ;/ EL:DL = s.p. - c.p. + t.p. 543 | dg_cl: rcall dg_add ;---Constant velocity loop 544 | brts b42 545 | cp T0H, DL 546 | cpc T2L, DH 547 | cpc T2H, EL 548 | brge dg_de 549 | rjmp dg_cl 550 | b42: cp DL, T0H 551 | cpc DH, T2L 552 | cpc EL, T2H 553 | brlt dg_cl 554 | 555 | dg_de: rcall dg_add 556 | dg_dl: lds DL, MvAcc+0 ;---Down ramp loop 557 | sub BH, DL ;Decrese velocity 558 | lds DL, MvAcc+1 ; 559 | sbc CL, DL ; 560 | sbc CH, _0 ;/ 561 | brcs dg_end 562 | rcall dg_add 563 | brlt dg_dl 564 | 565 | dg_end: ; End of action 566 | cli 567 | stdw Z+0, A 568 | std Z+2, BL 569 | dge_lp: cli ; Wait until position stabled 570 | cpw _Pos, A 571 | cpc _PosX, BL 572 | sei 573 | breq b43 574 | push AL 575 | rcall receive 576 | pop AL 577 | breq dge_lp 578 | b43: rjmp mainloop 579 | 580 | 581 | 582 | dg_add: 583 | cbr _Flags, bit7 ;Wait for 1kHz time interval 584 | b44: push AL ; 585 | rcall receive ; Break on ESC received 586 | breq b45 ; 587 | cpi AL, 0x1B ; 588 | breq dga_stop ; / 589 | b45: pop AL ; 590 | sbrs _Flags, 7 ; 591 | rjmp b44 ;/ 592 | sbrc _Flags, 6 ;Skip if torque limit did occur. 593 | rjmp dg_add ; 594 | sbrc _Flags, 5 ; 595 | rjmp dg_add ;/ 596 | brts b46 ;Increase commanded point by current velocity 597 | add T0L, BH ; 598 | adc T0H, CL ; 599 | adc T2L, CH ; 600 | adc T2H, CH ; 601 | cp T0H, AL ; 602 | cpc T2L, AH ; 603 | cpc T2H, BL ; 604 | brge dga_ov ; 605 | rjmp b47 ; 606 | b46: sub T0L, BH ; 607 | sbc T0H, CL ; 608 | sbc T2L, CH ; 609 | sbc T2H, CH ; 610 | cp AL, T0H ; 611 | cpc AH, T2L ; 612 | cpc BL, T2H ; 613 | brge dga_ov ;/ 614 | b47: cli 615 | std Z+0, T0H 616 | stdw Z+1, T2 617 | sei 618 | ses 619 | dga_ov: ret 620 | 621 | dga_stop: 622 | pop AL 623 | pop AL 624 | pop AL 625 | rjmp mainloop 626 | 627 | 628 | 629 | ;----------------------------------------------------------; 630 | ; Initialize servo system 631 | ; 632 | ;Call: AL = servo mode 633 | 634 | init_servo: 635 | cli 636 | sts Mode, AL ;Set servo mode 637 | ldiw YL, CtPos ;Clear cmmand regs and servo operators. 638 | b012: st Y+, _0 ; 639 | cpi YL, low(CtPos+11); 640 | brne b012 ;/ 641 | clrw _Pos ;Clear position counter 642 | clr _PosX ;/ 643 | sei 644 | led_off LED_ERROR ;Error LED off 645 | led_off LED_TORQUE ;Torque LED off 646 | led_on LED_READY ;Ready LED on 647 | ret 648 | 649 | 650 | 651 | ;----------------------------------------------------------; 652 | ; 83kHz Position capture and servo operation interrupt 653 | 654 | background: 655 | push T0L 656 | pushw ZL 657 | in T0L, SREG ;Save flags 658 | 659 | mov ZL, _PvEnc ;ZL[1:0] = previous A/B signal 660 | in _PvEnc, PIND ;Sample A/B signal into _PvEnc[1:0] 661 | swap _PvEnc ;/ 662 | ldi ZH, 1 ;Convert it to sequencial number. 663 | sbrc _PvEnc, 1 ; 664 | eor _PvEnc, ZH ;/ 665 | sub ZL, _PvEnc ;Decode motion 666 | andi ZL, 3 ;/ 667 | breq enc_zr ;-> Not moved 668 | cpi ZL, 3 ; 669 | breq enc_rev ;-> -1 count 670 | cpi ZL, 1 ; 671 | breq enc_fwd ;-> +1 count 672 | mov ZL, _PvDir ;-> Missing code recovery: 673 | mov ZH, _PvDir ; double count for previous direction 674 | lsl ZL ; 675 | asr ZH ;/ 676 | rjmp enc_add 677 | enc_rev:ldiw ZL, -1 678 | rjmp b013 679 | enc_fwd:ldiw ZL, 1 680 | b013: mov _PvDir, ZL 681 | enc_add:addw _Pos, ZL 682 | adc _PosX, ZH 683 | enc_zr: 684 | dec _CtDiv ;Decrement 1/83 divider 685 | rjne bgnd_exit ;If not overflow, exit interrupt routine. 686 | 687 | ; End of 83 kHz position captureing process. Follows are 1kHz servo 688 | ; operation. It will be interrupted itself, but will not be re-entered. 689 | 690 | ldi ZL, 83 ;Re-initialize 1/83 divider 691 | mov _CtDiv, ZL ;/ 692 | sei ;Enable interrupts 693 | pushw T0 694 | pushw T2 695 | pushw A 696 | pushw B 697 | pushw C 698 | push DL 699 | pushw YL 700 | ldiw YL, Parms ;Work area base pointer 701 | 702 | sbr _Flags, bit7 ; 1kHz interrupt flag 703 | 704 | lddw T2, Y+iPvPos ;Detect velocity 705 | cli ; 706 | subw T2, _Pos ; 707 | stdw Y+iPvPos, _Pos ; 708 | sei ; 709 | negw T2 ;/ T2 = velocity 710 | 711 | ldd AL, Y+iMode ;Branch by servo mode 712 | cpi AL, 3 ;Mode3? 713 | breq tap_position ;/ 714 | lddw T0, Y+iCtSub ;Get sub command 715 | cpi AL, 2 ;Mode2? 716 | breq tap_velocity ;/ 717 | cpi AL, 1 ;Mode1? 718 | rjeq tap_torque ;/ 719 | rjmp tap_voltage ;Mode0 720 | 721 | 722 | tap_position: 723 | cli ;Get position error (= velocity command) 724 | lddw T0, Y+iCtPos ; 725 | ldd BH, Y+iCtPos+2 ; 726 | subw T0, _Pos ; 727 | sbc BH, _PosX ; 728 | sei ;BH:T0 = position error 729 | 730 | lddw A, Y+iLimSpd ;Velocity limit (P0) 731 | clr BL ; 732 | cpw T0, A ; 733 | cpc BH, BL ; 734 | brge b10 ; 735 | negw A ; 736 | com BL ; 737 | cpw T0, A ; 738 | cpc BH, BL ; 739 | brge tap_velocity ; 740 | b10: movw T0L, AL ;T0 = velocity command 741 | 742 | 743 | tap_velocity: 744 | movw AL, T2L ;Velocity loop gain (P1) 745 | lddw B, Y+iGaSpd ; 746 | rcall muls1616 ;B = scaled velocity 747 | subw T0, B ;T0 = velocity error 748 | 749 | movw AL, T0L ;Velocity error P-gain (P2) 750 | lddw B, Y+iGaTqP ; 751 | rcall muls1616 ; 752 | movw ZL, BL ;Z = P term; 753 | 754 | lddw A, Y+iPvInt ;Velocity error I-gain (P3) 755 | lddw B, Y+iGaTqI ; 756 | rcall muls1616 ; 757 | addw ZL, B ;Z += I term; 758 | 759 | cbr _Flags, bit6+bit5;Torque limit (P4) 760 | lddw B, Y+iLimTrq ; 761 | cpw ZL, B ; 762 | brlt b20 ; 763 | movw ZL, BL ; 764 | sbr _Flags, bit6 ; 765 | b20: neg BL ; 766 | com BH ; 767 | cpw ZL, B ; 768 | brge b21 ; 769 | movw ZL, BL ; 770 | sbr _Flags, bit5 ;/ 771 | 772 | b21: tst T0H ;PvInt += T0, with anti-windup 773 | brmi b22 ; 774 | sbrc _Flags, 6 ; 775 | rjmp b24 ; 776 | rjmp b23 ; 777 | b22: sbrc _Flags, 5 ; 778 | rjmp b24 ; 779 | b23: lddw A, Y+iPvInt ; 780 | #if defined(USE_DECAY) 781 | ;-- addition: decay for the integral value after steady state is reached 782 | ; (useful for static positioning applications like XY-tables) 783 | ; if (T0 || T2) A+=T0 784 | ; else {B=(A>>6); A-=B} 785 | 786 | mov BL, T0L ; check for (T0==0)&&(T2==0) 787 | or BL, T0H 788 | or BL, T2L 789 | or BL, T2H 790 | brne b28 791 | movw BL, AL ; B = A>>5 792 | asr BH 793 | ror BL 794 | asr BH 795 | ror BL 796 | asr BH 797 | ror BL 798 | asr BH 799 | ror BL 800 | asr BH 801 | ror BL 802 | subw A, B ; A -= B; 803 | rjmp b29 804 | #endif 805 | b28: addw A, T0 ; 806 | b29: stdw Y+iPvInt, A ;/ 807 | 808 | b24: mov AL, _Flags ;Check torque limiter timer 809 | andi AL, bit6+bit5 ; OvTmr is increased by 3 when torque limitter 810 | lddw A, Y+iOvTmr ; works, decreased by 1 when no torque limit. 811 | breq b25 ; When the value reaches TL_TIME, servo turns 812 | led_on LED_TORQUE ; off and goes error state. 813 | addiw A, 3 ; 814 | rjmp b26 ; 815 | b25: led_off LED_TORQUE ; 816 | subiw A, 1 ; 817 | brlo b27 ; 818 | b26: stdw Y+iOvTmr, A ; 819 | ldiw B, TL_TIME ; 820 | cpw A, B ; 821 | brsh servo_error ;/ 822 | 823 | b27: movw T0L, ZL ;T0 = torque command 824 | 825 | 826 | tap_torque: 827 | movw AL, T2L ;EG compensation (P5) 828 | lddw B, Y+iGaEG ; 829 | rcall muls1616 ; 830 | addw T0, B ;T0 = voltage command 831 | 832 | 833 | tap_voltage: 834 | ldiw A, 240 ;Clip output voltage between -240 and +240. 835 | cpw T0, A ; Limit minimum duty ratio to 15/16 for bootstrap 836 | brge b30 ; type FET driver. 837 | ldiw A, -240 ; 838 | cpw T0, A ; 839 | brge b31 ; 840 | b30: movw T0L, AL ;T0 = PWM command 841 | 842 | b31: asrw T0 ;Set PWM register (OCR1A and ~OCR1B) 843 | ldi AL, 120 ; 844 | adc AL, T0L ; 845 | ldi AH, 120 ; 846 | sub AH, T0L ; 847 | .if OCR1AL < 0x40 848 | out OCR1AL, AL ; 849 | out OCR1BL, AH ;/ 850 | .else 851 | sts OCR1AL, AL ; 852 | sts OCR1BL, AH ;/ 853 | .endif 854 | 855 | rcall disp_pos 856 | 857 | popw YL 858 | pop DL 859 | popw C 860 | popw B 861 | popw A 862 | popw T2 863 | popw T0 864 | 865 | bgnd_exit: ;End of encoder capture and servo operation 866 | out SREG, T0L ;Restore flags 867 | popw ZL 868 | pop T0L 869 | reti 870 | 871 | 872 | servo_error: 873 | clr AL ;Enter Mode0 874 | rcall init_servo ;/ 875 | led_on LED_ERROR ;Error LED on 876 | clrw T0 ;Output off 877 | rjmp tap_voltage 878 | 879 | 880 | 881 | ;--------------------------------------; 882 | ; Display position counter 883 | 884 | disp_pos: 885 | lds DL,Disp+8 886 | inc DL 887 | cpi DL,8 888 | brcs dp_out 889 | 890 | mov AL,_PosL 891 | mov AH,_PosH 892 | mov BL,_PosX 893 | ldi CH,0 894 | sbrs BL,7 895 | rjmp b50 896 | ldi CH,0x40 897 | com AL 898 | com AH 899 | com BL 900 | adc AL,_0 901 | adc AH,_0 902 | adc BL,_0 903 | b50: ldiw YL,Disp 904 | dp_l1: ldi CL,24 905 | ldi BH,0 906 | dp_l2: lsl AL 907 | rol AH 908 | rol BL 909 | rol BH 910 | cpi BH,10 911 | brcs b51 912 | subi BH,10 913 | inc AL 914 | b51: dec CL 915 | brne dp_l2 916 | ldiw ZL,t_7seg 917 | add ZL,BH 918 | adc ZH,_0 919 | lpm CL,Z 920 | st Y+,CL 921 | cp AL,_0 922 | cpc AH,_0 923 | cpc BL,_0 924 | brne dp_l1 925 | b52: st Y+,CH 926 | clr CH 927 | cpi YL,low(Disp+8) 928 | brne b52 929 | clr DL 930 | 931 | #ifdef USIDR 932 | ; This code uses the USI device of the tiny2313. 933 | ; - write data to USIDR 934 | ; - double-toggle USCK 8 times to shift out the data 935 | ; - write 0x80 to USIDR to set DO to high 936 | ; - toggle DI to advance to the next digit. 937 | dp_out: ldiw Y,Disp 938 | std Y+8,DL 939 | add YL,DL 940 | adc YH,_0 941 | ld AL,Y 942 | com AL 943 | out USIDR,AL 944 | ldi AH,0x80 945 | ldi CL,8 946 | b53: out PINB,AH 947 | dec CL 948 | out PINB,AH 949 | brne b53 950 | cpi DL,0 951 | brne b54 952 | ldi AH,0 953 | b54: out USIDR,AH 954 | sbi PORTB,5 955 | cbi PORTB,5 956 | ret 957 | #else 958 | ;FIXME: add SPI code for ATmega. 959 | dp_out: 960 | ret 961 | #endif 962 | 963 | 964 | 965 | 966 | ;--------------------------------------; 967 | ; 16bit * 16bit signed multiply 968 | ; 969 | ; Multiplier: A(signed int) 970 | ; Multiplicand: B(unsigned, 8.8 fraction) 971 | ; Result: B(signed int) 972 | ; Clk: 181(max) 973 | 974 | muls1616: 975 | clt 976 | tst AH 977 | brpl b60 978 | set 979 | negw A 980 | 981 | b60: subw C, C ; clear high 16bit. 982 | ldi DL, 17 ; DL = loop count 983 | b61: brcc b62 ; ---- calculating loop 984 | addw C, A ; 985 | b62: rorw C ; 986 | rorw B ; 987 | dec DL ; if (--DL > 0) 988 | brne b61 ; continue loop; 989 | 990 | mov BL, BH 991 | mov BH, CL 992 | 993 | brtc b63 ; Negate the result if multiplier was negative. 994 | negw B 995 | b63: ret 996 | 997 | 998 | 999 | ;--------------------------------------; 1000 | ; Input a command line into LineBuf. 1001 | 1002 | get_line: 1003 | ldiw XL,LineBuf 1004 | ldi BH,0 1005 | rl_lp: rcall receive 1006 | breq rl_lp 1007 | st X,AL 1008 | cpi AL,0x0d ; CR 1009 | brne b80 1010 | ldiw XL,LineBuf 1011 | rjmp echo 1012 | b80: cpi AL,0x08 ; BS 1013 | brne b81 1014 | cpi BH,0 1015 | breq rl_lp 1016 | rcall echo 1017 | sbiw XL,1 1018 | dec BH 1019 | rjmp rl_lp 1020 | b81: cpi AL,' ' ; SP 1021 | brcs rl_lp 1022 | cpi BH,20-1 1023 | breq rl_lp 1024 | rcall echo 1025 | adiw XL,1 1026 | inc BH 1027 | rjmp rl_lp 1028 | 1029 | 1030 | 1031 | ;--------------------------------------; 1032 | ; Send ROM string 1033 | ; 1034 | ; Call: Z = top of the string (ASCIZ) 1035 | ; Ret: Z = next string 1036 | 1037 | b82: rcall xmit 1038 | dp_str: lpm AL, Z+ 1039 | tst AL 1040 | brne b82 1041 | ret 1042 | 1043 | 1044 | ;--------------------------------------; 1045 | ; Get value of decimal string 1046 | ; 1047 | ; Call: X -> ASCII string 1048 | ; Ret: X = updated 1049 | ; if C=1: error 1050 | ; elsif Z=1: end of line, value=0 1051 | ; else: BL:AL = 24bit value 1052 | ; 1053 | ; Positive: "300" 1054 | ; Negative: "-125000" 1055 | 1056 | get_val: 1057 | clt 1058 | clr AL 1059 | clr AH 1060 | clr BL 1061 | b83: ld BH,X+ 1062 | cpi BH,' ' 1063 | brcs gd_n 1064 | breq b83 1065 | cpi BH,'-' 1066 | brne b84 1067 | set 1068 | gd_l: ld BH,X+ 1069 | b84: cpi BH,' '+1 1070 | brcs gd_e 1071 | subi BH,'0' 1072 | brcs gd_x 1073 | cpi BH,10 1074 | brcc gd_x 1075 | ldi CL, 25 1076 | ldi CH, 10 1077 | sub r0, r0 1078 | b85: lsr r0 1079 | ror BL 1080 | ror AH 1081 | ror AL 1082 | brcc b86 1083 | add r0, CH 1084 | b86: dec CL 1085 | brne b85 1086 | add AL, BH 1087 | adc AH, _0 1088 | adc BL, _0 1089 | rjmp gd_l 1090 | gd_x: sec 1091 | sez 1092 | ret 1093 | gd_e: sbiw XL,1 1094 | brtc b87 1095 | com AL 1096 | com AH 1097 | com BL 1098 | subi AL,-1 1099 | sbci AH,-1 1100 | sbci BL,-1 1101 | b87: clc 1102 | ret 1103 | gd_n: sbiw XL,1 1104 | clc 1105 | sez 1106 | ret 1107 | 1108 | 1109 | 1110 | ;--------------------------------------; 1111 | ; Display a value in decimal string 1112 | ; 1113 | ; Call: BL:A = 24bit signed value to be displayed 1114 | ; Ret: BL:A = broken 1115 | 1116 | dp_dec: ldi CH,' ' 1117 | sbrs BL, 7 1118 | rjmp b88 1119 | com AL 1120 | com AH 1121 | com BL 1122 | adc AL,_0 1123 | adc AH,_0 1124 | adc BL,_0 1125 | ldi CH,'-' 1126 | b88: clr T0L ;digit counter 1127 | b088: inc T0L ;---- decimal string generating loop 1128 | clr BH ;var1 /= 10; 1129 | ldi CL,24 ; 1130 | b89: lslw A ; 1131 | rolw B ; 1132 | cpi BH,10 ; 1133 | brcs b89a ; 1134 | subi BH,10 ; 1135 | inc AL ; 1136 | b89a: dec CL ; 1137 | brne b89 ;/ 1138 | addi BH,'0' ;Push the remander (a decimal digit) 1139 | push BH ;/ 1140 | cp AL,_0 ;if(var1 =! 0) 1141 | cpc AH,_0 ; continue digit loop; 1142 | cpc BL,_0 ; 1143 | brne b088 ;/ 1144 | mov AL, CH ;Sign 1145 | rcall xmit ;/ 1146 | b89b: pop AL ;Transmit decimal string 1147 | rcall xmit ;<-- Put a char to memory, console or any other display device 1148 | dec T0L ; 1149 | brne b89b ;/ 1150 | ret 1151 | 1152 | 1153 | ;--------------------------------------; 1154 | ; Serial I/O driver 1155 | 1156 | ; Transmit AL. 1157 | echo: 1158 | xmit: 1159 | .if UDR < 0x40 1160 | b90: sbis UCSRA, UDRE 1161 | rjmp b90 1162 | out UDR, AL 1163 | ret 1164 | .else 1165 | push AH ; not sure if AH is really important. 1166 | b90: 1167 | lds AH, UCSRA 1168 | sbrs AH, UDRE 1169 | rjmp b90 1170 | sts UDR, AL 1171 | pop AH 1172 | ret 1173 | .endif 1174 | 1175 | receive:; Receive a char into AL. (ZR=no data) 1176 | push AH 1177 | pushw YL 1178 | ldiw YL, RxBuf 1179 | cli 1180 | ldd AH, Y+0 1181 | ldd AL, Y+1 1182 | cp AH, AL 1183 | breq b91 1184 | add YL, AH 1185 | ldd AL, Y+2 1186 | sub YL, AH 1187 | inc AH 1188 | andi AH, 15 1189 | std Y+0, AH 1190 | clz 1191 | b91: sei 1192 | popw YL 1193 | pop AH 1194 | ret 1195 | 1196 | rxint: ;USART0 Rx ready 1197 | push AL 1198 | in AL, SREG 1199 | push BL 1200 | pushw A 1201 | .if UCSRB < 0x40 1202 | in BL, UDR 1203 | cbi UCSRB, RXCIE 1204 | .else 1205 | lds BL, UDR 1206 | lds AL, UCSRB 1207 | andi AL, low(~(1< avra.disass 29 | 30 | clean: 31 | rm -rf *~ *.bak *.cof *.hex *.lst *.map *.obj build 32 | -------------------------------------------------------------------------------- /atmega/avr.inc: -------------------------------------------------------------------------------- 1 | ;------------------------------------------------; 2 | ; Constants 3 | 4 | .equ RAMTOP = SRAM_START ; SRAM top address 5 | 6 | 7 | .equ bit0 = 0b00000001 8 | .equ bit1 = 0b00000010 9 | .equ bit2 = 0b00000100 10 | .equ bit3 = 0b00001000 11 | .equ bit4 = 0b00010000 12 | .equ bit5 = 0b00100000 13 | .equ bit6 = 0b01000000 14 | .equ bit7 = 0b10000000 15 | 16 | 17 | .def T0L = r0 18 | .def T0H = r1 19 | .def T2L = r2 20 | .def T2H = r3 21 | .def T4L = r4 22 | .def T4H = r5 23 | .def T6L = r6 24 | .def T6H = r7 25 | .def T8L = r8 26 | .def T8H = r9 27 | .def T10L = r10 28 | .def T10H = r11 29 | .def T12L = r12 30 | .def T12H = r13 31 | .def T14L = r14 32 | .def T14H = r15 33 | 34 | 35 | .def AL = r16 36 | .def AH = r17 37 | .def BL = r18 38 | .def BH = r19 39 | .def CL = r20 40 | .def CH = r21 41 | .def DL = r22 42 | .def DH = r23 43 | .def EL = r24 44 | .def EH = r25 45 | 46 | 47 | 48 | ;------------------------------------------------; 49 | ; Push/Pop register pair 50 | ; 51 | ; pushw Z 52 | 53 | .macro pushw 54 | push @0H 55 | push @0L 56 | .endm 57 | 58 | .macro popw 59 | pop @0L 60 | pop @0H 61 | .endm 62 | 63 | 64 | ;------------------------------------------------; 65 | ; Load/store word from/to direct memory/immediate 66 | ; 67 | ; ldsw Z,mem 68 | ; ldiw Z,imm 69 | 70 | .macro ldiw 71 | ldi @0L,low(@1) 72 | ldi @0H,high(@1) 73 | .endm 74 | 75 | .macro ldsw 76 | lds @0L,@1 77 | lds @0H,@1+1 78 | .endm 79 | 80 | .macro stsw 81 | sts @0+1,@1H 82 | sts @0,@1L 83 | .endm 84 | 85 | .macro lddw 86 | ldd @0L,@1 87 | ldd @0H,@1+1 88 | .endm 89 | 90 | .macro stdw 91 | std @0+1,@1H 92 | std @0,@1L 93 | .endm 94 | 95 | .macro ldw 96 | ld @0L,@1 97 | ld @0H,@1 98 | .endm 99 | 100 | .macro stw 101 | st @0,@1L 102 | st @0,@1H 103 | .endm 104 | 105 | .macro inw 106 | .if @0L < 0x40 107 | in @0L,@1L 108 | in @0H,@1H 109 | .else 110 | lds @0L,@1L 111 | lds @0H,@1L 112 | .endif 113 | .endm 114 | 115 | .macro outw 116 | .if @0L < 0x40 117 | out @0H,@1H 118 | out @0L,@1L 119 | .else 120 | sts @0H,@1H 121 | sts @0L,@1L 122 | .endif 123 | .endm 124 | 125 | 126 | ;------------------------------------------------; 127 | ; Store immediate into indirect memory via r16 128 | ; 129 | ; sti Z,imm 130 | ; stdi Z+d,imm 131 | 132 | .macro sti 133 | ldi r16,@1 134 | st @0,r16 135 | .endm 136 | 137 | .macro stdi 138 | ldi r16,@1 139 | std @0,r16 140 | .endm 141 | 142 | .macro muli 143 | ldi r16,@1 144 | mul @0,r16 145 | .endm 146 | 147 | 148 | ;------------------------------------------------; 149 | ; add/sub/subc/cp/cpc/lsl/lsr/rol/ror to register pair 150 | ; 151 | 152 | .macro addiw 153 | subi @0L,low(-(@1)) 154 | sbci @0H,high(-(@1)) 155 | .endm 156 | 157 | .macro subiw 158 | subi @0L,low(@1) 159 | sbci @0H,high(@1) 160 | .endm 161 | 162 | .macro addw 163 | add @0L,@1L 164 | adc @0H,@1H 165 | .endm 166 | 167 | .macro adcw 168 | adc @0L,@1L 169 | adc @0H,@1H 170 | .endm 171 | 172 | .macro subw 173 | sub @0L,@1L 174 | sbc @0H,@1H 175 | .endm 176 | 177 | .macro sbcw 178 | sbc @0L,@1L 179 | sbc @0H,@1H 180 | .endm 181 | 182 | .macro cpw 183 | cp @0L,@1L 184 | cpc @0H,@1H 185 | .endm 186 | 187 | .macro cpcw 188 | cpc @0L,@1L 189 | cpc @0H,@1H 190 | .endm 191 | 192 | .macro cpiw 193 | cpi @0L,low(@1) 194 | ldi r16,high(@1) 195 | cpc @0H,r16 196 | .endm 197 | 198 | .macro andw 199 | and @0L,@1L 200 | and @0H,@1H 201 | .endm 202 | 203 | .macro andiw 204 | andi @0L,low(@1) 205 | andi @0H,high(@1) 206 | .endm 207 | 208 | .macro orw 209 | or @0L,@1L 210 | or @0H,@1H 211 | .endm 212 | 213 | .macro oriw 214 | ori @0L,low(@1) 215 | ori @0H,high(@1) 216 | .endm 217 | 218 | .macro lslw 219 | lsl @0L 220 | rol @0H 221 | .endm 222 | 223 | .macro lsrw 224 | lsr @0H 225 | ror @0L 226 | .endm 227 | 228 | .macro asrw 229 | asr @0H 230 | ror @0L 231 | .endm 232 | 233 | .macro rolw 234 | rol @0L 235 | rol @0H 236 | .endm 237 | 238 | .macro rorw 239 | ror @0H 240 | ror @0L 241 | .endm 242 | 243 | .macro clrw 244 | clr @0L 245 | clr @0H 246 | .endm 247 | 248 | .macro comw 249 | com @0L 250 | com @0H 251 | .endm 252 | 253 | .macro negw 254 | com @0H 255 | neg @0L 256 | brne end 257 | inc @0H 258 | end: 259 | .endm 260 | 261 | .macro movew 262 | mov @0L, @1L 263 | mov @0H, @1H 264 | .endm 265 | 266 | .macro lpmw 267 | lpm @0L, @1 268 | lpm @0H, @1 269 | .endm 270 | 271 | 272 | ;------------------------------------------------; 273 | ; Store immediate into direct memory via r16 274 | ; 275 | ; stsi var,imm 276 | 277 | .macro stsi 278 | ldi r16,@1 279 | sts @0,r16 280 | .endm 281 | 282 | 283 | ;------------------------------------------------; 284 | ; Output port immediate via r16 285 | ; 286 | ; outi port,var 287 | 288 | .macro outi 289 | .if @0 < 0x40 290 | ldi r16,@1 291 | out @0,r16 292 | .else 293 | ldi r16,@1 294 | sts @0,r16 295 | .endif 296 | .endm 297 | 298 | 299 | ;------------------------------------------------; 300 | ; Add immediate to register 301 | 302 | .macro addi 303 | subi @0,-(@1) 304 | .endm 305 | 306 | 307 | ;------------------------------------------------; 308 | ; Long branch 309 | 310 | 311 | .macro rjne 312 | breq end 313 | rjmp @0 314 | end: 315 | .endm 316 | 317 | .macro rjeq 318 | brne end 319 | rjmp @0 320 | end: 321 | .endm 322 | 323 | .macro rjcc 324 | brcs PC+2 325 | rjmp @0 326 | .endm 327 | 328 | .macro rjcs 329 | brcc PC+2 330 | rjmp @0 331 | .endm 332 | 333 | .macro rjtc 334 | brts PC+2 335 | rjmp @0 336 | .endm 337 | 338 | .macro rjts 339 | brtc PC+2 340 | rjmp @0 341 | .endm 342 | 343 | .macro rjge 344 | brlt PC+2 345 | rjmp @0 346 | .endm 347 | 348 | .macro rjlt 349 | brge PC+2 350 | rjmp @0 351 | .endm 352 | 353 | 354 | .macro retcc 355 | brcs PC+2 356 | ret 357 | .endm 358 | 359 | .macro retcs 360 | brcc PC+2 361 | ret 362 | .endm 363 | 364 | .macro reteq 365 | brne PC+2 366 | ret 367 | .endm 368 | 369 | .macro retne 370 | breq PC+2 371 | ret 372 | .endm 373 | 374 | 375 | ;------------------------------------------------; 376 | ; Move single bit between two registers 377 | ; 378 | ; bmov dstreg,dstbit,srcreg.srcbit 379 | 380 | .macro movb 381 | bst @2,@3 382 | bld @0,@1 383 | .endm 384 | 385 | 386 | -------------------------------------------------------------------------------- /atmega/inc/m328Pdef.inc: -------------------------------------------------------------------------------- 1 | ;***** THIS IS A MACHINE GENERATED FILE - DO NOT EDIT ******************** 2 | ;***** Created: 2011-02-09 12:03 ******* Source: ATmega328P.xml ********** 3 | ;************************************************************************* 4 | ;* A P P L I C A T I O N N O T E F O R T H E A V R F A M I L Y 5 | ;* 6 | ;* Number : AVR000 7 | ;* File Name : "m328Pdef.inc" 8 | ;* Title : Register/Bit Definitions for the ATmega328P 9 | ;* Date : 2011-02-09 10 | ;* Version : 2.35 11 | ;* Support E-mail : avr@atmel.com 12 | ;* Target MCU : ATmega328P 13 | ;* 14 | ;* DESCRIPTION 15 | ;* When including this file in the assembly program file, all I/O register 16 | ;* names and I/O register bit names appearing in the data book can be used. 17 | ;* In addition, the six registers forming the three data pointers X, Y and 18 | ;* Z have been assigned names XL - ZH. Highest RAM address for Internal 19 | ;* SRAM is also defined 20 | ;* 21 | ;* The Register names are represented by their hexadecimal address. 22 | ;* 23 | ;* The Register Bit names are represented by their bit number (0-7). 24 | ;* 25 | ;* Please observe the difference in using the bit names with instructions 26 | ;* such as "sbr"/"cbr" (set/clear bit in register) and "sbrs"/"sbrc" 27 | ;* (skip if bit in register set/cleared). The following example illustrates 28 | ;* this: 29 | ;* 30 | ;* in r16,PORTB ;read PORTB latch 31 | ;* sbr r16,(1<= N_PARM) goto cmd_err; 320 | cpi AL, N_PARM 321 | brcc cmd_err 322 | ; Y = &Parms[val]; 323 | lsl AL 324 | mov YL, AL 325 | clr YH 326 | subiw Y, -Parms 327 | ; c = get_val(); 328 | ds_set: rcall get_val 329 | ; if (c == 2) goto cmd_err; 330 | brcs cmd_err 331 | ; if (c == 1) { 332 | brne ds_st 333 | ldi AL, 0x0a 334 | rcall xmit 335 | lddw A, Y+0 336 | clr BL 337 | sbrc AH, 7 338 | dec BL 339 | rcall dp_dec 340 | ldi AL, ':' 341 | rcall xmit 342 | rcall get_line 343 | rcall get_val 344 | brcs cmd_err 345 | breq b03 346 | ; } 347 | ; *Y = A; 348 | ds_st: cli 349 | stdw Y+0, A 350 | sei 351 | b03: rjmp main 352 | 353 | 354 | 355 | ;------------------------------------------------; 356 | ; Load/Save parameters. 357 | 358 | do_reep: ; Load parameters from EEPROM 359 | rcall get_val 360 | breq cmd_err 361 | cpi AL, (EEPROMEND+1)/N_PARM/2 362 | brcc cmd_err 363 | cli 364 | rcall load_parms 365 | sei 366 | rjmp main 367 | 368 | 369 | do_weep: ; Save parameters into EEPROM 370 | rcall get_val 371 | breq cmd_err 372 | cpi AL, (EEPROMEND+1)/N_PARM/2 373 | brcc cmd_err 374 | rcall get_eeadr 375 | b04: sbic EECR, EEPE 376 | rjmp b04 377 | out EEAR, BH 378 | inc BH 379 | ld AL, Y+ 380 | out EEDR, AL 381 | cli 382 | sbi EECR, EEMPE 383 | sbi EECR, EEPE 384 | sei 385 | dec AH 386 | brne b04 387 | rjmp main 388 | 389 | 390 | load_parms: 391 | rcall get_eeadr 392 | b05: out EEAR, BH 393 | inc BH 394 | sbi EECR, EERE 395 | in AL, EEDR 396 | st Y+, AL 397 | dec AH 398 | brne b05 399 | ret 400 | 401 | 402 | get_eeadr: 403 | ldi AH, N_PARM*2 404 | clr BH 405 | b06: subi AL, 1 406 | brcs b07 407 | add BH, AH 408 | rjmp b06 409 | b07: ldiw Y, Parms 410 | ret 411 | 412 | 413 | ;------------------------------------------------; 414 | ; Show location counter 415 | 416 | do_loc: 417 | ldi AL, 0x0a 418 | rcall xmit 419 | dp_p: ldi AL, 0x0d ;Show position counter 420 | rcall xmit ; 421 | cli ; 422 | movw AL, _PosL ; 423 | mov BL, _PosX ; 424 | sei ; 425 | mov T0H, AL ; 426 | rcall dp_dec ; 427 | ldi AL, ' ' ; 428 | rcall xmit ;/ 429 | b08: rcall receive ;Break if any key was pressed 430 | rjne main ;/ 431 | cp T0H, _PosL ;Continue if not changed 432 | breq b08 ;/ 433 | rjmp dp_p 434 | 435 | 436 | 437 | ;------------------------------------------------; 438 | ; Change position command reg. immediataly. 439 | 440 | ; if (!get_val()) goto cmd_err; 441 | ; CtPos = val; 442 | ; goto main; 443 | do_jump: 444 | rcall get_val 445 | rjeq cmd_err 446 | ldiw Z, CtPos ;Set position command reg. 447 | cli ; 448 | stdw Z+0, A ; 449 | std Z+2, BL ; 450 | sei ;/ 451 | rjmp main 452 | 453 | 454 | 455 | ;------------------------------------------------; 456 | ; Go at trapezoidal/rectangular velocity profile. 457 | ; 458 | ; local variables: (24 bit, LSB first in list) 459 | ; T0H:T2 current position (kind of loop variable) (s24.8) 460 | ; T4:T6L start position (s24) 461 | ; A:BL target position (s24) 462 | ; BH:C velocity (u16.8) 463 | ; D:EL next important point on ramp (midpoint, start of end ramp) 464 | ; Tflag direction of movement (b1) 465 | 466 | ; T4:T6L = CtPos; // s24 467 | ; cur_pos T0:T2 = CtPos << 8; // s32 (wirklich?) 468 | ; cur_pos T0H:T2 = CtPos; // s24 (wirklich?) 469 | ; if (get_val()) 470 | ; { 471 | ; if (val==0) goto dg_0(); 472 | ; if (val==1) goto dg_1(); 473 | ; } 474 | ; goto cmd_err; 475 | do_go: 476 | ldiw Z, CtPos ;Z -> Position command reg. 477 | lddw T4, Z+0 ;T6L:T4 = start position 478 | ldd T6L, Z+2 ;/ 479 | mov T0H, T4L ;r3:r0 = commanded position 480 | mov T2L, T4H ; 481 | mov T2H, T6L ;/ 482 | rcall get_val ;sub command 483 | rjeq cmd_err ;/ 484 | cpi AL, 0 ;G0? 485 | breq dg_0 ;/ 486 | cpi AL, 1 ;G1? 487 | breq dg_1 ;/ 488 | rjmp cmd_err 489 | 490 | ; Rectangular velocity profile 491 | dg_1: rcall get_val ;BL:AL = target position 492 | rjeq cmd_err ;/ 493 | pushw A 494 | push BL 495 | rcall get_val ;CH:BH = velocity 496 | mov BH, AL ; 497 | mov CL, AH ; 498 | mov CH, _0 ;/ 499 | pop BL 500 | popw A 501 | rjeq cmd_err ;/ 502 | cpw A, T4 ;T = direction 503 | cpc BL, T6L ; 504 | clt ; 505 | brge b09 ; 506 | set ;/ 507 | b09: clr r0 ; 508 | b010: rcall dg_add ;---Constant velocity loop 509 | brlt b010 ; 510 | rjmp dg_end ;/ 511 | 512 | ; Trapezoidal velocity profile 513 | ; if (!get_val()) goto cmd_err; 514 | dg_0: rcall get_val ;BL:A = target posision 515 | rjeq cmd_err ;/ 516 | ; Tflag = (val= midpoint) goto dg_de; //if (T0H:T2 >= EL:D) goto dg_de 555 | cp T0H, DL ; 556 | cpc T2L, DH ; 557 | cpc T2H, EL ; 558 | brge dg_de ; 559 | rjmp b41 ; 560 | ; } else { 561 | ; if (cur_pos <= midpoint) goto dg_de; //if (T0H:T2 >= EL:D) goto dg_de 562 | b40: cp DL, T0H ; 563 | cpc DH, T2L ; 564 | cpc EL, T2H ; 565 | brge dg_de ;/ 566 | ; } 567 | ; } while (velocity < MvSpd); 568 | b41: ldsw D, MvSpd ;Has current velocity reached P6? 569 | cp BH, DL ;If reached, enter constant velocity mode. 570 | cpc CL, DH ; 571 | cpc CH, _0 ; 572 | brlo dg_ul ;/ 573 | 574 | ; // Calculate down ramp point: 575 | ; // up ramp width was (cur_pos-start_pos) 576 | ; // down ramp beginns at target - up_ramp_width = target - (cur-start) 577 | ; EL:D ramp_point = start_pos - cur_pos + target_pos 578 | movw DL, T4L ;Calculate down ramp point 579 | mov EL, T6L ; 580 | sub DL, T0H ; 581 | sbc DH, T2L ; 582 | sbc EL, T2H ; 583 | addw D, A ; 584 | adc EL, BL ;/ EL:DL = s.p. - c.p. + t.p. 585 | ; do { 586 | ; dg_add(); 587 | dg_cl: rcall dg_add ;---Constant velocity loop 588 | ; if (direction==0) { //Tflag==0) { 589 | brts b42 590 | ; if (cur_pos >= ramp_point) break; {// if (T0H:T2 >= EL:D) break; 591 | cp T0H, DL 592 | cpc T2L, DH 593 | cpc T2H, EL 594 | brge dg_de 595 | rjmp dg_cl 596 | ; } else { 597 | ; if (ramp_point >= cur_pos) break; {// if (EL:D < T0H:T2) goto dg_cl 598 | b42: cp DL, T0H 599 | cpc DH, T2L 600 | cpc EL, T2H 601 | brlt dg_cl 602 | ; } // do 603 | ; dg_add(); 604 | dg_de: rcall dg_add 605 | ; while ((velocity-=MvAcc) >= 0) { 606 | ; if (dg_add()==0) break; 607 | ; } 608 | dg_dl: lds DL, MvAcc+0 ;---Down ramp loop 609 | sub BH, DL ;Decrese velocity 610 | lds DL, MvAcc+1 ; 611 | sbc CL, DL ; 612 | sbc CH, _0 ;/ 613 | brcs dg_end 614 | rcall dg_add 615 | brlt dg_dl 616 | 617 | ; block_irq(); 618 | ; CtPos = target_pos; 619 | dg_end: ; End of action 620 | cli 621 | stdw Z+0, A 622 | std Z+2, BL 623 | ; while (block_irq(), Pos!=target_pos) { 624 | ; allow_irq(); 625 | ; if (receive() 626 | 627 | ; // irq blocking is not exactly right in the C translation 628 | ; do { 629 | ; block_irq(); 630 | ; if (cur_pos == target_pos) break; 631 | ; allow_irq(); 632 | ; } while (receive()==0); 633 | dge_lp: cli ; Wait until position stabled 634 | cpw _Pos, A 635 | cpc _PosX, BL 636 | sei 637 | breq b43 638 | push AL 639 | rcall receive 640 | pop AL 641 | breq dge_lp 642 | ; goto main; 643 | b43: rjmp main 644 | 645 | 646 | ; 647 | ; do one 1ms step with the current velocity 648 | ; 649 | ; wait for 1kHz time interval without any torque flags set. 650 | ; advance cur_pos by (velocity) 651 | ; if that does not overrun the target position, advance CtPos by (velocity) 652 | ; 653 | ; returns: s-flag=1, N^V=1: all ok 654 | ; N^V=0: overun the target position (brge will catch this) 655 | dg_add: 656 | cbr _Flags, bit7 ;Wait for 1kHz time interval 657 | b44: push AL ; 658 | rcall receive ; Break on ESC received 659 | breq b45 ; 660 | cpi AL, 0x1B ; 661 | breq dga_stop ; / 662 | b45: pop AL ; 663 | sbrs _Flags, 7 ; 664 | rjmp b44 ;/ 665 | sbrc _Flags, 6 ;Skip if torque limit did occur. 666 | rjmp dg_add ; 667 | sbrc _Flags, 5 ; 668 | rjmp dg_add ;/ 669 | ; if (direction == 0) { // if (Tflag==0) { 670 | ; cur_pos += velocity; //T2:T0 += BH:C 671 | brts b46 ;Increase commanded point by current velocity 672 | add T0L, BH ; 673 | adc T0H, CL ; 674 | adc T2L, CH ; 675 | adc T2H, CH ; 676 | ; if (cur_pos >= target) return 0; 677 | cp T0H, AL ; 678 | cpc T2L, AH ; 679 | cpc T2H, BL ; 680 | brge dga_ov ; 681 | rjmp b47 ; 682 | ; } else { 683 | ; cur_pos -= velocity; //T2:T0 -= C:BH 684 | b46: sub T0L, BH ; 685 | sbc T0H, CL ; 686 | sbc T2L, CH ; 687 | sbc T2H, CH ; 688 | ; if (cur_pos <= target) return 0; 689 | cp AL, T0H ; 690 | cpc AH, T2L ; 691 | cpc BL, T2H ; 692 | brge dga_ov ;/ 693 | ; } 694 | ; BEGIN_CRITICAL 695 | ; CtPos = cur_pos >> 8; 696 | ; END_CRITICAL; 697 | b47: cli 698 | std Z+0, T0H 699 | stdw Z+1, T2 700 | sei 701 | ; return (1); 702 | ses 703 | dga_ov: ret 704 | ; } 705 | 706 | dga_stop: 707 | pop AL 708 | pop AL 709 | pop AL 710 | rjmp main 711 | 712 | 713 | 714 | ;----------------------------------------------------------; 715 | ; Initialize servo system 716 | ; 717 | ;Call: AL = servo mode 718 | 719 | init_servo: 720 | cli 721 | sts Mode, AL ;Set servo mode 722 | ldiw Y, CtPos ;Clear cmmand regs and servo operators. 723 | b012: st Y+, _0 ; 724 | cpi YL, low(CtPos+11); 725 | brne b012 ;/ 726 | clrw _Pos ;Clear position counter 727 | clr _PosX ;/ 728 | sei 729 | led_off LED_ERROR ;Error LED off 730 | led_off LED_TORQUE ;Torque LED off 731 | led_on LED_READY ;Ready LED on 732 | ret 733 | 734 | 735 | 736 | ;----------------------------------------------------------; 737 | ; 83kHz Position capture and servo operation interrupt 738 | 739 | 740 | ; part 1: position capture 741 | ; 742 | 743 | ; input: PIND 744 | ; sideeffects/global variables: 745 | ; - PvEnc: previous A/B signal (r/w) 746 | ; - Pos/PosX: update the current position (r/w) 747 | ; - saved registers: T0L, SREG, Z (left on stack) 748 | ; modified registers: Z 749 | 750 | background: 751 | push T0L 752 | pushw Z 753 | in T0L, SREG ;Save flags 754 | 755 | ; ZL = PvEnc; 756 | mov ZL, _PvEnc ;ZL[1:0] = previous A/B signal 757 | ; PvEnc = swap(PIND); 758 | in _PvEnc, PIND ;Sample A/B signal into _PvEnc[1:0] 759 | swap _PvEnc ;/ 760 | ; if (PvEnc & 2) PvEnc ^= 1; 761 | ldi ZH, 1 ;Convert it to sequencial number. 762 | sbrc _PvEnc, 1 ; 763 | eor _PvEnc, ZH ;/ 764 | ; ZL = (ZL - PvEnc) & 3; 765 | sub ZL, _PvEnc ;Decode motion 766 | andi ZL, 3 ;/ 767 | ; if (ZL) { 768 | breq enc_zr ;-> Not moved 769 | ; if (ZL == 3) { 770 | ; // enc_rev 771 | ; PvDir = -1; 772 | ; Pos--; 773 | ; } else if (ZL == 1) { 774 | ; // enc_fwd 775 | ; PvDir = 1; 776 | ; Pos++; 777 | ; } else { 778 | ; // missing code recovery 779 | ; Pos += 2*PvDir; 780 | ; } 781 | cpi ZL, 3 ; 782 | breq enc_rev ;-> -1 count 783 | cpi ZL, 1 ; 784 | breq enc_fwd ;-> +1 count 785 | mov ZL, _PvDir ;-> Missing code recovery: 786 | mov ZH, _PvDir ; double count for previous direction 787 | lsl ZL ; 788 | asr ZH ;/ 789 | rjmp enc_add 790 | enc_rev:ldiw Z, -1 791 | rjmp b013 792 | ; } // if (ZL!=1) 793 | enc_fwd:ldiw Z, 1 794 | b013: mov _PvDir, ZL 795 | enc_add:addw _Pos, Z 796 | adc _PosX, ZH 797 | ; } // if (ZL) 798 | 799 | ; if (--CtDiv) return; 800 | enc_zr: 801 | dec _CtDiv ;Decrement 1/83 divider 802 | rjne bgnd_exit ;If not overflow, exit interrupt routine. 803 | 804 | ; End of 83 kHz position captureing process. Follows are 1kHz servo 805 | ; operation. It will be interrupted itself, but will not be re-entered. 806 | 807 | ; CtDiv = 83; 808 | ldi ZL, 83 ;Re-initialize 1/83 divider 809 | mov _CtDiv, ZL ;/ 810 | sei ;Enable interrupts 811 | pushw T0 812 | pushw T2 813 | pushw A 814 | pushw B 815 | pushw C 816 | push DL 817 | pushw Y 818 | ldiw Y, Parms ;Work area base pointer 819 | 820 | sbr _Flags, bit7 ; 1kHz interrupt flag 821 | 822 | ; T2 = Pos - PvPos; 823 | ; PvPos = Pos; 824 | lddw T2, Y+iPvPos ;Detect velocity 825 | cli ; 826 | subw T2, _Pos ; 827 | stdw Y+iPvPos, _Pos ; 828 | sei ; 829 | negw T2 ;/ T2 = velocity 830 | 831 | ; 832 | ldd AL, Y+iMode ;Branch by servo mode 833 | cpi AL, 3 ;Mode3? 834 | breq tap_position ;/ 835 | lddw T0, Y+iCtSub ;Get sub command 836 | cpi AL, 2 ;Mode2? 837 | breq tap_velocity ;/ 838 | cpi AL, 1 ;Mode1? 839 | rjeq tap_torque ;/ 840 | rjmp tap_voltage ;Mode0 841 | 842 | 843 | tap_position: 844 | cli ;Get position error (= velocity command) 845 | lddw T0, Y+iCtPos ; 846 | ldd BH, Y+iCtPos+2 ; 847 | subw T0, _Pos ; 848 | sbc BH, _PosX ; 849 | sei ;BH:T0 = position error 850 | 851 | ; if (pos_error >= LimSpd) pos_error = LimSpd; 852 | ; else if (pos_error <= -LimSpd) pos_error = -LimSpd; 853 | lddw A, Y+iLimSpd ;Velocity limit (P0) 854 | clr BL ; 855 | cpw T0, A ; 856 | cpc BH, BL ; 857 | brge b10 ; 858 | negw A ; 859 | com BL ; 860 | cpw T0, A ; 861 | cpc BH, BL ; 862 | brge tap_velocity ; 863 | b10: movw T0L, AL ;T0 = velocity command 864 | 865 | ; output a velocity value (mode 2) 866 | ; 867 | 868 | ; input: 869 | ; - T0: desired velocity value 870 | ; - T2: current velocity (in pules/ms) 871 | ; - Y: pointer to motor parameter block 872 | ; output: 873 | ; - T0: next torque value 874 | ; modified registers: A, B, Z 875 | tap_velocity: 876 | ; T0 -= speed{T2} * P1 877 | movw AL, T2L ;Velocity loop gain (P1) 878 | lddw B, Y+iGaSpd ; 879 | rcall muls1616 ;B = scaled velocity 880 | subw T0, B ;T0 = velocity error 881 | 882 | ; Z = T0 * GaTqP ; P term P2 883 | movw AL, T0L ;Velocity error P-gain (P2) 884 | lddw B, Y+iGaTqP ; 885 | rcall muls1616 ; 886 | movw ZL, BL ;Z = P term; 887 | 888 | ; Z += PvInt * GaTqI ; I term P3 889 | lddw A, Y+iPvInt ;Velocity error I-gain (P3) 890 | lddw B, Y+iGaTqI ; 891 | rcall muls1616 ; 892 | addw Z, B ;Z += I term; 893 | 894 | ; if (Z>= LimTrq) {Z=LimTrq; set_flag(6);} ;Torque limit P4 (LimTrq) 895 | cbr _Flags, bit6+bit5;Torque limit (P4) 896 | lddw B, Y+iLimTrq ; 897 | cpw Z, B ; 898 | brlt b20 ; 899 | movw ZL, BL ; 900 | sbr _Flags, bit6 ; 901 | ; if (Z< -LimTrq) {Z=-LimTrq; set_flag(5);} 902 | b20: neg BL ; 903 | com BH ; 904 | cpw Z, B ; 905 | brge b21 ; 906 | movw ZL, BL ; 907 | sbr _Flags, bit5 ;/ 908 | 909 | ; if ( ((T0<0)&&!flag(5)) || ((T0>=0)&&!flag(6)) ) PvInt += T0; 910 | b21: tst T0H ;PvInt += T0, with anti-windup 911 | brmi b22 ; 912 | sbrc _Flags, 6 ; 913 | rjmp b24 ; 914 | rjmp b23 ; 915 | b22: sbrc _Flags, 5 ; 916 | rjmp b24 ; 917 | ; PvInt += T0 918 | b23: lddw A, Y+iPvInt ; 919 | .ifdef USE_DECAY 920 | ;-- addition: decay for the integral value after steady state is reached 921 | ; (useful for static positioning applications like XY-tables) 922 | ; if (T0 || T2) A+=T0 923 | ; else {B=(A>>6); A-=B} 924 | 925 | mov BL, T0L ; check for (T0==0)&&(T2==0) 926 | or BL, T0H 927 | or BL, T2L 928 | or BL, T2H 929 | brne b28 930 | movw BL, AL ; B = A>>5 931 | asr BH,1 932 | ror BL 933 | asr BH,1 934 | ror BL 935 | asr BH,1 936 | ror BL 937 | asr BH,1 938 | ror BL 939 | asr BH,1 940 | ror BL 941 | subw A, B ; A -= B; 942 | rjmp b29 943 | .endif 944 | b28: addw A, T0 ; 945 | b29: stdw Y+iPvInt, A ;/ 946 | ; if (flags(5) || flags(6)) { 947 | ; led_on(LED_TORQUE); 948 | ; } else { 949 | ; led_off(LED_TORQUE); 950 | ; if (OvTmr-1)>0) { 951 | ; OvTmr--; 952 | ; if (OvTmr >= TL_TIME) goto servo_error; 953 | ; } 954 | ; } 955 | ; T0 = Z 956 | ; 957 | b24: mov AL, _Flags ;Check torque limiter timer 958 | andi AL, bit6+bit5 ; OvTmr is increased by 3 when torque limitter 959 | ;AL is overwritten right away, but the CPU flags survive 960 | lddw A, Y+iOvTmr ; works, decreased by 1 when no torque limit. 961 | breq b25 ; When the value reaches TL_TIME, servo turns 962 | led_on LED_TORQUE ; off and goes error state. 963 | addiw A, 3 ; 964 | rjmp b26 ; 965 | b25: led_off LED_TORQUE ; 966 | subiw A, 1 ; 967 | brlo b27 ; 968 | b26: stdw Y+iOvTmr, A ; 969 | ldiw B, TL_TIME ; 970 | cpw A, B ; 971 | brsh servo_error ;/ 972 | 973 | b27: movw T0L, ZL ;T0 = torque command 974 | 975 | ; output a torque/current value (mode 1) 976 | ; 977 | ; current_PWM{T0} += speed{T2}*P5; 978 | ; 979 | ; input: 980 | ; - T2: current velocity (in pules/ms) 981 | ; - Y: pointer to motor parameter block 982 | ; - T0: current PWM value (left over from the last round) 983 | ; output: 984 | ; - T0: next PWM value 985 | ; modified registers: A, B 986 | tap_torque: 987 | movw AL, T2L ;EG compensation (P5) 988 | lddw B, Y+iGaEG ; 989 | rcall muls1616 ; 990 | addw T0, B ;T0 = voltage command 991 | 992 | 993 | ; output a PWM value (mode 0) 994 | ; 995 | ; clip T0 to [-240;240] 996 | ; OCR1A = (T0/2) + 120; 997 | ; OCR1B = 120 - (T0/2); 998 | ; 999 | ; input: 1000 | ; - T0: value to output. 1001 | ; 1002 | ; valid outputs and side effects: 1003 | ; 1004 | ; modified registers: A 1005 | tap_voltage: 1006 | ldiw A, 240 ;Clip output voltage between -240 and +240. 1007 | cpw T0, A ; Limit minimum duty ratio to 15/16 for bootstrap 1008 | brge b30 ; type FET driver. 1009 | ldiw A, -240 ; 1010 | cpw T0, A ; 1011 | brge b31 ; 1012 | b30: movw T0L, AL ;T0 = PWM command 1013 | 1014 | b31: asrw T0 ;Set PWM register (OCR1A and ~OCR1B) 1015 | ldi AL, 120 ; 1016 | adc AL, T0L ; 1017 | ldi AH, 120 ; 1018 | sub AH, T0L ; 1019 | .if OCR1AL < 0x40 1020 | out OCR1AL, AL ; 1021 | out OCR1BL, AH ;/ 1022 | .else 1023 | sts OCR1AL, AL ; 1024 | sts OCR1BL, AH ;/ 1025 | .endif 1026 | 1027 | rcall disp_pos 1028 | 1029 | popw Y 1030 | pop DL 1031 | popw C 1032 | popw B 1033 | popw A 1034 | popw T2 1035 | popw T0 1036 | 1037 | bgnd_exit: ;End of encoder capture and servo operation 1038 | out SREG, T0L ;Restore flags 1039 | popw Z 1040 | pop T0L 1041 | reti 1042 | 1043 | 1044 | servo_error: 1045 | clr AL ;Enter Mode0 1046 | rcall init_servo ;/ 1047 | led_on LED_ERROR ;Error LED on 1048 | clrw T0 ;Output off 1049 | rjmp tap_voltage 1050 | 1051 | 1052 | 1053 | ;--------------------------------------; 1054 | ; Display position counter 1055 | 1056 | disp_pos: 1057 | lds DL,Disp+8 1058 | inc DL 1059 | cpi DL,8 1060 | brcs dp_out 1061 | 1062 | mov AL,_PosL 1063 | mov AH,_PosH 1064 | mov BL,_PosX 1065 | ldi CH,0 1066 | sbrs BL,7 1067 | rjmp b50 1068 | ldi CH,0x40 1069 | com AL 1070 | com AH 1071 | com BL 1072 | adc AL,_0 1073 | adc AH,_0 1074 | adc BL,_0 1075 | b50: ldiw Y,Disp 1076 | dp_l1: ldi CL,24 1077 | ldi BH,0 1078 | dp_l2: lsl AL 1079 | rol AH 1080 | rol BL 1081 | rol BH 1082 | cpi BH,10 1083 | brcs b51 1084 | subi BH,10 1085 | inc AL 1086 | b51: dec CL 1087 | brne dp_l2 1088 | ldiw Z,t_7seg*2 1089 | add ZL,BH 1090 | adc ZH,_0 1091 | lpm CL,Z 1092 | st Y+,CL 1093 | cp AL,_0 1094 | cpc AH,_0 1095 | cpc BL,_0 1096 | brne dp_l1 1097 | b52: st Y+,CH 1098 | clr CH 1099 | cpi YL,low(Disp+8) 1100 | brne b52 1101 | clr DL 1102 | 1103 | .ifdef USIDR 1104 | ; This code uses the USI device of the tiny2313. 1105 | ; - write data to USIDR 1106 | ; - double-toggle USCK 8 times to shift out the data 1107 | ; - write 0x80 to USIDR to set DO to high 1108 | ; - toggle DI to advance to the next digit. 1109 | dp_out: ldiw Y,Disp 1110 | std Y+8,DL 1111 | add YL,DL 1112 | adc YH,_0 1113 | ld AL,Y 1114 | com AL 1115 | out USIDR,AL 1116 | ldi AH,0x80 1117 | ldi CL,8 1118 | b53: out PINB,AH 1119 | dec CL 1120 | out PINB,AH 1121 | brne b53 1122 | cpi DL,0 1123 | brne b54 1124 | ldi AH,0 1125 | b54: out USIDR,AH 1126 | sbi PORTB,5 1127 | cbi PORTB,5 1128 | ret 1129 | .else 1130 | ;FIXME: add SPI code for ATmega. 1131 | dp_out: 1132 | ret 1133 | .endif 1134 | 1135 | 1136 | 1137 | 1138 | ;--------------------------------------; 1139 | ; 16bit * 16bit signed multiply 1140 | ; 1141 | ; Multiplier: A(signed int) 1142 | ; Multiplicand: B(unsigned, 8.8 fraction) 1143 | ; Result: B(signed int) 1144 | ; Clk: 181(max) 1145 | 1146 | muls1616: 1147 | clt 1148 | tst AH 1149 | brpl b60 1150 | set 1151 | negw A 1152 | 1153 | b60: subw C, C ; clear high 16bit. 1154 | ldi DL, 17 ; DL = loop count 1155 | b61: brcc b62 ; ---- calculating loop 1156 | addw C, A ; 1157 | b62: rorw C ; 1158 | rorw B ; 1159 | dec DL ; if (--DL > 0) 1160 | brne b61 ; continue loop; 1161 | 1162 | mov BL, BH 1163 | mov BH, CL 1164 | 1165 | brtc b63 ; Negate the result if multiplier was negative. 1166 | negw B 1167 | b63: ret 1168 | 1169 | 1170 | 1171 | ;--------------------------------------; 1172 | ; Input a command line into LineBuf. 1173 | ; 1174 | ; returns: 1175 | ; - BH: number of char in buffer 1176 | ; - X: pointer to LineBuf 1177 | get_line: 1178 | ldiw X,LineBuf 1179 | ldi BH,0 1180 | rl_lp: rcall receive 1181 | breq rl_lp 1182 | st X,AL 1183 | cpi AL,0x0d ; CR 1184 | brne b80 1185 | ldiw X,LineBuf 1186 | rjmp echo 1187 | b80: cpi AL,0x08 ; BS 1188 | brne b81 1189 | cpi BH,0 1190 | breq rl_lp 1191 | rcall echo 1192 | sbiw XL,1 1193 | dec BH 1194 | rjmp rl_lp 1195 | b81: cpi AL,' ' ; SP 1196 | brcs rl_lp 1197 | cpi BH,20-1 1198 | breq rl_lp 1199 | rcall echo 1200 | adiw XL,1 1201 | inc BH 1202 | rjmp rl_lp 1203 | 1204 | 1205 | 1206 | ;--------------------------------------; 1207 | ; Send ROM string 1208 | ; 1209 | ; Call: Z = top of the string (ASCIZ) 1210 | ; Ret: Z = next string 1211 | 1212 | b82: rcall xmit 1213 | dp_str: lpm AL, Z+ 1214 | tst AL 1215 | brne b82 1216 | ret 1217 | 1218 | 1219 | ;--------------------------------------; 1220 | ; Get value of decimal string 1221 | ; 1222 | ; Call: X -> ASCII string 1223 | ; Ret: X = updated 1224 | ; if C=1: error 1225 | ; elsif Z=1: end of line, value=0 1226 | ; else: BL:AL = 24bit value 1227 | ; 1228 | ; Positive: "300" 1229 | ; Negative: "-125000" 1230 | 1231 | get_val: 1232 | ; flag_neg = 0; // use T as flag for negative value 1233 | clt 1234 | ; val = 0; 1235 | clr AL 1236 | clr AH 1237 | clr BL 1238 | ; do { 1239 | ; BH = *X++; 1240 | b83: ld BH,X+ 1241 | ; if (BH < ' ') return (c1z0); // return empty line 1242 | cpi BH,' ' 1243 | brcs gd_n 1244 | breq b83 1245 | ; } while (BH == ' '); // ignore spaces 1246 | ; if (BH == '-') { 1247 | ; flag_neg = 1; 1248 | ; BH = *X++; 1249 | ; } 1250 | cpi BH,'-' 1251 | brne b84 1252 | set 1253 | gd_l: ld BH,X+ ; in C this would be at the end of loop 1254 | ; do { 1255 | ; if (BH <= ' ') goto gd_e; // found end of number, return success 1256 | b84: cpi BH,' '+1 1257 | brcs gd_e 1258 | ; BH -= '0'; 1259 | subi BH,'0' 1260 | ; if (BH < 0) goto gd_x; // return syntax error 1261 | brcs gd_x ; could be left out, this is covered by the next compare. 1262 | ; if (BH >= 10) goto gd_x; // return syntax error 1263 | cpi BH,10 1264 | brcc gd_x 1265 | ; val = val*10 + BH; // this loop is a 24x8 bit multiplication 1266 | ldi CL, 25 1267 | ldi CH, 10 1268 | sub r0, r0 1269 | b85: lsr r0 1270 | ror BL 1271 | ror AH 1272 | ror AL 1273 | brcc b86 1274 | add r0, CH 1275 | b86: dec CL 1276 | brne b85 1277 | add AL, BH 1278 | adc AH, _0 1279 | adc BL, _0 1280 | rjmp gd_l 1281 | ; BH = *X++; // implemented in assembler at the beginning 1282 | ; } while (1); 1283 | ; return (z1c1); // return value for error 1284 | gd_x: sec 1285 | sez 1286 | ret 1287 | ; X-- 1288 | ; if (flag_neg) { 1289 | ; val = -val; 1290 | ; } 1291 | ; return (c0); // return value for success 1292 | gd_e: sbiw XL,1 1293 | brtc b87 1294 | com AL 1295 | com AH 1296 | com BL 1297 | subi AL,-1 1298 | sbci AH,-1 1299 | sbci BL,-1 1300 | b87: clc 1301 | ret 1302 | ; X-- 1303 | ; return (z1c0); // return value for empty line 1304 | gd_n: sbiw XL,1 1305 | clc 1306 | sez 1307 | ret 1308 | 1309 | 1310 | 1311 | ;--------------------------------------; 1312 | ; Display a value in decimal string 1313 | ; 1314 | ; Call: BL:A = 24bit signed value to be displayed 1315 | ; Ret: BL:A = broken 1316 | 1317 | ; This implements the standard algorithmn for unsigned division Q,R=N/D with 1318 | ; N and Q both in BL:A, R in BH and D fixed to 10. 1319 | ; 1320 | ; Q := 0 -- Initialize quotient and remainder to zero 1321 | ; R := 0 1322 | ; for i := n − 1 .. 0 do -- Where n is number of bits in N 1323 | ; R := R << 1 -- Left-shift R by 1 bit 1324 | ; R(0) := N(i) -- Set the least-significant bit of R equal to bit i of the numerator 1325 | ; if R ≥ D then 1326 | ; R := R − D 1327 | ; Q(i) := 1 1328 | ; end 1329 | ; end 1330 | 1331 | dp_dec: ldi CH,' ' 1332 | ; CH = ' '; 1333 | ; if (val < 0) { 1334 | sbrs BL, 7 1335 | rjmp b88 1336 | ; val = -val; 1337 | com AL 1338 | com AH 1339 | com BL 1340 | adc AL,_0 1341 | adc AH,_0 1342 | adc BL,_0 1343 | ; CH = '-' 1344 | ldi CH,'-' 1345 | ; } 1346 | ; // convert the value into an ASCII string on the stack 1347 | ; T0L = 0; 1348 | b88: clr T0L ;digit counter 1349 | ; do { 1350 | ; T0L++; 1351 | ; BH = 0; 1352 | ; CL=24; 1353 | b088: inc T0L ;---- decimal string generating loop 1354 | clr BH ;var1 /= 10; 1355 | ldi CL,24 ; 1356 | ; do { 1357 | ; var1 *= 2; 1358 | b89: lslw A ; 1359 | rolw B ; 1360 | ; if ((var1 >> 24) >= 10) { 1361 | cpi BH,10 ; 1362 | brcs b89a ; 1363 | ; var1 -= (10<<24); 1364 | ; var1++; 1365 | subi BH,10 ; 1366 | inc AL ; 1367 | ; } while (--CL); 1368 | b89a: dec CL ; 1369 | brne b89 ;/ 1370 | ; push(BH+'0'); 1371 | addi BH,'0' ;Push the remainder (a decimal digit) 1372 | push BH ;/ 1373 | ; } while (var1); 1374 | cp AL,_0 ;if(var1 =! 0) 1375 | cpc AH,_0 ; continue digit loop; 1376 | cpc BL,_0 ; 1377 | brne b088 ;/ 1378 | ; // output the stored string in opposite order 1379 | ; xmit(CH); // sign 1380 | mov AL, CH ;Sign 1381 | rcall xmit ;/ 1382 | ; do { 1383 | ; xmit(pop()); 1384 | ; } while (--T0L); 1385 | b89b: pop AL ;Transmit decimal string 1386 | rcall xmit ;<-- Put a char to memory, console or any other display device 1387 | dec T0L ; 1388 | brne b89b ;/ 1389 | ret 1390 | 1391 | 1392 | ;--------------------------------------; 1393 | ; Serial I/O driver 1394 | 1395 | ; Transmit AL. 1396 | echo: 1397 | xmit: 1398 | .if UDR < 0x40 1399 | b90: sbis UCSRA, UDRE 1400 | rjmp b90 1401 | out UDR, AL 1402 | ret 1403 | .else 1404 | push AH ; not sure if AH is really important. 1405 | b90: 1406 | lds AH, UCSRA 1407 | sbrs AH, UDRE 1408 | rjmp b90 1409 | sts UDR, AL 1410 | pop AH 1411 | ret 1412 | .endif 1413 | 1414 | ; Receive a char into AL. (ZR=no data) 1415 | ; 1416 | ; modified regs: AL 1417 | ; return status: Z=1: no data, Z=0: data in AL valid 1418 | ; global variables: 1419 | ; read: RxBuf: Rp, Wp, Buff 1420 | ; write: Rp 1421 | receive: 1422 | push AH 1423 | pushw Y 1424 | ldiw Y, RxBuf 1425 | cli 1426 | ldd AH, Y+0 1427 | ldd AL, Y+1 1428 | cp AH, AL 1429 | breq b91 1430 | add YL, AH 1431 | ldd AL, Y+2 1432 | sub YL, AH 1433 | inc AH 1434 | andi AH, 15 1435 | std Y+0, AH 1436 | clz 1437 | b91: sei 1438 | popw Y 1439 | pop AH 1440 | ret 1441 | 1442 | rxint: ;USART0 Rx ready 1443 | push AL 1444 | in AL, SREG 1445 | push BL 1446 | pushw A 1447 | .if UCSRB < 0x40 1448 | in BL, UDR 1449 | cbi UCSRB, RXCIE 1450 | .else 1451 | lds BL, UDR 1452 | lds AL, UCSRB 1453 | andi AL, low(~(1< Position command reg. 349 | lddw T4, Z+0 ;T6L:T4 = start posision 350 | ldd T6L, Z+2 ;/ 351 | mov T0H, T4L ;r3:r0 = commanded posision 352 | mov T2L, T4H ; 353 | mov T2H, T6L ;/ 354 | rcall get_val ;sub command 355 | rjeq cmd_err ;/ 356 | cpi AL, 0 ;G0? 357 | breq dg_0 ;/ 358 | cpi AL, 1 ;G1? 359 | breq dg_1 ;/ 360 | rjmp cmd_err 361 | 362 | ; Rectanguler velocity profile 363 | dg_1: rcall get_val ;BL:AL = target posision 364 | rjeq cmd_err ;/ 365 | pushw A 366 | push BL 367 | rcall get_val ;CH:BH = velocity 368 | mov BH, AL ; 369 | mov CL, AH ; 370 | mov CH, _0 ;/ 371 | pop BL 372 | popw A 373 | rjeq cmd_err ;/ 374 | cpw A, T4 ;T = direction 375 | cpc BL, T6L ; 376 | clt ; 377 | brge PC+2 ; 378 | set ;/ 379 | clr r0 ; 380 | rcall dg_add ;---Constant velocity loop 381 | brlt PC-1 ; 382 | rjmp dg_end ;/ 383 | 384 | ; Trapezoidal velocity profile 385 | dg_0: rcall get_val ;BL:AL = target posision 386 | rjeq cmd_err ;/ 387 | cpw A, T4 ;T = direction 388 | cpc BL, T6L ; 389 | clt ; 390 | brge PC+2 ; 391 | set ;/ 392 | clr r0 ; 393 | clr BH ;CL:BH = start velocity 394 | clrw C ;/ 395 | 396 | dg_ul: lds DL, MvAcc+0 ;---Up ramp loop 397 | add BH, DL ;Increace velocity 398 | lds DL, MvAcc+1 ; 399 | adc CL, DL ; 400 | adc CH, _0 ;/ 401 | rcall dg_add 402 | brge dg_end 403 | movw DL, AL ;Check current posisiton has passed half of distance. 404 | mov EL, BL ;If passed, enter to down ramp. 405 | sub DL, T4L ; 406 | sbc DH, T4H ; 407 | sbc EL, T6L ; 408 | asr EL ; 409 | rorw D ; 410 | addw D, T4 ; 411 | adc EL, T6L ; 412 | brts PC+6 ; 413 | cp T0H, DL ; 414 | cpc T2L, DH ; 415 | cpc T2H, EL ; 416 | brge dg_de ; 417 | rjmp PC+5 ; 418 | cp DL, T0H ; 419 | cpc DH, T2L ; 420 | cpc EL, T2H ; 421 | brge dg_de ;/ 422 | ldsw D, MvSpd ;Has current velocity reached P6? 423 | cp BH, DL ;If reached, enter constant velocity mode. 424 | cpc CL, DH ; 425 | cpc CH, _0 ; 426 | brcs dg_ul ;/ 427 | 428 | movw DL, T4L ;Calcurate down ramp point 429 | mov EL, T6L ; 430 | sub DL, T0H ; 431 | sbc DH, T2L ; 432 | sbc EL, T2H ; 433 | addw D, A ; 434 | adc EL, BL ;/ EL:DL = s.p. - c.p. + t.p. 435 | dg_cl: rcall dg_add ;---Constant velocity loop 436 | brts PC+6 437 | cp T0H, DL 438 | cpc T2L, DH 439 | cpc T2H, EL 440 | brge dg_de 441 | rjmp dg_cl 442 | cp DL, T0H 443 | cpc DH, T2L 444 | cpc EL, T2H 445 | brlt dg_cl 446 | 447 | dg_de: rcall dg_add 448 | dg_dl: lds DL, MvAcc+0 ;---Down ramp loop 449 | sub BH, DL ;Decrese velocity 450 | lds DL, MvAcc+1 ; 451 | sbc CL, DL ; 452 | sbc CH, _0 ;/ 453 | brcs dg_end 454 | rcall dg_add 455 | brlt dg_dl 456 | 457 | dg_end: ; End of action 458 | cli 459 | stdw Z+0, A 460 | std Z+2, BL 461 | dge_lp: cli ; Wait until position stabled 462 | cpw _Pos, A 463 | cpc _PosX, BL 464 | sei 465 | breq PC+5 466 | push AL 467 | rcall receive 468 | pop AL 469 | breq dge_lp 470 | rjmp main 471 | 472 | 473 | 474 | dg_add: 475 | cbr _Flags, bit7 ;Wait for 1kHz time interval 476 | push AL ; 477 | rcall receive ; Break on ESC received 478 | breq PC+3 ; 479 | cpi AL, 0x1B ; 480 | breq dga_stop ; / 481 | pop AL ; 482 | sbrs _Flags, 7 ; 483 | rjmp PC-7 ;/ 484 | sbrc _Flags, 6 ;Skip if torque limit has being occured. 485 | rjmp dg_add ; 486 | sbrc _Flags, 5 ; 487 | rjmp dg_add ;/ 488 | brts PC+10 ;Increase commanded point by current velocity 489 | add T0L, BH ; 490 | adc T0H, CL ; 491 | adc T2L, CH ; 492 | adc T2H, CH ; 493 | cp T0H, AL ; 494 | cpc T2L, AH ; 495 | cpc T2H, BL ; 496 | brge dga_ov ; 497 | rjmp PC+9 ; 498 | sub T0L, BH ; 499 | sbc T0H, CL ; 500 | sbc T2L, CH ; 501 | sbc T2H, CH ; 502 | cp AL, T0H ; 503 | cpc AH, T2L ; 504 | cpc BL, T2H ; 505 | brge dga_ov ;/ 506 | cli 507 | std Z+0, T0H 508 | stdw Z+1, T2 509 | sei 510 | ses 511 | dga_ov: ret 512 | 513 | dga_stop: 514 | pop AL 515 | pop AL 516 | pop AL 517 | rjmp main 518 | 519 | 520 | 521 | ;----------------------------------------------------------; 522 | ; Initialize servo system 523 | ; 524 | ;Call: AL = servo mode 525 | 526 | init_servo: 527 | cli 528 | sts Mode, AL ;Set servo mode 529 | ldiw Y, CtPos ;Clear cmmand regs and servo operators. 530 | st Y+, _0 ; 531 | cpi YL, low(CtPos+11); 532 | brne PC-2 ;/ 533 | clrw _Pos ;Clear position counter 534 | clr _PosX ;/ 535 | sei 536 | cbi PORTB, 0 ;Error LED off 537 | cbi PORTB, 1 ;Torque LED off 538 | sbi PORTB, 2 ;Ready LED on 539 | ret 540 | 541 | 542 | 543 | ;----------------------------------------------------------; 544 | ; 83kHz Position capture and servo operation interrupt 545 | 546 | background: 547 | push T0L 548 | pushw Z 549 | in T0L, SREG ;Save flags 550 | 551 | mov ZL, _PvEnc ;ZL[1:0] = previous A/B signal 552 | in _PvEnc, PIND ;Sample A/B signal into _PvEnc[1:0] 553 | swap _PvEnc ;/ 554 | ldi ZH, 1 ;Convert it to sequencial number. 555 | sbrc _PvEnc, 1 ; 556 | eor _PvEnc, ZH ;/ 557 | sub ZL, _PvEnc ;Decode motion 558 | andi ZL, 3 ;/ 559 | breq enc_zr ;-> Not moved 560 | cpi ZL, 3 ; 561 | breq enc_rev ;-> -1 count 562 | cpi ZL, 1 ; 563 | breq enc_fwd ;-> +1 count 564 | mov ZL, _PvDir ;-> Missing code recovery: 565 | mov ZH, _PvDir ; double count for previous direction 566 | lsl ZL ; 567 | asr ZH ;/ 568 | rjmp enc_add 569 | enc_rev:ldiw Z, -1 570 | rjmp PC+3 571 | enc_fwd:ldiw Z, 1 572 | mov _PvDir, ZL 573 | enc_add:addw _Pos, Z 574 | adc _PosX, ZH 575 | enc_zr: 576 | dec _CtDiv ;Decrement 1/83 divider 577 | rjne bgnd_exit ;If not overflow, exit interrupt routine. 578 | 579 | ; End of 83 kHz position captureing process. Follows are 1kHz servo 580 | ; operation. It will be interrupted itself, but will not be re-entered. 581 | 582 | ldi ZL, 83 ;Re-initialize 1/83 divider 583 | mov _CtDiv, ZL ;/ 584 | sei ;Enable interrupts 585 | pushw T0 586 | pushw T2 587 | pushw A 588 | pushw B 589 | pushw C 590 | push DL 591 | pushw Y 592 | ldiw Y, Parms ;Work area base pointer 593 | 594 | sbr _Flags, bit7 ; 1kHz inerrupt flag 595 | 596 | lddw T2, Y+iPvPos ;Detect velocity 597 | cli ; 598 | subw T2, _Pos ; 599 | stdw Y+iPvPos, _Pos ; 600 | sei ; 601 | negw T2 ;/ T2 = velocity 602 | 603 | ldd AL, Y+iMode ;Branch by servo mode 604 | cpi AL, 3 ;Mode3? 605 | breq tap_position ;/ 606 | lddw T0, Y+iCtSub ;Get sub command 607 | cpi AL, 2 ;Mode2? 608 | breq tap_velocity ;/ 609 | cpi AL, 1 ;Mode1? 610 | rjeq tap_torque ;/ 611 | rjmp tap_voltage ;Mode0 612 | 613 | 614 | tap_position: 615 | cli ;Get position error (= velocity command) 616 | lddw T0, Y+iCtPos ; 617 | ldd BH, Y+iCtPos+2 ; 618 | subw T0, _Pos ; 619 | sbc BH, _PosX ; 620 | sei ;BL:T0 = position error 621 | 622 | lddw A, Y+iLimSpd ;Velocity limit (P0) 623 | clr BL ; 624 | cpw T0, A ; 625 | cpc BH, BL ; 626 | brge PC+10 ; 627 | negw A ; 628 | com BL ; 629 | cpw T0, A ; 630 | cpc BH, BL ; 631 | brge PC+2 ; 632 | movw T0L, AL ;T0 = velocity command 633 | 634 | 635 | tap_velocity: 636 | movw AL, T2L ;Velocity loop gain (P1) 637 | lddw B, Y+iGaSpd ; 638 | rcall muls1616 ;B = scaled velocity 639 | subw T0, B ;T0 = velocity error 640 | 641 | movw AL, T0L ;Velocity error P-gain (P2) 642 | lddw B, Y+iGaTqP ; 643 | rcall muls1616 ; 644 | movw ZL, BL ;Z = P term; 645 | 646 | lddw A, Y+iPvInt ;Velocity error I-gain (P3) 647 | lddw B, Y+iGaTqI ; 648 | rcall muls1616 ; 649 | addw Z, B ;Z += I term; 650 | 651 | cbr _Flags, bit6+bit5;Torque limit (P4) 652 | lddw B, Y+iLimTrq ; 653 | cpw Z, B ; 654 | brlt PC+3 ; 655 | movw ZL, BL ; 656 | sbr _Flags, bit6 ; 657 | neg BL ; 658 | com BH ; 659 | cpw Z, B ; 660 | brge PC+3 ; 661 | movw ZL, BL ; 662 | sbr _Flags, bit5 ;/ 663 | 664 | tst T0H ;PvInt += T0, with anti-windup 665 | brmi PC+4 ; 666 | sbrc _Flags, 6 ; 667 | rjmp PC+10 ; 668 | rjmp PC+3 ; 669 | sbrc _Flags, 5 ; 670 | rjmp PC+7 ; 671 | lddw A, Y+iPvInt ; 672 | addw A, T0 ; 673 | stdw Y+iPvInt, A ;/ 674 | 675 | mov AL, _Flags ;Check torque limiter timer 676 | andi AL, bit6+bit5 ; OvTmr is increased by 3 when torque limitter 677 | lddw A, Y+iOvTmr ; works, decreased by 1 when no torque limit. 678 | breq PC+5 ; When the value reaches TL_TIME, servo turns 679 | sbi PORTB, 1 ; off and goes error state. 680 | addiw A, 3 ; 681 | rjmp PC+5 ; 682 | cbi PORTB, 1 ; 683 | subiw A, 1 ; 684 | brcs PC+8 ; 685 | stdw Y+iOvTmr, A ; 686 | ldiw B, TL_TIME ; 687 | cpw A, B ; 688 | brcc servo_error ;/ 689 | 690 | movw T0L, ZL ;T0 = torque command 691 | 692 | 693 | tap_torque: 694 | movw AL, T2L ;EG compensation (P5) 695 | lddw B, Y+iGaEG ; 696 | rcall muls1616 ; 697 | addw T0, B ;T0 = voltage command 698 | 699 | 700 | tap_voltage: 701 | ldiw A, 240 ;Clip output voltage between -240 and +240. 702 | cpw T0, A ; Limit minimum duty ratio to 15/16 for bootstrap 703 | brge PC+6 ; type FET driver. 704 | ldiw A, -240 ; 705 | cpw T0, A ; 706 | brge PC+2 ; 707 | movw T0L, AL ;T0 = PWM command 708 | 709 | asrw T0 ;Set PWM register (OCR1A and ~OCR1B) 710 | ldi AL, 120 ; 711 | adc AL, T0L ; 712 | ldi AH, 120 ; 713 | sub AH, T0L ; 714 | out OCR1AL, AL ; 715 | out OCR1BL, AH ;/ 716 | 717 | rcall disp_pos 718 | 719 | popw Y 720 | pop DL 721 | popw C 722 | popw B 723 | popw A 724 | popw T2 725 | popw T0 726 | 727 | bgnd_exit: ;End of encoder capture and servo operation 728 | out SREG, T0L ;Restore flags 729 | popw Z 730 | pop T0L 731 | reti 732 | 733 | 734 | servo_error: 735 | clr AL ;Enter Mode0 736 | rcall init_servo ;/ 737 | sbi PORTB, 0 ;Error LED on 738 | clrw T0 ;Output off 739 | rjmp tap_voltage 740 | 741 | 742 | 743 | ;--------------------------------------; 744 | ; Display position counter 745 | 746 | disp_pos: 747 | lds DL,Disp+8 748 | inc DL 749 | cpi DL,8 750 | brcs dp_out 751 | 752 | mov AL,_PosL 753 | mov AH,_PosH 754 | mov BL,_PosX 755 | ldi CH,0 756 | sbrs BL,7 757 | rjmp PC+8 758 | ldi CH,0x40 759 | com AL 760 | com AH 761 | com BL 762 | adc AL,_0 763 | adc AH,_0 764 | adc BL,_0 765 | ldiw Y,Disp 766 | dp_l1: ldi CL,24 767 | ldi BH,0 768 | dp_l2: lsl AL 769 | rol AH 770 | rol BL 771 | rol BH 772 | cpi BH,10 773 | brcs PC+3 774 | subi BH,10 775 | inc AL 776 | dec CL 777 | brne dp_l2 778 | ldiw Z,t_7seg*2 779 | add ZL,BH 780 | adc ZH,_0 781 | lpm CL,Z 782 | st Y+,CL 783 | cp AL,_0 784 | cpc AH,_0 785 | cpc BL,_0 786 | brne dp_l1 787 | st Y+,CH 788 | clr CH 789 | cpi YL,Disp+8 790 | brne PC-3 791 | clr DL 792 | 793 | dp_out: ldiw Y,Disp 794 | std Y+8,DL 795 | add YL,DL 796 | adc YH,_0 797 | ld AL,Y 798 | com AL 799 | out USIDR,AL 800 | ldi AH,0x80 801 | ldi CL,8 802 | out PINB,AH 803 | dec CL 804 | out PINB,AH 805 | brne PC-3 806 | cpi DL,0 807 | brne PC+2 808 | ldi AH,0 809 | out USIDR,AH 810 | sbi PORTB,5 811 | cbi PORTB,5 812 | ret 813 | 814 | 815 | 816 | 817 | 818 | ;--------------------------------------; 819 | ; 16bit * 16bit signed multiply 820 | ; 821 | ; Multiplyer: A(signed int) 822 | ; Multiplicand: B(unsigned, 8.8 fraction) 823 | ; Result: B(signed int) 824 | ; Clk: 181(max) 825 | 826 | muls1616: 827 | clt 828 | tst AH 829 | brpl PC+6 830 | set 831 | negw A 832 | 833 | subw C, C ; clear high 16bit. 834 | ldi DL, 17 ; DL = loop count 835 | brcc PC+3 ; ---- calcurating loop 836 | addw C, A ; 837 | rorw C ; 838 | rorw B ; 839 | dec DL ; if (--DL > 0) 840 | brne PC-8 ; continue loop; 841 | 842 | mov BL, BH 843 | mov BH, CL 844 | 845 | brtc PC+5 ; Negate the result if multiplyer was negative. 846 | negw B 847 | ret 848 | 849 | 850 | 851 | ;--------------------------------------; 852 | ; Input a command line into LineBuf. 853 | 854 | get_line: 855 | ldiw X,LineBuf 856 | ldi BH,0 857 | rl_lp: rcall receive 858 | breq PC-1 859 | st X,AL 860 | cpi AL,0x0d ; CR 861 | brne PC+4 862 | ldiw X,LineBuf 863 | rjmp echo 864 | cpi AL,0x08 ; BS 865 | brne PC+7 866 | cpi BH,0 867 | breq rl_lp 868 | rcall echo 869 | sbiw XL,1 870 | dec BH 871 | rjmp rl_lp 872 | cpi AL,' ' ; SP 873 | brcs rl_lp 874 | cpi BH,20-1 875 | breq rl_lp 876 | rcall echo 877 | adiw XL,1 878 | inc BH 879 | rjmp rl_lp 880 | 881 | 882 | 883 | ;--------------------------------------; 884 | ; Send ROM string 885 | ; 886 | ; Call: Z = top of the string (ASCIZ) 887 | ; Ret: Z = next string 888 | 889 | dp_str: lpm AL, Z+ 890 | tst AL 891 | brne PC+2 892 | ret 893 | rcall xmit 894 | rjmp dp_str 895 | 896 | 897 | ;--------------------------------------; 898 | ; Get value of decimal string 899 | ; 900 | ; Call: X -> ASCII string 901 | ; Ret: X = updated 902 | ; if C=1: error 903 | ; elsif Z=1: end of line, value=0 904 | ; else: BL:AL = 24bit value 905 | ; 906 | ; Positive: "300" 907 | ; Negative: "-125000" 908 | 909 | get_val: 910 | clt 911 | clr AL 912 | clr AH 913 | clr BL 914 | ld BH,X+ 915 | cpi BH,' ' 916 | brcs gd_n 917 | breq PC-3 918 | cpi BH,'-' 919 | brne PC+3 920 | set 921 | gd_l: ld BH,X+ 922 | cpi BH,' '+1 923 | brcs gd_e 924 | subi BH,'0' 925 | brcs gd_x 926 | cpi BH,10 927 | brcc gd_x 928 | ldi CL, 25 929 | ldi CH, 10 930 | sub r0, r0 931 | lsr r0 932 | ror BL 933 | ror AH 934 | ror AL 935 | brcc PC+2 936 | add r0, CH 937 | dec CL 938 | brne PC-7 939 | add AL, BH 940 | adc AH, _0 941 | adc BL, _0 942 | rjmp gd_l 943 | gd_x: sec 944 | sez 945 | ret 946 | gd_e: sbiw XL,1 947 | brtc PC+7 948 | com AL 949 | com AH 950 | com BL 951 | subi AL,-1 952 | sbci AH,-1 953 | sbci BL,-1 954 | clc 955 | ret 956 | gd_n: sbiw XL,1 957 | clc 958 | sez 959 | ret 960 | 961 | 962 | 963 | ;--------------------------------------; 964 | ; Display a value in decimal string 965 | ; 966 | ; Call: BL:A = 24bit signed value to be displayed 967 | ; Ret: BL:A = broken 968 | 969 | dp_dec: ldi CH,' ' 970 | sbrs BL, 7 971 | rjmp PC+8 972 | com AL 973 | com AH 974 | com BL 975 | adc AL,_0 976 | adc AH,_0 977 | adc BL,_0 978 | ldi CH,'-' 979 | clr T0L ;digit counter 980 | inc T0L ;---- decimal string generating loop 981 | clr BH ;var1 /= 10; 982 | ldi CL,24 ; 983 | lslw A ; 984 | rolw B ; 985 | cpi BH,10 ; 986 | brcs PC+3 ; 987 | subi BH,10 ; 988 | inc AL ; 989 | dec CL ; 990 | brne PC-9 ;/ 991 | addi BH,'0' ;Push the remander (a decimal digit) 992 | push BH ;/ 993 | cp AL,_0 ;if(var1 =! 0) 994 | cpc AH,_0 ; continue digit loop; 995 | cpc BL,_0 ; 996 | brne PC-18 ;/ 997 | mov AL, CH ;Sign 998 | rcall xmit ;/ 999 | pop AL ;Transmit decimal string 1000 | rcall xmit ;<-- Put a char to memory, console or any other display device 1001 | dec T0L ; 1002 | brne PC-3 ;/ 1003 | ret 1004 | 1005 | 1006 | ;--------------------------------------; 1007 | ; Serial I/O driver 1008 | 1009 | ; Transmit AL. 1010 | echo: 1011 | xmit: sbis UCSRA, UDRE 1012 | rjmp PC-1 1013 | out UDR, AL 1014 | ret 1015 | 1016 | receive:; Receive a char into AL. (ZR=no data) 1017 | push AH 1018 | pushw Y 1019 | ldiw Y, RxBuf 1020 | cli 1021 | ldd AH, Y+0 1022 | ldd AL, Y+1 1023 | cp AH, AL 1024 | breq PC+8 1025 | add YL, AH 1026 | ldd AL, Y+2 1027 | sub YL, AH 1028 | inc AH 1029 | andi AH, 15 1030 | std Y+0, AH 1031 | clz 1032 | sei 1033 | popw Y 1034 | pop AH 1035 | ret 1036 | 1037 | rxint: ;USART0 Rx ready 1038 | push AL 1039 | in AL, SREG 1040 | push BL 1041 | in BL, UDR 1042 | cbi UCSRB, RXCIE 1043 | sei 1044 | pushw A 1045 | pushw Y 1046 | ldiw Y, RxBuf 1047 | ldd AL, Y+0 1048 | ldd AH, Y+1 1049 | inc AH 1050 | andi AH, 15 1051 | cp AH, AL 1052 | breq PC+6 1053 | std Y+1, AH 1054 | dec AH 1055 | andi AH, 15 1056 | add YL, AH 1057 | std Y+2, BL 1058 | popw Y 1059 | popw A 1060 | pop BL 1061 | out SREG, AL 1062 | pop AL 1063 | cli 1064 | sbi UCSRB, RXCIE 1065 | reti 1066 | 1067 | 1068 | ;----------------------------------------------------------; 1069 | ; Strings 1070 | 1071 | m_prompt: .db 13,10, "%", 0 1072 | m_error: .db 10, "?", 0 1073 | m_start: .db 13,10, "SMC type 3", 13,10, 0 1074 | 1075 | t_7seg: .db 0x3F,0x06,0x5B,0x4F,0x66,0x6D,0x7D,0x07,0x7F,0x6F 1076 | 1077 | -------------------------------------------------------------------------------- /chan/smc3/SMC3.TXT: -------------------------------------------------------------------------------- 1 | Features of SMC3 2 | 3 | o Using Only a Cheap ATtiny2313 Microcontroller 4 | o Controlled in Serial Command via UART 5 | - Direct Access to Position Command Register 6 | - G0/G1 Motion Generation 7 | o Three Control Modes 8 | - Absolute Positioning Mode 9 | - Constant Speed Mode 10 | - Constant Torque Mode 11 | o Software Decoding of Quadrature Encoder Input 12 | - Count rate over 100,000 counts/sec in quadrature decoding 13 | - No External Component Required 14 | o Two PWM Outputs for H-bridge Driver 15 | o Three Diagnostic Outputs 16 | - Ready, Torque Limit and Servo Error 17 | o On-the-Fly Servo Tuning 18 | 19 | 20 | Changes from SMC to SMC3 21 | 22 | o Removed Pulsed Input 23 | o Added Diagnostic Outputs 24 | - Ready: Servo control is running. 25 | - Torque Limit: Torque limit indicator for mode 2/3. 26 | - Servo Error: Servo error occured and enterd to mode 0. M command or 27 | reset can restart servo. 28 | 29 | -------------------------------------------------------------------------------- /chan/smc3/SMC3A.ASM: -------------------------------------------------------------------------------- 1 | ;----------------------------------------------------------------------------; 2 | ; AVR SERVOMOTOR CONTROLLER R0.3a (C)ChaN, 2005 ; 3 | ;----------------------------------------------------------------------------; 4 | ; SMC3A: Specified for standalone operation for stepper motor replacement 5 | 6 | 7 | .include "tn2313def.inc" 8 | .include "avr.inc" 9 | 10 | .equ INI_MODE = 0 ;Initial Servo Mode (0-3) for no serial control 11 | 12 | .equ SYSCLK = 16000000 13 | .equ BPS = 38400 14 | 15 | 16 | .def _0 = r15 ;Permanent zero register 17 | 18 | .def _PvEnc = r14 ;Previous encoder signal A/B 19 | .def _PvDir = r13 ;Previous direction 20 | .def _CtDiv = r12 ;1/83 divider 21 | 22 | .def _PosH = r11 ;Current position (circular counter) 23 | .def _PosL = r10 ;/ 24 | .def _CmdH = r9 ;Commanded position (circular counter) 25 | .def _CmdL = r8 ;/ 26 | .def _CmdF = r7 ;Gear fraction 27 | 28 | .def _Flags = r25 ; -|Sat.F|Sat.R|-|-|-|Echo 29 | 30 | 31 | ;----------------------------------------------------------; 32 | ; EEPROM Area 33 | 34 | .eseg 35 | ; Memory bank 0 : Yasukawa Electric UGTMEM-A1SA51 36 | .dw 300, 0x0500, 0x0300, 0x00c0, 240, 0x0340, 0x0100, 0 37 | 38 | ; Memory bank 1 : Yasukawa Electric UGTMEM-A1SA51 39 | .dw 300, 0x0500, 0x0300, 0x00c0, 240, 0x0340, 0x0100, 0 40 | 41 | ; Memory bank 2 : Tamagawa Seiki TS1410N1 42 | .dw 300, 0x0800, 0x0300, 0x0060, 180, 0x0550, 0x0100, 0 43 | 44 | ; Memory bank 3 : Matsushita Electric MCN-14EAEC (6V, 40p/r) 45 | .dw 200, 0x0800, 0x0a00, 0x0400, 200, 0x0840, 0x0100, 0 46 | 47 | .equ N_PARM = 8 ; Number of parameter words par bank. 48 | 49 | 50 | 51 | ;----------------------------------------------------------; 52 | ; Data memory area 53 | 54 | .dseg 55 | .org RAMTOP 56 | ; Servo / G command parameters 57 | Parms: 58 | LimSpd: .byte 2 ;P0,Velocity limit Integer 59 | GaSpd: .byte 2 ;P1,Velocity feedback gain 8.8 fixed point 60 | GaTqP: .byte 2 ;P2,Proportional gain 8.8 fixed point 61 | GaTqI: .byte 2 ;P3,Integral gain 8.8 fixed point 62 | LimTrq: .byte 2 ;P4,Torque limit Integer 63 | GaEG: .byte 2 ;P5,EG feedback gain 8.8 fixed point 64 | Gear: .byte 2 ;P6,Gear ratio for pulsed input 8.8 fixed point 65 | .byte 2 ;P7 66 | 67 | ; Command/Servo registers 68 | CtSub: .byte 2 ;Sub command s mode 0/1/2 69 | PvInt: .byte 2 ;Integration register 70 | PvPos: .byte 2 ;Velocity detection register 71 | OvTmr: .byte 2 ;Torque limit timer 72 | 73 | Mode: .byte 1 ;Servo Mode m 74 | 75 | ; Displacements referrd from RAMTOP 76 | .equ iLimSpd = LimSpd-RAMTOP 77 | .equ iGaSpd = GaSpd-RAMTOP 78 | .equ iGaTqP = GaTqP-RAMTOP 79 | .equ iGaTqI = GaTqI-RAMTOP 80 | .equ iLimTrq = LimTrq-RAMTOP 81 | .equ iGaEG = GaEG-RAMTOP 82 | .equ iGear = Gear-RAMTOP 83 | .equ iCtSub = CtSub-RAMTOP 84 | .equ iPvInt = PvInt-RAMTOP 85 | .equ iPvPos = PvPos-RAMTOP 86 | .equ iOvTmr = OvTmr-RAMTOP 87 | .equ iMode = Mode-RAMTOP 88 | 89 | 90 | ; Host command 91 | RxBuf: .byte 2+16 ; Serial receive buffer (Rp, Wp, Buff[16]) 92 | LineBuf:.byte 20 ; Command line input buffer 93 | 94 | 95 | 96 | ;----------------------------------------------------------; 97 | ; Program code 98 | 99 | .cseg 100 | ; Interrupt Vectors (ATtiny2313) 101 | rjmp reset ;Reset 102 | rjmp int0_isr ;INT0 103 | rjmp 0 ;INT1 104 | rjmp 0 ;TC1 CAPT 105 | rjmp 0 ;TC1 COMPA 106 | rjmp 0 ;TC1 overflow 107 | rjmp 0 ;TC0 overflow 108 | rjmp rxint ;USART0 Rx ready 109 | rjmp 0 ;USART0 Tx UDRE 110 | rjmp 0 ;USART0 Tx empty 111 | rjmp 0 ;Analog comparator 112 | rjmp 0 ;PCINT 113 | rjmp 0 ;TC1 COMPB 114 | rjmp background ;TC0 COMPA 115 | ; rjmp 0 ;TC0 COMPB 116 | ; rjmp 0 ;USI START 117 | ; rjmp 0 ;USI OVF 118 | ; rjmp 0 ;EEPROM 119 | ; rjmp 0 ;WDT 120 | 121 | 122 | reset: 123 | outi SPL, low(RAMEND) ;Stack ptr 124 | clr _0 ;Clear RAM 125 | ldiw Y, RAMTOP ; 126 | st Y+, _0 ; 127 | cpi YL, low(RAMTOP+128) ; 128 | brne PC-2 ;/ 129 | 130 | outi PORTD, 0b01111111 ;Initialize PORTD 131 | outi DDRD, 0b00000010 ;/ 132 | 133 | outi PORTB, 0b11100000 ;Initialize PORTB 134 | outi DDRB, 0b00011111 ;/ 135 | 136 | ldiw A, SYSCLK/16/BPS-1 ;UART 137 | outw UBRR, A ; 138 | outi UCSRB, 0b10011000 ;/ 139 | 140 | ldiw A, 128 ;TC1: 8bit PWM mode 141 | outw OCR1A, A ; 142 | outw OCR1B, A ; 143 | outi TCCR1A, 0b10100001 ; 144 | outi TCCR1B, 0b00000001 ;/ 145 | 146 | outi OCR0A, 2 ;TC0: 83kHz interval timer 147 | outi TCCR0A, 0b00000010 ; 148 | outi TCCR0B, 0b00000011 ; 149 | outi TIMSK, 0b00000001 ;/ 150 | 151 | outi MCUCR, 0b00000010 ;INT0: Falling edge 152 | outi GIMSK, 0b01000000 ;/ 153 | 154 | ldi _Flags, 0b00000001 ;Echo ON 155 | 156 | ldi AL, 0 ;Load servo parms form bank 0 157 | rcall load_parms ;/ 158 | 159 | ldi AL, INI_MODE ;Initial servo mode 160 | rcall init_servo ;/ 161 | 162 | ldiw Z, m_start*2 ;Start up message 163 | rcall dp_str ;/ 164 | 165 | 166 | ;----------------------------------------------------------; 167 | ; Command processing loop 168 | 169 | main: 170 | ldiw Z, m_prompt*2 ;Display command prompt 171 | rcall dp_str ;/ 172 | rcall get_line ;Get a command line 173 | ld BH,X+ ;BH = command char 174 | cpi BH,'a' ;CAPS 175 | brcs PC+2 ; 176 | subi BH,0x20 ;/ 177 | cpi BH,' ' ;Null line ? 178 | brlt main ; 179 | cpi BH,'+' ;+Step? 180 | rjeq do_step ; 181 | cpi BH,'-' ;-Step? 182 | rjeq do_step ; 183 | cpi BH, '?' ;Help? 184 | breq do_help ; 185 | cpi BH, 'M' ;Set mode? 186 | breq do_mode ; 187 | cpi BH, 'S' ;Set sub command? 188 | breq do_sub ; 189 | cpi BH, 'P' ;Set parms? 190 | breq do_parm ; 191 | cpi BH, 'E' ;Echo mode? 192 | breq do_echo ; 193 | cpi BH, 'R' ;Read parms? 194 | breq do_reep ; 195 | cpi BH, 'W' ;Write parms? 196 | breq do_weep ; 197 | 198 | cmd_err:ldiw Z,m_error*2 ;Syntax error 199 | rjmp PC+3 200 | do_help:ldiw Z,m_help*2 ;Help 201 | rcall dp_str 202 | rjmp main 203 | 204 | 205 | ;------------------------------------------------; 206 | ; Change parameters, command regs or servo mode. 207 | 208 | do_mode: ; Change servo mode 209 | rcall get_val 210 | breq cmd_err 211 | rcall init_servo 212 | rjmp main 213 | 214 | do_echo: ; Change echo mode 215 | rcall get_val 216 | breq cmd_err 217 | bst AL, 0 218 | bld _Flags,0 219 | rjmp main 220 | 221 | do_sub: ; Set subcommand reg. 222 | ldiw Y, CtSub 223 | rjmp ds_set 224 | 225 | do_parm: ; Set parameters 226 | rcall get_val 227 | breq cmd_err 228 | cpi AL, N_PARM 229 | brcc cmd_err 230 | lsl AL 231 | mov YL, AL 232 | clr YH 233 | subiw Y, -Parms 234 | ds_set: rcall get_val 235 | brcs cmd_err 236 | brne ds_st 237 | ldi AL, 0x0a 238 | rcall xmit 239 | lddw A, Y+0 240 | rcall dp_dec 241 | ldi AL, ':' 242 | rcall xmit 243 | rcall get_line 244 | rcall get_val 245 | brcs cmd_err 246 | breq PC+5 247 | ds_st: cli 248 | stdw Y+0, A 249 | sei 250 | rjmp main 251 | 252 | 253 | 254 | ;------------------------------------------------; 255 | ; Load/Save parameters. 256 | 257 | do_reep: ; Load parameters from EEPROM 258 | rcall get_val 259 | breq cmd_err 260 | cpi AL, (EEPROMEND+1)/N_PARM/2 261 | brcc cmd_err 262 | cli 263 | rcall load_parms 264 | sei 265 | rjmp main 266 | 267 | 268 | do_weep: ; Save parameters into EEPROM 269 | rcall get_val 270 | breq cmd_err 271 | cpi AL, (EEPROMEND+1)/N_PARM/2 272 | brcc cmd_err 273 | rcall get_eeadr 274 | sbic EECR, EEWE 275 | rjmp PC-1 276 | out EEAR, BH 277 | inc BH 278 | ld AL, Y+ 279 | out EEDR, AL 280 | cli 281 | sbi EECR, EEMWE 282 | sbi EECR, EEWE 283 | sei 284 | dec AH 285 | brne PC-11 286 | rjmp main 287 | 288 | 289 | load_parms: 290 | rcall get_eeadr 291 | out EEAR, BH 292 | inc BH 293 | sbi EECR, EERE 294 | in AL, EEDR 295 | st Y+, AL 296 | dec AH 297 | brne PC-6 298 | ret 299 | 300 | 301 | get_eeadr: 302 | ldi AH, N_PARM*2 303 | clr BH 304 | subi AL, 1 305 | brcs PC+3 306 | add BH, AH 307 | rjmp PC-3 308 | ldiw Y, Parms 309 | ret 310 | 311 | 312 | ;------------------------------------------------; 313 | ; Change position command reg. immediataly. 314 | 315 | do_step: 316 | mov DL, BH 317 | rcall get_val 318 | rjeq cmd_err 319 | cli 320 | cpi DL, '-' 321 | breq PC+4 322 | addw _Cmd, A 323 | rjmp PC+3 324 | subw _Cmd, A 325 | sei 326 | rjmp main 327 | 328 | 329 | 330 | ;----------------------------------------------------------; 331 | ; Initialize servo system 332 | ; 333 | ;Call: AL = servo mode 334 | 335 | init_servo: 336 | cli 337 | sts Mode, AL ;Set servo mode 338 | ldiw Y, CtSub ;Clear cmmand regs and servo operators. 339 | st Y+, _0 ; 340 | cpi YL, low(CtSub+8); 341 | brne PC-2 ;/ 342 | clrw _Pos ;Clear position counter 343 | clrw _Cmd ;Clear command counter 344 | clr _CmdF ;/ 345 | sei 346 | cbi PORTB, 0 ;Torque LED on 347 | cbi PORTB, 1 ;Error LED off 348 | sbi PORTB, 2 ;Ready LED on 349 | ret 350 | 351 | 352 | 353 | ;----------------------------------------------------------; 354 | ; Pulse drive interrupt 355 | 356 | int0_isr: 357 | pushw A ;Save flag/reg 358 | in AH, SREG ;/ 359 | 360 | sbis PIND, 6 ;Branch by Dir input 361 | rjmp pi_rev 362 | 363 | pi_fwd: ;Cmd += Gear 364 | lds AL, Gear+0 365 | add _CmdF, AL 366 | lds AL, Gear+1 367 | adc _CmdL, AL 368 | adc _CmdH, _0 369 | rjmp pi_exit 370 | 371 | pi_rev: ;Cmd -= Gear 372 | lds AL, Gear+0 373 | sub _CmdF, AL 374 | lds AL, Gear+1 375 | sbc _CmdL, AL 376 | sbc _CmdH, _0 377 | 378 | pi_exit: 379 | out SREG, AH ;Restore flag/reg 380 | popw A ;/ 381 | reti 382 | 383 | 384 | 385 | ;----------------------------------------------------------; 386 | ; 83kHz Position capture and servo operation interrupt 387 | 388 | background: 389 | push T0L 390 | pushw Z 391 | in T0L, SREG ;Save flags 392 | 393 | mov ZL, _PvEnc ;ZL[1:0] = previous A/B signal 394 | in _PvEnc, PIND ;Sample A/B signal into _PvEnc[1:0] 395 | swap _PvEnc ;/ 396 | ldi ZH, 1 ;Convert it to sequencial number. 397 | sbrc _PvEnc, 1 ; 398 | eor _PvEnc, ZH ;/ 399 | sub ZL, _PvEnc ;Decode motion 400 | andi ZL, 3 ;/ 401 | breq enc_zr ;-> Not moved 402 | cpi ZL, 3 ; 403 | breq enc_rev ;-> -1 count 404 | cpi ZL, 1 ; 405 | breq enc_fwd ;-> +1 count 406 | mov ZL, _PvDir ;-> Missing code recovery: 407 | mov ZH, _PvDir ; double count for previous direction 408 | lsl ZL ; 409 | asr ZH ;/ 410 | rjmp enc_add 411 | enc_rev:ldiw Z, -1 412 | rjmp PC+3 413 | enc_fwd:ldiw Z, 1 414 | mov _PvDir, ZL 415 | enc_add:addw _Pos, Z 416 | enc_zr: 417 | dec _CtDiv ;Decrement 1/83 divider 418 | rjne bgnd_exit ;If not overflow, exit interrupt routine. 419 | 420 | ; End of 83 kHz position captureing process. Follows are 1kHz servo 421 | ; operation. It will be interrupted itself, but will not be re-entered. 422 | 423 | ldi ZL, 83 ;Re-initialize 1/83 divider 424 | mov _CtDiv, ZL ;/ 425 | sei ;Enable interrupts 426 | pushw T0 427 | pushw T2 428 | pushw A 429 | pushw B 430 | pushw C 431 | push DL 432 | pushw Y 433 | ldiw Y, Parms ;Work area base pointer 434 | 435 | lddw T2, Y+iPvPos ;Detect velocity 436 | cli ; 437 | subw T2, _Pos ; 438 | stdw Y+iPvPos, _Pos ; 439 | sei ; 440 | negw T2 ;/ T2 = velocity 441 | 442 | ldd AL, Y+iMode ;Branch by servo mode 443 | cpi AL, 3 ;Mode3? 444 | breq tap_position ;/ 445 | lddw T0, Y+iCtSub ;Get sub command 446 | cpi AL, 2 ;Mode2? 447 | breq tap_velocity ;/ 448 | cpi AL, 1 ;Mode1? 449 | rjeq tap_torque ;/ 450 | rjmp tap_voltage ;Mode0 451 | 452 | 453 | tap_position: 454 | cli ;Get position error (= velocity command) 455 | movw T0L, _CmdL ; 456 | subw T0, _Pos ; 457 | sei ;T0 = position error 458 | 459 | ldiw A, 20000 ;Check if position error is too much 460 | cpw T0, A ; 461 | rjge servo_error ; 462 | ldiw A, -20000 ; 463 | cpw T0, A ; 464 | rjlt servo_error ;/ 465 | 466 | lddw A, Y+iLimSpd ;Velocity limit (P0) 467 | cpw T0, A ; 468 | brge PC+8 ; 469 | negw A ; 470 | cpw T0, A ; 471 | brge PC+2 ; 472 | movw T0L, AL ;T0 = velocity command 473 | 474 | 475 | tap_velocity: 476 | movw AL, T2L ;Velocity loop gain (P1) 477 | lddw B, Y+iGaSpd ; 478 | rcall muls1616 ;B = scaled velocity 479 | subw T0, B ;T0 = velocity error 480 | 481 | movw AL, T0L ;Velocity error P-gain (P2) 482 | lddw B, Y+iGaTqP ; 483 | rcall muls1616 ; 484 | movw ZL, BL ;Z = P term; 485 | 486 | lddw A, Y+iPvInt ;Velocity error I-gain (P3) 487 | lddw B, Y+iGaTqI ; 488 | rcall muls1616 ; 489 | addw Z, B ;Z += I term; 490 | 491 | cbr _Flags, bit6+bit5;Torque limit (P4) 492 | lddw B, Y+iLimTrq ; 493 | cpw Z, B ; 494 | brlt PC+3 ; 495 | movw ZL, BL ; 496 | sbr _Flags, bit6 ; 497 | neg BL ; 498 | com BH ; 499 | cpw Z, B ; 500 | brge PC+3 ; 501 | movw ZL, BL ; 502 | sbr _Flags, bit5 ;/ 503 | 504 | tst T0H ;PvInt += T0, with anti-windup 505 | brmi PC+4 ; 506 | sbrc _Flags, 6 ; 507 | rjmp PC+10 ; 508 | rjmp PC+3 ; 509 | sbrc _Flags, 5 ; 510 | rjmp PC+7 ; 511 | lddw A, Y+iPvInt ; 512 | addw A, T0 ; 513 | stdw Y+iPvInt, A ;/ 514 | 515 | mov AL, _Flags ;Check torque limiter timer 516 | andi AL, bit6+bit5 ; OvTmr is increased by 3 when torque limitter 517 | lddw A, Y+iOvTmr ; works, decreased by 1 when no torque limit. 518 | breq PC+5 ; When the value reaches 3000, servo error 519 | sbi PORTB, 1 ; will beoccured. 520 | addiw A, 3 ; 521 | rjmp PC+5 ; 522 | cbi PORTB, 1 ; 523 | subiw A, 1 ; 524 | brcs PC+8 ; 525 | stdw Y+iOvTmr, A ; 526 | ldiw B, 3000 ; 527 | cpw A, B ; 528 | brcc servo_error ;/ 529 | 530 | movw T0L, ZL ;T0 = torque command 531 | 532 | 533 | tap_torque: 534 | movw AL, T2L ;EG compensation (P5) 535 | lddw B, Y+iGaEG ; 536 | rcall muls1616 ; 537 | addw T0, B ;T0 = voltage command 538 | 539 | 540 | tap_voltage: 541 | ldiw A, 240 ;Clip output voltage between -240 and +240. 542 | cpw T0, A ; Limit minimum duty ratio to 15/16 for bootstrap 543 | brge PC+6 ; type FET driver. 544 | ldiw A, -240 ; 545 | cpw T0, A ; 546 | brge PC+2 ; 547 | movw T0L, AL ;T0 = PWM command 548 | 549 | asrw T0 ;Set PWM register (OCR1A and ~OCR1B) 550 | ldi AL, 120 ; 551 | adc AL, T0L ; 552 | ldi AH, 120 ; 553 | sub AH, T0L ; 554 | out OCR1AL, AL ; 555 | out OCR1BL, AH ;/ 556 | 557 | popw Y 558 | pop DL 559 | popw C 560 | popw B 561 | popw A 562 | popw T2 563 | popw T0 564 | 565 | 566 | bgnd_exit: ;End of encoder capture and servo operation 567 | out SREG, T0L ;Restore flags 568 | popw Z 569 | pop T0L 570 | reti 571 | 572 | 573 | servo_error: 574 | ldi AL, 0 ;Enter mode 0 575 | rcall init_servo ;/ 576 | sbi PORTB, 0 ;Error LED on 577 | clrw T0 ;Output = 0V 578 | rjmp tap_voltage 579 | 580 | 581 | 582 | ;--------------------------------------; 583 | ; 16bit * 16bit signed multiply 584 | ; 585 | ; Multiplyer: A(signed int) 586 | ; Multiplicand: B(unsigned, 8.8 fraction) 587 | ; Result: B(signed int) 588 | ; Clk: 181(max) 589 | 590 | muls1616: 591 | clt 592 | tst AH 593 | brpl PC+6 594 | set 595 | negw A 596 | 597 | subw C, C ; clear high 16bit. 598 | ldi DL, 17 ; DL = loop count 599 | brcc PC+3 ; ---- calcurating loop 600 | addw C, A ; 601 | rorw C ; 602 | rorw B ; 603 | dec DL ; if (--DL > 0) 604 | brne PC-8 ; continue loop; 605 | 606 | mov BL, BH 607 | mov BH, CL 608 | 609 | brtc PC+5 ; Negate the result if multiplyer was negative. 610 | negw B 611 | ret 612 | 613 | 614 | 615 | ;--------------------------------------; 616 | ; Input a command line into LineBuf. 617 | 618 | get_line: 619 | ldiw X,LineBuf 620 | ldi BH,0 621 | rl_lp: rcall receive 622 | breq PC-1 623 | st X,AL 624 | cpi AL,0x0d ; CR 625 | brne PC+4 626 | ldiw X,LineBuf 627 | rjmp echo 628 | cpi AL,0x08 ; BS 629 | brne PC+7 630 | cpi BH,0 631 | breq rl_lp 632 | rcall echo 633 | sbiw XL,1 634 | dec BH 635 | rjmp rl_lp 636 | cpi AL,' ' ; SP 637 | brcs rl_lp 638 | cpi BH,20-1 639 | breq rl_lp 640 | rcall echo 641 | adiw XL,1 642 | inc BH 643 | rjmp rl_lp 644 | 645 | 646 | 647 | ;--------------------------------------; 648 | ; Send ROM string 649 | ; 650 | ; Call: Z = top of the string (ASCIZ) 651 | ; Ret: Z = next string 652 | 653 | dp_str: lpm AL, Z+ 654 | tst AL 655 | brne PC+2 656 | ret 657 | rcall xmit 658 | rjmp dp_str 659 | 660 | 661 | 662 | ;--------------------------------------; 663 | ; Get value of decimal string 664 | ; 665 | ; Call: X -> ASCII string 666 | ; Ret: X = updated 667 | ; if C=1: error 668 | ; elsif Z=1: end of line, value=0 669 | ; else: A = 16bit value 670 | ; 671 | ; Positive: "300" 672 | ; Negative: "-1250" 673 | 674 | get_val: 675 | clt 676 | clrw A 677 | ld BH, X+ 678 | cpi BH, ' ' 679 | brcs gd_n 680 | breq PC-3 681 | cpi BH, '-' 682 | brne PC+3 683 | set 684 | gd_l: ld BH, X+ 685 | cpi BH, ' '+1 686 | brcs gd_e 687 | subi BH, '0' 688 | brcs gd_x 689 | cpi BH, 10 690 | brcc gd_x 691 | ldi CL, 17 692 | clr BL 693 | lsr BL 694 | rorw A 695 | brcc PC+2 696 | addi BL, 10 697 | dec CL 698 | brne PC-6 699 | add AL, BH 700 | adc AH, _0 701 | rjmp gd_l 702 | gd_x: sec 703 | sez 704 | ret 705 | gd_e: sbiw XL, 1 706 | brtc PC+5 707 | negw A 708 | clc 709 | ret 710 | gd_n: sbiw XL,1 711 | clc 712 | sez 713 | ret 714 | 715 | 716 | 717 | ;--------------------------------------; 718 | ; Display a value in decimal string 719 | ; 720 | ; Call: A = 16bit signed value to be displayed 721 | ; Ret: A = broken 722 | 723 | dp_dec: ldi CH,' ' 724 | sbrs AH, 7 725 | rjmp PC+6 726 | negw A 727 | ldi CH,'-' 728 | clr T0L ;digit counter 729 | inc T0L ;---- decimal string generating loop 730 | clr BL ;var1 /= 10; 731 | ldi CL,16 ; 732 | lslw A ; 733 | rol BL ; 734 | cpi BL,10 ; 735 | brcs PC+3 ; 736 | subi BL,10 ; 737 | inc AL ; 738 | dec CL ; 739 | brne PC-8 ;/ 740 | addi BL,'0' ;Push the remander (a decimal digit) 741 | push BL ;/ 742 | cp AL,_0 ;if(var1 =! 0) 743 | cpc AH,_0 ; continue digit loop; 744 | brne PC-16 ;/ 745 | mov AL, CH ;Sign 746 | rcall xmit ;/ 747 | pop AL ;Transmit decimal string 748 | rcall xmit ;<-- Put a char into console 749 | dec T0L ; 750 | brne PC-3 ;/ 751 | ret 752 | 753 | 754 | 755 | ;--------------------------------------; 756 | ; Serial I/O driver 757 | 758 | ; Transmit AL. 759 | echo: sbrs _Flags, 0 760 | ret 761 | xmit: sbis UCSRA, UDRE 762 | rjmp PC-1 763 | out UDR, AL 764 | ret 765 | 766 | receive:; Receive a char into AL. (ZR=no data) 767 | push AH 768 | pushw Y 769 | ldiw Y, RxBuf 770 | cli 771 | ldd AH, Y+0 772 | ldd AL, Y+1 773 | cp AH, AL 774 | breq PC+8 775 | add YL, AH 776 | ldd AL, Y+2 777 | sub YL, AH 778 | inc AH 779 | andi AH, 15 780 | std Y+0, AH 781 | clz 782 | sei 783 | popw Y 784 | pop AH 785 | ret 786 | 787 | rxint: ;USART0 Rx ready 788 | push AL 789 | in AL, SREG 790 | push BL 791 | in BL, UDR 792 | cbi UCSRB, RXCIE 793 | sei 794 | pushw A 795 | pushw Y 796 | ldiw Y, RxBuf 797 | ldd AL, Y+0 798 | ldd AH, Y+1 799 | inc AH 800 | andi AH, 15 801 | cp AH, AL 802 | breq PC+6 803 | std Y+1, AH 804 | dec AH 805 | andi AH, 15 806 | add YL, AH 807 | std Y+2, BL 808 | popw Y 809 | popw A 810 | pop BL 811 | out SREG, AL 812 | pop AL 813 | cli 814 | sbi UCSRB, RXCIE 815 | reti 816 | 817 | 818 | 819 | ;----------------------------------------------------------; 820 | ; Strings 821 | 822 | m_prompt: .db 13,10, "%", 0 823 | m_error: .db 10, "???", 0 824 | m_start: .db 13,10, "SMC3A - Servo Motor Controller (?:help)", 13,10, 0 825 | m_help: .db 13,10, "m - servo mode",13,10, "e - echo mode",13,10, "s - sub cmd",13,10, "{+|-} - step cmd",13,10, "p [] - examine/change parms",13,10, "w - save parms",13,10, "r - load parms", 0 826 | 827 | -------------------------------------------------------------------------------- /chan/smc3/SMC3A.TXT: -------------------------------------------------------------------------------- 1 | Features of SMC3A 2 | 3 | o Using Only a Cheap ATtiny2313 Microcontroller 4 | o Optimized for Replacement of Stepping Motors 5 | - Pulse/Dir Command Input with Pulse Multiplyer 6 | - Pulse rate over 100,000 pulses/sec 7 | - Gear ratio from 1/256 to 255 in 1/256 step 8 | o Three Control Modes 9 | - Relative Positioning Mode 10 | - Constant Speed Mode 11 | - Constant Torque Mode 12 | o Software Decoding of Quadrature Encoder Input 13 | - Count rate over 100,000 counts/sec in quadrature decoding 14 | - No External Component is Required 15 | o Two PWM Outputs for H-bridge Driver 16 | o Three Diagnostic Outputs 17 | - Ready, Torque Limit and Servo Error 18 | o On-the-Fly Servo Tuning 19 | 20 | 21 | Changes from SMC to SMC3A 22 | 23 | o Removed G0/G1 Commands 24 | o Added +/- Commands 25 | - {+|-}: Increase/Decrease command counter. 26 | o Extened Pulsed Input Function 27 | - Pulse Multiplyer: Gear ratio (8.8 fixed-point) can be set with parameter 28 | #6, 256 means 1.0. Parameter #7 has no effect. 29 | o Added Diagnostic Outputs 30 | - Ready: Servo control is running. 31 | - Torque Limit: Torque limit indicator for mode 2/3. 32 | - Servo Error: Servo error occured and enterd to mode 0. M command or 33 | reset can restart servo. 34 | -------------------------------------------------------------------------------- /cport/Makefile: -------------------------------------------------------------------------------- 1 | BOARD_TAG=uno 2 | 3 | CPPFLAGS=-DUSE_TIM2 4 | 5 | include /usr/share/arduino/Arduino.mk 6 | 7 | sim: 8 | simduino build-uno/cport.hex 9 | -------------------------------------------------------------------------------- /cport/cli.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "csmc3.h" 4 | #include "cli.h" 5 | #include "uart.h" 6 | #include "control.h" 7 | #include "motion-planning.h" 8 | #include "eeprom.h" 9 | 10 | 11 | static const char m_prompt[] PROGMEM = "\r\n%"; 12 | static const char m_error[] PROGMEM = "\n?"; 13 | 14 | 15 | static void do_jump(); 16 | //static void do_go(); 17 | static void do_loc(); 18 | static void do_reep(); 19 | static void do_weep(); 20 | static void do_mode(); 21 | static void do_sub(); 22 | static void do_parm(); 23 | 24 | static void ds_set(int16_t *ptr); 25 | 26 | void dp_dec(int16_t v); // print an integer value (should be 24 bit) 27 | uint8_t get_line(void); 28 | 29 | int24_t val; 30 | 31 | uint8_t LineBuf[20]; // Command line input buffer 32 | uint8_t *buf; // read pointer into line buffer 33 | 34 | //;----------------------------------------------------------; 35 | //; Command processing loop 36 | 37 | void task_cli() 38 | { 39 | uint8_t c; 40 | 41 | dp_str(m_prompt); // ;Display command prompt 42 | 43 | get_line(); // ;Get a command line 44 | c = *buf++; 45 | if ( c>='a' ) c -= 0x20;// tolower(c) 46 | 47 | if (c<32) return; // empty line 48 | if (c=='J') {do_jump(); return;} // Jump? 49 | if (c=='G') {do_go(); return;} // Go? 50 | if (c=='L') {do_loc(); return;} // Show posision counter? 51 | if (c=='R') {do_reep(); return;} // Read parms? 52 | if (c=='W') {do_weep(); return;} // Write parms? 53 | if (c=='M') {do_mode(); return;} // Set mode? 54 | if (c=='S') {do_sub(); return;} // Set sub command? 55 | if (c=='P') {do_parm(); return;} // Set parms? 56 | // Syntax error 57 | dp_str(m_error); 58 | } 59 | /* 60 | switch (c) { 61 | case ' ': break; // empty line 62 | case 'J': 63 | do_jump(); 64 | break; 65 | case 'G': 66 | do_go(); 67 | break; 68 | case 'L': // ;Show posision counter? 69 | do_loc(); 70 | break; 71 | case 'R': // ;Read parms? 72 | do_reep(); 73 | break; 74 | case 'W': // ;Write parms? 75 | do_weep(); 76 | break; 77 | case 'M': // ;Set mode? 78 | do_mode(); 79 | break; 80 | case 'S': // ;Set sub command? 81 | do_sub(); 82 | break; 83 | case 'P': // ;Set parms? 84 | do_parm(); 85 | break; 86 | default: // ;Syntax error 87 | dp_str(m_error); 88 | }; 89 | } 90 | */ 91 | 92 | 93 | //void do_go(){Serial_print_s(__func__);} 94 | 95 | 96 | /** 97 | * Load parameters from EEPROM 98 | * 99 | */ 100 | void do_reep() 101 | { 102 | if (get_val() || ((uint24_t) val>= EEBANKS)) { 103 | cmd_err(); 104 | return; 105 | } 106 | load_parms(val); 107 | } 108 | 109 | 110 | /** 111 | * Save parameters into EEPROM 112 | * 113 | */ 114 | void do_weep() 115 | { 116 | if (get_val() || ((uint24_t) val>= EEBANKS)) { 117 | cmd_err(); 118 | return; 119 | } 120 | save_parms(val); 121 | } 122 | 123 | 124 | /** 125 | * print the current location 126 | * 127 | * keep updating the output until a key press is received. 128 | * 129 | * It is sufficient to compare only the lower 8 bits of the position counter. 130 | * To help the compiler avoiding an unneeded cast to int for the comparision 131 | * the two temp variabes a and b are used. The (nonsense) variable assignments 132 | * are cut out by the optimizer and never make it into the output. 133 | */ 134 | static void do_loc() 135 | { 136 | int24_t p; 137 | uint8_t a,b; // temp variables to help the compiler casting the type 138 | 139 | Serial_print_s(__func__); 140 | xmit(10); 141 | do { 142 | xmit(13); 143 | BEGIN_CRITICAL 144 | p = Pos; 145 | END_CRITICAL 146 | dp_dec(p); 147 | xmit(32); 148 | do { 149 | if (Serial_available()) return; 150 | // } while ((uint8_t)p == (uint8_t)Pos); // cast does not work 151 | a = (uint8_t) p; // all this is optimized away 152 | b = (uint8_t) Pos; 153 | } while (a == b); // and this results in a 8 bit compare 154 | } while (1); 155 | } 156 | 157 | 158 | /** 159 | * 160 | * Change position command register immediately. 161 | * 162 | */ 163 | 164 | void do_jump() 165 | { 166 | if (get_val()) { 167 | cmd_err(); 168 | return; 169 | } 170 | CtPos = val; 171 | } 172 | 173 | 174 | 175 | void cmd_err(void) 176 | { 177 | dp_str(m_error); 178 | } 179 | 180 | 181 | 182 | //;------------------------------------------------; 183 | //; Change parameters, command regs or servo mode. 184 | 185 | static void do_mode(void) //: ; Change servo mode 186 | { 187 | if (get_val()) { 188 | cmd_err(); 189 | return; 190 | } 191 | init_servo(val); 192 | } 193 | 194 | 195 | static void do_sub(void) //: ; Set subcommand reg. 196 | { 197 | ds_set(&CtSub); 198 | } 199 | 200 | 201 | static void do_parm(void) // Set parameters 202 | { 203 | if (get_val() || (val >= N_PARM)) { 204 | cmd_err(); 205 | return; 206 | } 207 | 208 | ds_set((int16_t*)&Parms[val]); 209 | } 210 | 211 | static void ds_set(int16_t *ptr) 212 | { 213 | uint8_t c; 214 | 215 | c = get_val(); 216 | if (c == 2) { 217 | cmd_err(); 218 | return; 219 | } 220 | if (c) { // no value given: 221 | // show current value 222 | xmit('\n'); 223 | dp_dec(*ptr); 224 | xmit(':'); 225 | 226 | // allow to enter a new value 227 | get_line(); 228 | c = get_val(); 229 | if (c == 2) { 230 | cmd_err(); 231 | return; 232 | } 233 | if (c) return; // don't change anything for empty input 234 | } 235 | //FIXME: BEGIN_CRITICAL 236 | *ptr = val; 237 | //FIXME: END_CRITICAL 238 | } 239 | 240 | 241 | /* 242 | * read one line from serial 243 | * 244 | * reads until CR (13) is received or the line buffer is full. 245 | * This is similar to Arduino Serial.readBytesUntil(), but without any timeout. 246 | * 247 | * @returns: number of valid bytes in the line buffer 248 | */ 249 | uint8_t get_line(void) 250 | { 251 | uint8_t n; // BH: number of char in buffer 252 | uint8_t c; 253 | uint8_t *ptr; // write pointer in line buffer 254 | 255 | ptr = LineBuf; 256 | n = 0; 257 | do { 258 | // while ((c=receive())<0); // wait for one char 259 | while(Serial_available()<=0); 260 | c = Serial_read(); 261 | *ptr = c; 262 | if (c==13) break; 263 | if (c==8) { // BS 264 | if (n) { 265 | echo(c); 266 | ptr--; 267 | n--; 268 | } 269 | continue; 270 | } 271 | if (c<32) continue; 272 | if (n==19) continue; 273 | echo(c); 274 | ptr++; 275 | n++; 276 | } while(1); 277 | echo(c); 278 | buf = LineBuf; 279 | return n; 280 | } 281 | 282 | 283 | /** 284 | * print a zero-terminated string in flash over uart 285 | * 286 | * 287 | * This could be _much_ more efficient if the parameter would be in Z already: 288 | 289 | b82: rcall xmit 290 | dp_str: lpm AL, Z+ 291 | tst AL 292 | brne b82 293 | ret 294 | 295 | */ 296 | void dp_str(const char *pstr) // ;Display string 297 | { 298 | uint8_t c; 299 | 300 | while ((c=pgm_read_byte(pstr++))) { // intentional assign 301 | xmit(c); 302 | } 303 | } 304 | 305 | /* 306 | ;--------------------------------------; 307 | ; Get value of decimal string 308 | ; 309 | ; Call: X -> ASCII string 310 | ; Ret: X = updated 311 | ; if C=1: error 312 | ; elsif Z=1: end of line, value=0 313 | ; else: BL:AL = 24bit value 314 | ; 315 | ; Positive: "300" 316 | ; Negative: "-125000" 317 | * 318 | * The meaning of the return value is different from the assembler version. 319 | * In C zero is ok. 320 | * 321 | * used/effected global variables: 322 | * buf: in: the first byte to be read 323 | * out: the first unprocessed byte 324 | * 325 | * @returns: 326 | * 0: ok, valid value in global variable val 327 | * 1: empty line, val=0 328 | * 2: syntax error, val invalid 329 | */ 330 | uint8_t get_val(void) 331 | { 332 | uint8_t neg=0; 333 | uint8_t c; // unsigned is essential, see below 334 | 335 | val = 0; 336 | 337 | // ignore leading space 338 | do { 339 | c = (uint8_t) *buf++; 340 | if (c < ' ') return (1); // found empty line 341 | } while (c == ' '); // read until non-space character 342 | 343 | // handle minus sign 344 | if (c == '-') { 345 | neg = 1; 346 | c = (uint8_t) *buf++; 347 | } 348 | 349 | // read all characters up to CR or space 350 | while (c > ' ') { 351 | c -= '0'; // c is unsigned, so values below 0x30 wrap over 352 | if (c>9) { 353 | buf--; 354 | return (2); // syntax error 355 | } 356 | val = val*10 + c; 357 | c = (uint8_t) *buf++; 358 | } 359 | 360 | // success 361 | buf--; 362 | if (neg) val = -val; 363 | return (0); // ok 364 | } 365 | 366 | 367 | #ifdef USE_ARDUINO 368 | // #define dp_dec Serial_print_i 369 | inline void dp_dec(int16_t v) 370 | { 371 | Serial_print_i(v); 372 | } 373 | #else 374 | void dp_dec(int16_t v) // print an integer value (should be 24 bit) 375 | { 376 | char sign, c; 377 | uint8_t digits; 378 | 379 | sign = ' '; 380 | if (v<0) { 381 | sign = '-'; 382 | v = -v; 383 | } 384 | 385 | // FIXME: This implementation is inefficient as it does the 386 | // division two times instead of saving the quotient the first time. 387 | digits = 0; 388 | do { 389 | digits++; 390 | push((v%10)+'0'); 391 | } while (v /= 10); 392 | 393 | // print the saved string from the stack 394 | xmit(sign); 395 | while (digits--) { 396 | xmit(pop()); 397 | } 398 | } 399 | #endif 400 | 401 | /* 402 | unsigned division Q,R = N/D 403 | 404 | Q := 0 -- Initialize quotient and remainder to zero 405 | R := 0 406 | for i := n − 1 .. 0 do -- Where n is number of bits in N 407 | R := R << 1 -- Left-shift R by 1 bit 408 | R(0) := N(i) -- Set the least-significant bit of R equal to bit i of the numerator 409 | if R ≥ D then 410 | R := R − D 411 | Q(i) := 1 412 | end 413 | end 414 | 415 | BH R 416 | AL Q 417 | */ 418 | -------------------------------------------------------------------------------- /cport/cli.h: -------------------------------------------------------------------------------- 1 | /* gebrauchte Funktionen: 2 | */ 3 | 4 | #ifndef _CLI_H_ 5 | #define _CLI_H_ 6 | 7 | #ifdef __cplusplus 8 | extern "C" { 9 | #endif 10 | 11 | //extern const char m_prompt[] PROGMEM; 12 | //extern const char m_error[] PROGMEM; 13 | //extern const char m_start[] PROGMEM; 14 | 15 | // getval(): 0=kein Wert, 1=Wert ok. Wert ist in val 16 | uint8_t get_val(); 17 | extern int24_t val; 18 | 19 | void task_cli(void); 20 | 21 | void cmd_err(void); 22 | void dp_str(const char *pstr); // ;Display string in flash 23 | 24 | #ifdef __cplusplus 25 | } 26 | #endif 27 | 28 | #endif 29 | -------------------------------------------------------------------------------- /cport/control.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "csmc3.h" 4 | #include "control.h" 5 | #include "mul.h" 6 | 7 | 8 | static int16_t T2; // current velocity 9 | 10 | 11 | /** 12 | * Initialize servo system 13 | * 14 | * Zero out all status variables and turn off all LEDs. 15 | */ 16 | void init_servo(uint8_t mode) 17 | { 18 | BEGIN_CRITICAL 19 | Mode = mode; 20 | // bzero(CtPos, &Mode-&CtPos); // more efficient, but a little crude 21 | CtPos = 22 | CtSub = 23 | PvInt = 24 | PvPos = 25 | OvTmr = 0; 26 | 27 | Pos = 0; 28 | END_CRITICAL 29 | 30 | led_off(LED_ERROR); 31 | led_off(LED_TORQUE); 32 | led_off(LED_READY); 33 | } 34 | 35 | 36 | /** 37 | * postion control loop (mode 3) 38 | * 39 | * FIXME: compiles very inefficient 40 | */ 41 | static inline int16_t tap_position() 42 | { 43 | int24_t T0; // position error 44 | int24_t max; // temp. variabe to ensure proper signiness 45 | 46 | BEGIN_CRITICAL 47 | T0 = CtPos - Pos; 48 | END_CRITICAL 49 | 50 | max = (int24_t) LimSpd; 51 | // clamp position error to LimSpd (P0) 52 | if (T0 >= max) T0 = max; 53 | else if (T0 <= -max) T0 = -max; 54 | 55 | return (int16_t) T0; 56 | } 57 | 58 | 59 | /** 60 | * velocity control loop (mode 2) 61 | * 62 | * flag 5: lower torque limit active 63 | * flag 6: lower torque limit active 64 | * FIXME: combine these two flags into one. That simplyfies everything. 65 | * 66 | * @parms: 67 | * T0: desired velocity value. This value is expected to be already clamped 68 | * to +/-LimSpd 69 | * 70 | * used global variables, side effects: 71 | * GaSpd (r), GaTqP (r), PvInt (rw), GaTqI (r), LimTrq (r), T2/speed (r) 72 | * 73 | * @returns: torque value 74 | */ 75 | static inline int16_t tap_velocity(int16_t T0) 76 | { 77 | int16_t Z; 78 | 79 | T0 -= muls1616(T2, GaSpd); // error = desired_value - speed*P1 80 | Z = muls1616(T0,GaTqP) 81 | + muls1616(PvInt,GaTqI); // Z = Pval*error + Ival*Isum 82 | 83 | // torque limit (P4) 84 | clear_flag(5); 85 | clear_flag(6); 86 | if (Z >= (int16_t) LimTrq) { // force a signed compare 87 | Z = LimTrq; 88 | set_flag(6); 89 | } 90 | if (Z <= (int16_t) -LimTrq) { // force a signed compare 91 | Z = -LimTrq; 92 | set_flag(5); 93 | } 94 | 95 | // calculate integral part, with anti-windup 96 | if ( ((T0<0)&&!flag(5)) || ((T0>=0)&&!flag(6)) ) { 97 | PvInt += T0; 98 | } 99 | 100 | if (flag(5) || flag(6)) { 101 | led_on(LED_TORQUE); 102 | OvTmr += 3; 103 | } else { 104 | led_off(LED_TORQUE); 105 | if (OvTmr == 0) return (Z); 106 | OvTmr--; 107 | } 108 | if (OvTmr >= TL_TIME) { 109 | // trigger a servo error. Go back into save mode 0 (and make 110 | // servo_operation() skip tap_torque) 111 | init_servo(0); // Enter mode 0 112 | led_on(LED_ERROR); 113 | return (0); 114 | } 115 | 116 | return (Z); 117 | } 118 | 119 | 120 | static inline int16_t tap_torque(int16_t T0) 121 | { 122 | return (T0 + muls1616(T2,GaEG)); // T0 + speed*P5 123 | } 124 | 125 | 126 | /** 127 | * output a PWM value (mode 0) 128 | * 129 | * Clip output voltage between -240 and +240. Limit minimum duty ratio to 130 | * 15/16 for bootstrap type FET driver. 131 | * 132 | * NOTE: This function compiles quite inefficient for AVR. The direct 133 | * translation of the original assembler code (v1) even blocks the CPU for 134 | * negative values of T0 (it works with simavr, though) 135 | * - v1: 64 bytes and blocking the CPU (too slow to finish in one interrupt?) 136 | * - v2: 38 bytes, but doesn't work for T0>255 or T0<-255 137 | * - v3: 46 bytes, works. 138 | * 139 | * Consider using the original assembler function instead. (42 bytes) 140 | */ 141 | static inline void tap_voltage(int16_t T0) 142 | { // v3: 0x1e-0x4c = 46 bytes 143 | int8_t v; 144 | 145 | // Clip output voltage between -240 and +240. 146 | if (T0 >= 240) { 147 | v = 120; 148 | } else if (T0 < -240) { 149 | v = -120; 150 | } else { 151 | v = T0/2; 152 | } 153 | 154 | OCR1AL = v + 120; 155 | OCR1BL = 120 - v; 156 | } 157 | /* 158 | { // v2: 0x1e-0x44 = 38 bytes. wrong output for abs(T0)>255 (clipping too early) 159 | int8_t v; 160 | 161 | v = T0/2; 162 | 163 | // Clip output voltage between -240 and +240. 164 | if (v >= 120) { 165 | v=120; 166 | } else if (v < -120) { 167 | v=-120; 168 | } 169 | 170 | OCR1AL = v + 120; 171 | OCR1BL = 120 - v; 172 | } 173 | */ 174 | /* // v1: 0x1e-0x5e = 64 bytes, blocks the UART for negative values. (why?) 175 | { 176 | // Clip output voltage between -240 and +240. 177 | if (T0 >= 240) { 178 | T0=240; 179 | } else if (T0 < -240) { 180 | T0=-240; 181 | } 182 | 183 | OCR1AL = (T0/2) + 120; 184 | OCR1BL = 120 - (T0/2); 185 | } 186 | */ 187 | /* 42 bytes: 188 | tap_voltage: 189 | ldiw A, 240 ;Clip output voltage between -240 and +240. 190 | cpw T0, A ; Limit minimum duty ratio to 15/16 for bootstrap 191 | brge b30 ; type FET driver. 192 | ldiw A, -240 ; 193 | cpw T0, A ; 194 | brge b31 ; 195 | b30: movw T0L, AL ;T0 = PWM command 196 | 197 | b31: asrw T0 ;Set PWM register (OCR1A and ~OCR1B) 198 | ldi AL, 120 ; 199 | adc AL, T0L ; 200 | ldi AH, 120 ; 201 | sub AH, T0L ; 202 | .if OCR1AL < 0x40 203 | out OCR1AL, AL ; 204 | out OCR1BL, AH ;/ 205 | .else 206 | sts OCR1AL, AL ; 207 | sts OCR1BL, AH ;/ 208 | .endif 209 | */ 210 | 211 | void servo_operation() 212 | { 213 | int16_t T0; // input value to the module 214 | 215 | set_flag(7); // 1kHz interrupt flag 216 | BEGIN_CRITICAL 217 | T2 = Pos - PvPos; // calculate current velocity 218 | PvPos = Pos; // update previous position 219 | END_CRITICAL 220 | 221 | T0 = CtSub; // only needed for mode 0,1,2 222 | /* 223 | switch (Mode) { 224 | case 3: T0 = tap_position(); 225 | // intentional fall-through 226 | case 2: T0 = tap_velocity(T0); 227 | // intentional fall-through 228 | case 1: // tap_velocity might have triggered a servo error 229 | // and reset the system into mode 0. 230 | if (Mode) { 231 | T0 = tap_torque(T0); 232 | } 233 | // intentional fall-through 234 | default: tap_voltage(T0); 235 | } 236 | */ 237 | if (Mode>=3) T0 = tap_position(); 238 | if (Mode>=2) T0 = tap_velocity(T0); 239 | if (Mode>=1) T0 = tap_torque(T0); 240 | tap_voltage(T0); 241 | } 242 | -------------------------------------------------------------------------------- /cport/control.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _CONTROL_H_ 3 | #define _CONTROL_H_ 4 | 5 | #ifdef __cplusplus 6 | extern "C" { 7 | #endif 8 | 9 | 10 | void init_servo(uint8_t mode); 11 | void servo_operation(); 12 | 13 | #ifdef __cplusplus 14 | } 15 | #endif 16 | 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /cport/csmc3.h: -------------------------------------------------------------------------------- 1 | /** 2 | * SMC4 servo motor controller v4 3 | * 4 | */ 5 | 6 | #ifndef _CSCM3_H_ 7 | #define _CSCM3_H_ 8 | 9 | #include 10 | #include 11 | 12 | #ifdef __cplusplus 13 | extern "C" { 14 | #endif 15 | 16 | #define uint24_t __uint24 // requires gcc v4.7 17 | #define int24_t __int24 18 | 19 | #define BEGIN_CRITICAL asm("cli"); 20 | #define END_CRITICAL asm("sei"); 21 | 22 | 23 | /* --- System configuration ----------------------------------------------- */ 24 | 25 | #define SYSCLK 16000000 // System clock 26 | #define BPS 38400 // UART bps 27 | #define TL_TIME 1500 // Error timer (tError(ms)=TL_TIME/3) 28 | 29 | 30 | 31 | /* --- LED pin mapping ---------------------------------------------------- */ 32 | 33 | #if defined(__AVR_ATtiny2313__) || defined(__AVR_ATtiny2313A__) 34 | #define LED_ERROR PORTB,0 35 | #define LED_TORQUE PORTB,1 36 | #define LED_READY PORTB,2 37 | 38 | #elif defined(__AVR_ATmega328__) || defined(__AVR_ATmega328P__) 39 | #define LED_ERROR PORTB,0 40 | #define LED_TORQUE PORTC,3 41 | #define LED_READY PORTC,2 42 | 43 | #else 44 | #error "no LED pin definition found for this device" 45 | #endif 46 | 47 | //#define led_on(PORT,BIT) PORT|=(1< 5 | #include 6 | #include "csmc3.h" 7 | 8 | #define EEBANKS ((E2END+1)/sizeof(struct control_s)) 9 | 10 | extern struct control_s parameter_bank[EEBANKS] EEMEM; 11 | 12 | void load_parms(uint8_t idx); 13 | void save_parms(uint8_t idx); 14 | 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /cport/encoder.c: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include "csmc3.h" 5 | #include "encoder.h" 6 | // -------- 7 | 8 | // avr-gcc is smart enough to optimize this into a single swap command 9 | static unsigned char nibbleSwap(unsigned char a) 10 | { 11 | return (a<<4) | (a>>4); 12 | } 13 | 14 | 15 | void position_capture() 16 | { 17 | static uint8_t PvEnc; // Previous encoder signal A/B 18 | static uint8_t PvDir; // Previous direction 19 | uint8_t ZL; 20 | 21 | ZL = PvEnc; 22 | PvEnc = nibbleSwap(PIND); 23 | if (PvEnc & 2) PvEnc ^= 1; // Convert it to sequencial number. 24 | ZL = (ZL - PvEnc) & 3; // Decode motion 25 | 26 | if (ZL) { 27 | if (ZL == 3) { 28 | // enc_rev 29 | PvDir = -1; 30 | Pos--; 31 | } else if (ZL == 1) { 32 | // enc_fwd 33 | PvDir = 1; 34 | Pos++; 35 | } else { 36 | // missing code recovery 37 | Pos += 2*PvDir; 38 | } 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /cport/encoder.h: -------------------------------------------------------------------------------- 1 | #ifndef _ENCODER_H_ 2 | #define _ENCODER_H_ 3 | 4 | 5 | #ifdef __cplusplus 6 | extern "C" { 7 | #endif 8 | 9 | void position_capture(); 10 | 11 | #ifdef __cplusplus 12 | } 13 | #endif 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /cport/motion-planning.c: -------------------------------------------------------------------------------- 1 | 2 | #include "csmc3.h" 3 | #include "cli.h" 4 | #include "motion-planning.h" 5 | #include "uart.h" 6 | 7 | 8 | /* --- private variables --------------------------------------------------- */ 9 | 10 | //static s24_8 cur_pos; // next planned position, u24.8 11 | //static s16_8 velocity; // currently planned velocity (mostly unsigned) 12 | //static bool direction; // 0 = positive, 1 = negative 13 | static int32_t cur_pos; // next planned position, u24.8 14 | static int24_t velocity; // currently planned velocity (mostly unsigned) 15 | static uint8_t direction; // 0 = positive, 1 = negative 16 | 17 | static int24_t start_pos; // start position of the move 18 | static int24_t target_pos; // target position of the move 19 | 20 | 21 | /* --- private functions --------------------------------------------------- */ 22 | 23 | static void dg_0(); 24 | static void dg_1(); 25 | static uint8_t dg_add(); 26 | 27 | 28 | /* --- implementation of public functions ---------------------------------- */ 29 | 30 | /** 31 | ;------------------------------------------------; 32 | ; Go at trapezoidal/rectangular velocity profile. 33 | ; 34 | ; local variables: (24 bit, LSB first in list) 35 | ; T0H:T2 current position (kind of loop variable) (s24.8) 36 | ; T4:T6L start position (s24) 37 | ; A:BL target position (s24) 38 | ; BH:C velocity (u16.8) 39 | ; D:EL next important point on ramp (midpoint, start of end ramp) 40 | ; Tflag direction of movement (b1) 41 | 42 | ; T4:T6L = CtPos; // s24 43 | ; cur_pos T0:T2 = CtPos << 8; // s32 (wirklich?) 44 | ; cur_pos T0H:T2 = CtPos; // s24 (wirklich?) 45 | ; if (get_val()) 46 | ; { 47 | ; if (val==0) goto dg_0(); 48 | ; if (val==1) goto dg_1(); 49 | ; } 50 | ; goto cmd_err; 51 | */ 52 | void do_go() 53 | { 54 | start_pos = CtPos; 55 | cur_pos = start_pos << 8; // cur_pos is s24.8 56 | 57 | if (get_val()==0) { 58 | if (val==0) { 59 | dg_0(); 60 | return; 61 | } else if (val==1) { 62 | dg_1(); 63 | return; 64 | } 65 | } 66 | cmd_err(); 67 | } 68 | 69 | 70 | /* --- implementation of private functions --------------------------------- */ 71 | 72 | 73 | /** 74 | * Rectangular velocity profile - G1 command 75 | * 76 | */ 77 | //void dg_1(uint24_t target, int24_t speed) 78 | static void dg_1() 79 | { 80 | uint8_t c; 81 | 82 | if (get_val()) {cmd_err(); return;} 83 | target_pos = val; 84 | 85 | if (get_val()) {cmd_err(); return;} 86 | velocity = val; // this is u16.8 87 | 88 | direction = (target_pos < start_pos); 89 | 90 | while ((c=dg_add())==0); // loop until target pos is reached 91 | 92 | if (c==1) { 93 | // if not interrupted by the user, go for the target position 94 | CtPos = target_pos; 95 | } 96 | } 97 | 98 | 99 | 100 | /** 101 | * Trapezoidal velocity profile - G0 command 102 | * 103 | */ 104 | static void dg_0() 105 | { 106 | uint8_t c; 107 | uint8_t keep_waiting; 108 | int24_t next_mark; 109 | 110 | if (get_val()) {cmd_err(); return;} 111 | target_pos = val; 112 | 113 | direction = (target_pos < start_pos); 114 | 115 | // calculate the midpoint of the movement 116 | next_mark = (target_pos - start_pos)/2 + start_pos; 117 | 118 | // ---Up ramp loop 119 | velocity = 0; 120 | do { 121 | velocity += MvAcc; 122 | c = dg_add(); // do the next step 123 | if (c==1) goto dg_end; // end position reached 124 | if (c==2) return; // user interruption 125 | 126 | // check if we already passed the midpoint. 127 | // if yes, go straight for the down ramp. 128 | if (direction==0) { 129 | if (cur_pos>>8 >= next_mark) goto dg_de; 130 | } else { 131 | if (cur_pos>>8 <= next_mark) goto dg_de; 132 | } 133 | } while (velocity < MvSpd); // FIXME: velocity might exceed MvSpd 134 | 135 | // the speed reached P6 now. Before we keep goint at this speed 136 | // calculate the down ramp point, assuming that the down ramp will 137 | // be the same width than the the up ramp: 138 | // up ramp width was (cur_pos-start_pos) 139 | // down ramp beginns at target - up_ramp_width = target - (cur-start) 140 | next_mark = start_pos - (cur_pos>>8) + target_pos; 141 | 142 | // ---Constant velocity loop 143 | for(;;) { 144 | if (dg_add()==2) return; //check for user interruption 145 | 146 | // check if it is time for the down ramp: 147 | if (direction==0) { 148 | if (cur_pos>>8 >= next_mark) break; 149 | } else { 150 | if (cur_pos>>8 <= next_mark) break; 151 | } 152 | } 153 | 154 | // ---Down ramp loop 155 | dg_de: 156 | // if (dg_add()==2) return; //check for user interruption 157 | // the following decrement is the reason why velocity is only 158 | // "mostly unsigned". We definitly need a signed comparision here. 159 | // while (( (s16_8)velocity-=MvAcc) >= 0) { 160 | // if (dg_add()==0) break; 161 | // } 162 | 163 | c = dg_add(); 164 | while ((velocity >= MvAcc) && (c==0)) { 165 | velocity -= MvAcc; 166 | c = dg_add(); 167 | } 168 | if (c==2) return; // user interruption 169 | 170 | // End of action 171 | dg_end: 172 | // uint8_t keep_waiting; 173 | 174 | BEGIN_CRITICAL; 175 | CtPos = target_pos; 176 | 177 | do { 178 | // access to Pos needs to be protected from IRQs, that's why 179 | // it can't be done inside the while statement. 180 | BEGIN_CRITICAL 181 | keep_waiting = (Pos!=target_pos); 182 | END_CRITICAL 183 | } while (keep_waiting && !Serial_available()); // allow to end the loop by keypress 184 | 185 | /* 186 | //FIXME: the comparison needs to be protected from IRQs, but 187 | // that is difficult to express in C. Would be better to 188 | // define an inline function with some asm code. 189 | BEGIN_CRITICAL 190 | if (Pos!=target_pos) { 191 | END_CRITICAL 192 | break; 193 | } 194 | END_CRITICAL 195 | } while (!Serial.available()); // allow to end the loop by keypress 196 | */ 197 | } 198 | 199 | 200 | 201 | /** 202 | ; 203 | ; do one 1ms step with the current velocity 204 | ; 205 | ; wait for 1kHz time interval without any torque flags set. 206 | ; advance cur_pos by (velocity) 207 | ; if that does not overrun the target position, advance CtPos by (velocity) 208 | ; 209 | * The return value differs from the assembler version: 210 | * @returns: 211 | * 0: all ok, target not reached yet (Assember: S=1, N^V=1) 212 | * 1: overrun target position (Assembler: N^V=0) 213 | * 2: user interruption, ESC-key hit (Assembler: Jump back to main) 214 | 215 | //assembler: * @returns: TRUE=ok, FALSE=overrun the target position 216 | */ 217 | static uint8_t dg_add() 218 | { 219 | do { 220 | // Wait for next 1kHz time interval 221 | clear_flag(7); 222 | do { 223 | if (Serial_available()) { 224 | if (Serial_read()==27) { 225 | return 2; 226 | } 227 | } 228 | } while (flag(7)==0); // wait for 1kHz time interval 229 | // } while (flag(6)||flag(5)); // wait if torque limit did occur. 230 | } while (Flags & 0b0110000); // keep waiting if torque limit did occur 231 | 232 | // Increase commanded point by current velocity 233 | if (direction == 0) { 234 | // move in positive direction 235 | cur_pos += velocity; 236 | if (cur_pos >> 8 >= target_pos) return 1; 237 | } else { 238 | // move in negative direction 239 | cur_pos -= velocity; 240 | if (cur_pos >> 8 <= target_pos) return 1; 241 | } 242 | 243 | BEGIN_CRITICAL 244 | CtPos = cur_pos >> 8; 245 | END_CRITICAL; 246 | 247 | return 0; // target no reached yet 248 | } 249 | -------------------------------------------------------------------------------- /cport/motion-planning.h: -------------------------------------------------------------------------------- 1 | #ifndef _MOTION_PLANNING_H_ 2 | #define _MOTION_PLANNING_H_ 3 | 4 | void do_go(); 5 | 6 | //void dg_0(uint24_t target); 7 | //void dg_1(uint24_t target, int24_t speed); 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /cport/mul.c: -------------------------------------------------------------------------------- 1 | #include "mul.h" 2 | 3 | /* 4 | ;--------------------------------------; 5 | ; 16bit * 16bit signed multiply 6 | ; 7 | ; Multiplier: A(signed int) 8 | ; Multiplicand: B(unsigned, 8.8 fraction) 9 | ; Result: A(signed int) 10 | ; Clk: 181(max) 11 | */ 12 | int16_t muls1616(int16_t i16, uint16_t u8_8) 13 | { 14 | uint8_t DL; 15 | int16_t C; // temp var 16 | 17 | asm volatile( 18 | " clt" "\n\t" 19 | " tst %B[A]" "\n\t" 20 | " brpl 1f" "\n\t" 21 | " set" "\n\t" 22 | " com %B[A]" "\n\t" 23 | " neg %A[A]" "\n\t" 24 | " brne 1f" "\n\t" 25 | " inc %B[A]" "\n\t" 26 | "\n\t" 27 | "1: clr %A[C] ; clear high 16bit." "\n\t" 28 | " clr %B[C]" "\n\t" 29 | " ldi %[DL], 17; DL = loop count" "\n\t" 30 | "2: brcc 3f ; ---- calculating loop" "\n\t" 31 | " add %A[C], %A[A];" "\n\t" 32 | " adc %B[C], %B[A];" "\n\t" 33 | "3: ror %B[C] ;" "\n\t" 34 | " ror %A[C] ;" "\n\t" 35 | " ror %B[B] ;" "\n\t" 36 | " ror %A[B] ;" "\n\t" 37 | " dec %[DL] ; if (--DL > 0)" "\n\t" 38 | " brne 2b ; continue loop;" "\n\t" 39 | 40 | " mov %A[A], %B[B]" "\n\t" 41 | " mov %B[A], %A[C]" "\n\t" 42 | 43 | " brtc 4f ; Negate the result if multiplier was negative." "\n\t" 44 | " com %B[A]" "\n\t" 45 | " neg %A[A]" "\n\t" 46 | " brne 4f" "\n\t" 47 | " inc %B[A]" "\n\t" 48 | "4:" "\n\t" 49 | "" 50 | : [A] "+r" (i16), [C] "=&r" (C), [DL] "=&d" (DL), [B] "+r" (u8_8) // output 51 | : // input 52 | : // clobber 53 | ); 54 | 55 | return i16; // return value is in the same register as the first arg 56 | } 57 | -------------------------------------------------------------------------------- /cport/mul.h: -------------------------------------------------------------------------------- 1 | #ifndef _MUL_H_ 2 | #define _MUL_H_ 3 | 4 | #include 5 | 6 | int16_t muls1616(int16_t i16, uint16_t u8_8); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /cport/not_ready/display.c: -------------------------------------------------------------------------------- 1 | #include "display.h" 2 | 3 | //; Display buffer 4 | uint8_t Disp[1+8]; // Display buffer, index 5 | -------------------------------------------------------------------------------- /cport/not_ready/display.h: -------------------------------------------------------------------------------- 1 | #ifndef _DISPLAY_H_ 2 | #define _DISPLAY_H_ 3 | 4 | #include 5 | 6 | //; Display buffer 7 | extern uint8_t Disp[1+8]; // Display buffer, index 8 | 9 | #endif 10 | -------------------------------------------------------------------------------- /cport/uart.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * wrapper layer to access the Arduino Serial class from C 3 | * 4 | * This is a replacement for the actual UART implementation. 5 | * 6 | */ 7 | 8 | #include "Arduino.h" 9 | #include "HardwareSerial.h" 10 | 11 | #include "uart.h" 12 | 13 | 14 | 15 | 16 | 17 | #ifdef USE_ARDUINO 18 | 19 | #ifdef __cplusplus 20 | extern "C" { 21 | #endif 22 | 23 | uint8_t Serial_available(){return Serial.available();} 24 | uint8_t Serial_read(){return Serial.read();} 25 | void Serial_write(uint8_t c){Serial.print(c);} 26 | void Serial_print_c(char c){Serial.print(c);} 27 | void Serial_print_i(long v){Serial.print(v);} 28 | void Serial_print_s(const char *s){Serial.print(s);} 29 | 30 | 31 | // #define xmit Serial_write_c 32 | void xmit(char c) 33 | { 34 | Serial_print_c(c); 35 | } 36 | 37 | #ifdef __cplusplus 38 | } 39 | #endif 40 | 41 | #else 42 | #error "no implementation for xmit()" 43 | #endif 44 | -------------------------------------------------------------------------------- /cport/uart.h: -------------------------------------------------------------------------------- 1 | #ifndef _UART_H_ 2 | #define _UART_H_ 3 | 4 | #ifdef __cplusplus 5 | extern "C" { 6 | #endif 7 | 8 | 9 | #define USE_ARDUINO 10 | 11 | #ifdef USE_ARDUINO 12 | uint8_t Serial_available(); 13 | uint8_t Serial_read(); 14 | void Serial_write(uint8_t); 15 | void Serial_print_c(char); 16 | void Serial_print_i(long); 17 | void Serial_print_s(const char *); 18 | #endif 19 | 20 | 21 | 22 | //; Host command 23 | #define echo xmit 24 | void xmit(char); // transmit one byte 25 | 26 | #ifdef __cplusplus 27 | } 28 | #endif 29 | 30 | #endif 31 | --------------------------------------------------------------------------------