Xan's Phoenix code in C running on Bap28

[kurte]

Thanks Alan,

Just got back from having some non-robotics fun.

The difficulty with cleaning up this code is It may end up smaller but harder to read. For example if I take the TimerZ1 handler that handles the 4 timer counter match interrupts. It currently looks like:

[code]extern “C” void TIMERZ1() attribute ((interrupt_handler));
extern “C” void TIMERZ1()
{
DSERVOTOGGLE(40); // BUGBUG:: For debug servo stuff.

if (TZ1.TSR.BIT.IMFA && TZ1.TIER.BIT.IMIEA) {
	DSERVOTOGGLE(41);		// BUGBUG:: For debug servo stuff.

	TZ1.GRA = 0xffff;			// should trigger and do its processing before overflow happens.
	TZ1.TIORA.BIT.IOA = 0x001;	// Will set to zero when timer happens
	TZ1.TIER.BIT.IMIEA = 0;		// don't need interrupt here
	TZ1.TSR.BIT.IMFA = 0;
}	
if (TZ1.TSR.BIT.IMFB && TZ1.TIER.BIT.IMIEB) {
	DSERVOTOGGLE(41);		// BUGBUG:: For debug servo stuff.
	TZ1.GRB = 0xffff;			// should trigger and do its processing before overflow happens.
	TZ1.TIORA.BIT.IOB = 0x001;	// Will set to zero when timer happens
	TZ1.TIER.BIT.IMIEB = 0;		// don't need interrupt here
	TZ1.TSR.BIT.IMFB = 0;
}	

… similar code repeated for IMIEC and IMIED… except also TIORA goes to TIORC (4 bits each…)
[/code]
Disregard the DSERVOTOGGLE. When compiled for debug it toggles the IO line when compiled for non-debug it does nothing…
I could probably write this something like this:

[code]extern “C” void TIMERZ1() attribute ((interrupt_handler));
extern “C” void TIMERZ1()
{
DSERVOTOGGLE(40); // BUGBUG:: For debug servo stuff.
int i;
for (i=0; i < 4; i++) {
if ((TZ1.TSR.BYTE & (1<<i)) && (TZ1.TIER.BYTE & (1<<i))) {
(((u16)&(TZ1.GRA))+i) = 0xffff; // How to treat GRA-GRD like an array

		// How to handle TIORA/TIORC???  
		if (i < 2)
			TZ1.TIORA.BYTE = (TZ1.TIORA.BYTE & (0xf0 >> (i*4))) | (1 << (i*4));
		else
			TZ1.TIORC.BYTE = (TZ1.TIORC.BYTE & (0xf0 >> ((i&1)*4))) | (1 << ((i&1)*4));
		TZ1.TIER.BYTE &= ~(1<<i);
		TZ1.TSR.BYTE &= ~(1<<i);
	}
}

}
[/code]
Note: I just typed in the code above, have not compiled it or the like. It would probably generate smaller code, but it would be slower and I personally don’t think it is overly readable… Not sure what I am going to do yet…

Kurt

[kurte]

FYI - I did compile it… It turned out larger than the original…

[km6vv]

Larger? what sort optimizations are being done? Maybe it’s “unrolling” the loop. Maybe it was doing more global optimizations before, and lost some over this code. Hard to say, I’m not familiar with the GNU compilers. I got to know the HighTech compilers I was using pretty well.

Alan KM6VV

@kurte

FYI - I did compile it… It turned out larger than the original…

[kurte]

@KM6VV

Larger? what sort optimizations are being done? Maybe it’s “unrolling” the loop. Maybe it was doing more global optimizations before, and lost some over this code. Hard to say, I’m not familiar with the GNU compilers. I got to know the HighTech compilers I was using pretty well.

It actually did not surprise me, there are ways to change the code I wrote there to make it smaller. For example if you look at the generated code for the first test:

644:servo.cpp **** if ((TZ1.TSR.BYTE & (1<<i)) && (TZ1.TIER.BYTE & (1<<i))) { 8621 .ln 5 8622 0d7e 6A0AF713 mov.b @-2285:16,r2l 8623 0d82 1752 extu.w r2 8624 0d84 0C9B mov.b r1l,r3l 8625 0d86 4F00 ble .Lle42 8626 .Llt42: 8627 0d88 1192 shar.w r2 8628 0d8a 8BFF add #0xff,r3l 8629 0d8c 4600 bne .Llt42 8630 .Lle42: 8631 0d8e 79620001 and.w #1,r2 8632 0d92 0CAA mov.b r2l,r2l 8633 0d94 58700000 beq .L140:16 8634 0d98 6A0AF714 mov.b @-2284:16,r2l 8635 0d9c 1752 extu.w r2 8636 0d9e 0C9D mov.b r1l,r5l 8637 0da0 4F00 ble .Lle43 8638 .Llt43: 8639 0da2 1192 shar.w r2 8640 0da4 8DFF add #0xff,r5l 8641 0da6 4600 bne .Llt43 8642 .Lle43: 8643 0da8 79620001 and.w #1,r2 8644 0dac 0CAA mov.b r2l,r2l 8645 0dae 58700000 beq .L140:16 645:servo.cp
You see that it has to loop to generate the mask. Also since it turns out they are on the same bit, it generates smaller code (by 20 bytes) if the if was coded like:

if (TZ1.TSR.BYTE &TZ1.TIER.BYTE & (1<<i)) {
Note: with the current code, it knows the bits, so it can do things like:

606:servo.cpp **** if (TZ1.TSR.BIT.IMFA && TZ1.TIER.BIT.IMIEA) { 8482 .ln 4 8483 0c70 6A0AF713 mov.b @-2285:16,r2l 8484 0c74 EA01 and #1,r2l 8485 0c76 4700 beq .L133 8486 0c78 6A0AF714 mov.b @-2284:16,r2l 8487 0c7c EA01 and #1,r2l 8488 0c7e 4700 beq .L133
Again I may be able to make this smaller by changing the compare like I mentioned. (it would remove one of the beq)…

Next attempt may be to have tables for this. That is instead of doing shifts, maybe have a table with pointer to which variable and the mask… Not sure if it is worth it yet…

Kurt

[km6vv]

Shifts should be fast. A table look-up (pointers) will almost have to be slower.

Alan KM6VV

[kurte]

I am having some fun playing with this (probably boring others to death…) But a simple table lookup code like:

[code]volatile unsigned int* apwGRx] = {&TZ1.GRA, &TZ1.GRB, &TZ1.GRC, &TZ1.GRD};
volatile unsigned char* apbTIOARx] = {&TZ1.TIORA.BYTE, &TZ1.TIORA.BYTE, &TZ1.TIORC.BYTE, &TZ1.TIORC.BYTE};
volatile unsigned char abTOARxMask] = {0x7, 0x70, 0x7, 0x70};

extern “C” void TIMERZ2() attribute ((interrupt_handler));
extern “C” void TIMERZ2()
{
DSERVOTOGGLE(40); // BUGBUG:: For debug servo stuff.
int i;
register int iMask = 1;
for (i=0; i < 4; i++) {
if (TZ1.TSR.BYTE & TZ1.TIER.BYTE & iMask) {
apwGRx = 0xffff; // How to treat GRA-GRD like an array

		// How to handle TIORA/TIORC???  
		*apbTIOARx* = (*apbTIOARx* & ~abTOARxMask*) | (abTOARxMask* & 0x11);	// will only set the appropriate nibble to 1...
		TZ1.TIER.BYTE &= ~iMask;
		TZ1.TSR.BYTE &= ~iMask;
	}
	iMask <<= 1;
}

}[/code]
Disregarding the table sizes, this actually cut the code size by another 92 bytes.

@KM6VV

Shifts should be fast. A table look-up (pointers) will almost have to be slower.

---

Yes shifts of a bit are fast, they take 2 cycles, but when you do shift by a variable like (1<<i), it sets up a register with a 1, it sets up another register with the value of i, it has to do the shifts, decrement the counter and jump if not zero. So for each value of i it takes maybe 8 cycles plus the setup… But array access are not bad on the H8 either. For example in the code:

*apwGRx* = 0xffff; // How to treat GRA-GRD like an array 8656 .ln 7 8657 0d8e 0D10 mov.w r1,r0 8658 0d90 1010 shll.w r0 8659 0d92 78006B23 mov.w @(_apwGRx,er0),r3 8660 0d9a 7902FFFF mov.w #-1,r2 8661 0d9e 69B2 mov.w r2,@er3 652:
r1 has the value of i in it. So we simply move it to another register, shift it left 1 bit as our array is for 16 bit values, and then we use a load indirect where we simply give the starting address of our array, plus the offset and the value is loaded into r3… So again pretty nice. I may continue down this road as I can now easily merge in TimerZ0 and TimerZ1 code in the same array… And the setup code could probably all be done a similar way.

Again sorry for to everyone for this diversion… Soon back to making the phoenix on Arc32 work smoothly and then back to Hservo on Bap28…

Kurt**

[km6vv]

I almost wonder if you couldn’t use a union.

Alan KM6VV

[kurte]

I thought I would do another quick update here. I think I have the HServo2 type code working pretty well now. I have reduced the number of calls out of this to only use some in-line functions for setting the actual bits and the like. With that I have my C version of the Arc32 phoenix walking reasonably. I think there are still some tweaks left to do. Also I wanted to be able to run sequences on my Arc32 and the like. On my Phoenix->Arc32 port, I had code that used the Arc32’s EEProm to save the sequences. So I ported this code over to C. To do this I needed some EEPRom and Hardware I2C code, so I ported over some of MinionBuilders code over to my library. I made a few tweaks to it, like to initialize the hardware I2c when the first read or write was requested…

I also ported over some of my Arc32 phoenix extensions like the terminal monitor that allows me to set my debug level or dump EEPROM memory. I have also ported over the code to try to zero the servos, but I have not tried this part out yet, nor have I added the code to use these offsets within the Servo driver… Soon… Need to resolve how these zero points are saved/used. That is in basic they are currently stored in offsets in HSERVO units, whereas my C HSERVO system uses uSecs (IE 1500 is the center spot…)

Things to play with next.

1. Finish servo offset code both usage and terminal monitor code that allows me to adjust on the fly…
2. Go back to Hservo(Bap28/40) code and fully debug that…
3. Other stuff - receiving new leg tubes for my CHR-3 to use with contact switches. Nice to try that stuff out and ideas on how to process the input. Question will be to do in Basic or C…
4. Maybe a poor quality video showing the phoenix actually working with the C code…

Kurt
CPhoenix with Arduinosh lib.zip (226 KB)

[kurte]

There have been a couple of people up on the BasicMicro and Trossen Forums wanting to code in C using the Arc32. So I decided to play with it a little again.

I first was playing the CHR-3 with SSC-32 and Arc32 and had it working. I decided to re implement the differences again for having it run on an Arc32 without an SSC-32. This time I did it within the same project and files with #ifdefs which are defined in the different configuration files. Currently defining which type of configuration you are building for is done in the file: globals.h in the CPhoenx_Arduino directory. You set this up by enabling different defines in the code.

#define ARC32_PHOENIX

#define BASICATOMPROARC32
//#define BASICATOMPRO40	//try a 40
//#define BASICATOMPRO28

The first define there has the code use the phoenix config file otherwise it uses the CHR-3 config file. You must define ARC32 for the phoenix config. There are probably issues with the Bap40/Bap28 for the CHR-3 config as well.

I updated the Servo system to allow me to set servo offsets, such that I only need to set them once and than not have to change all of the code to use these
offsets. Note: the servo system is still only working on the Arc32. I have not gone back to fix it for the Bap28 or Bap40, I will probably do that soon unless I get back into a different diversion To test the servos for a Bap28, I will probably try this for a Brat.

Things left to do with the Arc32 phoenix code:

1. see why there are delays in running when I am doing translations/rotations.
2. work a little more on robustness of communications. It appears to drop from time to time…
3. HServo2 type code is implemented, but I have not implemented the Analog Input code

Maybe a better video. Here is one that shows that it at least works:

There will most likely be more tweaks to the sources, but in case anyone wishes a peek:

Kurt

[zenta]

Hi Kurt,

So you’ve finally joined youtube!
I hope you’ll post some more vids in the future.
It sure looks like the code need some tuning, but great progress.

[xan]

Hi kurt,

I second Zenta, good to see you joined YouTube!

I see that the phoenix is a bit shakey… Probably some timing issues causing the next step to hit in before the previous one is finished. I’m sure you’ll figure it out.

Xan

[kurte]

Quick update:

I decided to take a break from the phoenix (Arc32 with/without SSC-32) and try Brat on Bap28 again… For this I needed to clean up the HServo type code that works without the extra hardware of the SSC-32). Taking longer than I expected as I ran into some issues that took me awhile (longer than it should have) to debug.

1. After I would start the servos, The processor randomly would reset… My debug code (dump memory when fault happens), implied I was trying to do an I2c Interrupt. Finally tracked down to a simple line in my Ps2 code that looked like: ps2DPrev = ps2D;
   It appears that the runtime memory copy is not interrupt safe and if an interrupt happens at the wrong time it may copy to wrong place…

2. Some servos would jitter. Figured out caused by my IO pin high/low/toggle functions were not interrupt safe. That is my fasted way to set an IO pin high looks like: #define PI_HIGH(pi) (*(pi.pPDR) |= pi.bMask)
   This is not atomic and if between the time you do the fetch the data from the register and the time you put back the value for the 8 IO lines, an interrupt comes along and changes the state of some of those pins, those changes will be clobbered when this macro completes and puts the result back out. Fixed by creating versions which disable interrupts during this time frame…

So things are looking more solid now. Now need to work on making the Brat code work properly (things like servo offsets…)

Let me know if anyone wants to look at the code before than…

Kurt

[kurte]

Made some more progress on the brat Now walks reasonably well (still need to work more on servo zeroing…) Reads them from EEPROM and uses the same values as I had for Basic version, so I needed to scale.

FYI - I found a problem in my basic code in the ReadServoOffsets and WriteServoOffsets where the checksums were not generated correctly. I fixed on my version, but need to replicate on all of the tutorial versions…

Decided to upload new versions of library/Phoenix/Brat code here as it fixed the issues with hservo as well as problems with interactions with interrupts doing IO…

Kurt

[kurte]

Quick update. Brat code now rotate hips and turret, like MechBrat. Fixed issues with HServoPos to make sure it takes the offset into account. That is if you set a servo to some location and then do a query, you expect the same value to be returned…

Kurt
CPhoenix with Arduinosh lib.zip (213 KB)

[kurte]

Unless I divert myself with something else which is always possible , I am going to play around next with adding something like hserinstat support for the Arc32 in my C library code. But before that I will post a zip with current stuff. Some of the changes since my lost posting include:

1. Fix Servo code. There were times with Arc32, when I logically turn the hex off (hit logical start button), the legs would go spastic. Figured it out, problem with code when Pulse width went from something to 0…

2. Reworked Servo code for Bap40. Found older version was taking about 26ms per cycle, was only using TimerZ0 now using both TImerZ0 and Z1…

3. Did not like that TimerZ0 and now Z1 were having to decide at run time if was Bap40 or Arc32 on every interrupt. So changed the interrupt handlers to names like: TImerZ0_BAP40 and TimerZ0_Arc32 and changed the vects.c in each of my robot projects to conditionally compile to use those vectors instead of the generic TimerZ0… So saved time of not having to compare and either branch or call… Warning: this implies changes to each project…

3a) As part of the above, I split the Servo code into a couple of files, such that hopefully if you compile for the Arc32, it does not bring into your executable the code for a Bap40…

4. ADIN - Changed the code to table drive this from the IO pin information instead of hard coded pin numbers. Have tested on Bap28 with the CBrat project that uses the sharp sensor… Tested on Arc32 with CPhoenix_Arduino project to grab the voltage information for VL and VS0. Note: Found issue with input function that as part of ADIN I made sure that the IO pin was input, but on the Arc32 case the IO pins were on PDRB which has corresponding PCR register and this caused issues, so it now checks for this…

Thats all for now

Kurt
P.S. - I am thinking of splitting this off soon to different thread either under BAP or Arc32 as this is not just phoenix code…
CPhoenix with Arduinosh lib.zip (216 KB)

[kurte]

As I mentioned in the last post, I am now posting more generic updates into a the thread about adding C support for the Arc32…
viewtopic.php?f=45&t=6677&p=70050#p70050

I have just uploaded the code that adds the equivalent of HSERVOSTATE for the Arc32…

Kurt

[botdoctor]

Hey RobotGuru, can this be run on the raspberry pi? Or in other words. Can it be compiled under linux?

And how to install the library?

[kurte]

Not that I know of. That is I don’t think the Basic Micro Studio can be setup to run under linux. Could be wrong. Sounds like a question to ask Nathan (AcidTech). Might want to ask it up on their forum forums.basicmicro.net/.

Kurt

P.S. - As you probably have already found out, I do also have this running (and more up to date) on Arduinos and I believe you can setup Arduino to run on Linux.