Event based control

[cp]

I am developing a platform that uses the MR-C3024 and four Hitec HS-5955TG servos. Although I am not using the Robonova-I, my situation may be relevant to others here. I am using RoboBasic and the Hitec IR remote. What I need to achieve is for the robot to quickly respond to inputs on the A/D lines, ideally within 10ms. However, I am unable to get the controller to consistently respond, the reaction time appears to be anywhere from 10-500ms.

I have tested code with a short control loop that looks for three inputs. Two (i & k) are digital signals that I am reading through the A/D lines (for lack of an easy to use digital input), the third input is for the remote. i and k operate independently, as you will see in the code below.

The code will loop continuously until one of the three input conditions is met. In two of those cases, the motors are then commanded to new positions. It is the delay in sending the motor commands that is causing me much frustration.

Does anyone here have enough knowledge of how the firmware functions to know if it is possible for me to achieve my goals?

Thanks in advance...

CODE LOOP ---------------------------------

do_forward:

'Read sensors here
i = AD(0)
IF i > AD_ON AND FRONT_LS_FLAG = LF_STATE THEN

SPEED RETRACT_SPEED
MOVE G6A,RUN_ANG_LEFT_AFT,,,,,RUN_ANG_RIGHT_AFT
FRONT_LS_FLAG = LS_STATE

ELSEIF i < AD_OFF AND FRONT_LS_FLAG = LS_STATE THEN

SPEED RETURN_SPEED
MOVE G6A,RUN_ANG_LEFT_FORE,,,,,RUN_ANG_RIGHT_FORE
FRONT_LS_FLAG = LF_STATE

ENDIF

k = AD(3)
IF k > AD_ON AND REAR_LS_FLAG = LF_STATE THEN

SPEED RETRACT_SPEED
MOVE G6D,RUN_ANG_LEFT_AFT,,,,,RUN_ANG_RIGHT_AFT
REAR_LS_FLAG = LS_STATE

ELSEIF k < AD_OFF AND REAR_LS_FLAG = LS_STATE THEN

SPEED RETURN_SPEED
MOVE G6D,RUN_ANG_LEFT_FORE,,,,,RUN_ANG_RIGHT_FORE
REAR_LS_FLAG = LF_STATE

ENDIF

' read remote:
A=REMOCON(1)
IF A <> 0 THEN

GOSUB do_remote
RETURN

ENDIF
GOTO do_forward

[Bullit]

Robobasic is very slow. It executes directly from the serial eeprom as intermediate code. I'd say whats costing you the most time is the read of the IR sensor. I believe it has a timeout associated with it. Reads from serial ports are significantly faster.
I too have had difficulties getting robobasic to perform in any kind of realtime.
I-bot and Dan Albert have done a great deal of work on creating a replacement for robobasic that would allow us to program in C. I don't believe either is finished but they both sound close.

[cp]

Robobasic is very slow. It executes directly from the serial eeprom as intermediate code. I'd say whats costing you the most time is the read of the IR sensor. I believe it has a timeout associated with it. Reads from serial ports are significantly faster.
I too have had difficulties getting robobasic to perform in any kind of realtime.
I-bot and Dan Albert have done a great deal of work on creating a replacement for robobasic that would allow us to program in C. I don't believe either is finished but they both sound close.

Wow! That made a HUGE difference. The only change I made was to put the IF statements into a FOR .. NEXT loop so they would be executed a number of times before an attempt is made to read the remote.

Thanks for the very helpful insight!

See my changes below:

CODE LOOP ----------------------------------------

do_forward:

FOR COUNT = 1 TO 20

'Read sensors here
i = AD(0)
IF i > AD_ON AND FRONT_LS_FLAG = LF_STATE THEN

SPEED RETRACT_SPEED
MOVE G6A,RUN_ANG_LEFT_AFT,,,,,RUN_ANG_RIGHT_AFT
FRONT_LS_FLAG = LS_STATE

ELSEIF i < AD_OFF AND FRONT_LS_FLAG = LS_STATE THEN

SPEED RETURN_SPEED
MOVE G6A,RUN_ANG_LEFT_FORE,,,,,RUN_ANG_RIGHT_FORE
FRONT_LS_FLAG = LF_STATE

ENDIF

k = AD(3)
IF k > AD_ON AND REAR_LS_FLAG = LF_STATE THEN

SPEED RETRACT_SPEED
MOVE G6D,RUN_ANG_LEFT_AFT,,,,,RUN_ANG_RIGHT_AFT
REAR_LS_FLAG = LS_STATE

ELSEIF k < AD_OFF AND REAR_LS_FLAG = LS_STATE THEN

SPEED RETURN_SPEED
MOVE G6D,RUN_ANG_LEFT_FORE,,,,,RUN_ANG_RIGHT_FORE
REAR_LS_FLAG = LF_STATE

ENDIF

NEXT COUNT

' read remote:
A=REMOCON(1)
IF A <> 0 THEN

GOSUB do_remote
RETURN

ENDIF
GOTO do_forward

[i-Bot]

I'm please to see you have some success.

To better understand the C3024 timing for real time.

Take the following code:

DIM A AS BYTE
TIGHT: TOGGLE 23
' MOTOR G6A
'REMOCON(1)
GOTO TIGHT

When you run this you get a square wave with a period of about 6 ms. This shows the typical rate of the interpreter in the C3024 at about 1 to 2 ms per instruction. You will see some jitter on the toggle, this is because there is a timer interrupt running every 4 ms which take about 0.5 ms

If you now uncomment the motor command and run again you will see the period is now about 40 ms. This is beacause the time taken in the 4ms interrupt has increased to about 3ms, due to fact that servos are updated in this routine if enabled. Any motor enabled will make the change to longer delay.

Finally if you recomment the motor and uncomment the remocon. You now see the period increase to around 180ms due to wait for a response from the remote control of about 80msec.

So as suggested by bullit, and what you have done with remocon has the largest effect. If you still need better then turn off the motors when you don't need them. Your 5955 servos are digital, so will hold their position.

If you have access to an oscilloscope check out the above for yourself, it is a good way to understand the timing of the different statements.

As described by bullit, I use GCC on the C3024 using the original asembler routines as C functions. This means I also have similar timing constraints. Obviously my instruction cycle time is 140ns if I use in line assembly. I also have the timer interrupt runing, and this is a hog of processing. This means I also have large chunks of time lost especially if motors are enabled. So far I have got around this by turning off interrupts for short time critical code ( the normal C3024 code also does this). I also put code in the timer interrupt at 4 or 16 ms if I need to perform it regularily.

[cp]

Thanks i-Bot, I have a couple of questions to clarify my understanding.

If you now uncomment the motor command and run again you will see the period is now about 40 ms. This is beacause the time taken in the 4ms interrupt has increased to about 3ms, due to fact that servos are updated in this routine if enabled. Any motor enabled will make the change to longer delay.

If you still need better then turn off the motors when you don't need them. Your 5955 servos are digital, so will hold their position.

Does this mean that if I use

MOTOR G6A - instead of

MOTOR 0
MOTOR 5

that the interrupt delay would be 3 times longer? (having enabled six motors instead of two)

Too bad the MOTOR and MOTOROFF command can only be invoked for one motor at a time, presumably this increases the time required to execute the commands.

Secondly, when digital motors are "turned off" do they hold their position because of the gearing, or simply because they are actively controlling their position while awaiting their next command? I need all of my motors to hold a load at their target positions at all times.

Also, can the MOTOROFF command be invoked while the motors are moving to their new targets? (I presume the answer to this question will be tied to my question above)

Assuming the motors would remain energized after MOTOROFF, to get the best response times I would think my code could be structured as follows:

---------- BEFORE ------------

'Read sensors here
i = AD(0)
IF i > AD_ON AND FRONT_LS_FLAG = LF_STATE THEN

SPEED RETRACT_SPEED
MOVE G6A,RUN_ANG_LEFT_AFT,,,,,RUN_ANG_RIGHT_AFT
FRONT_LS_FLAG = LS_STATE

ELSEIF i < AD_OFF AND FRONT_LS_FLAG = LS_STATE THEN

SPEED RETURN_SPEED
MOVE G6A,RUN_ANG_LEFT_FORE,,,,,RUN_ANG_RIGHT_FORE
FRONT_LS_FLAG = LF_STATE

ENDIF

------- AFTER -------- (note motor numbers are intentionally changed from above)

'Read sensors here
k = AD(3)
IF k > AD_ON AND REAR_LS_FLAG = LF_STATE THEN

MOTOR 2
MOTOR 3
SPEED RETRACT_SPEED
MOVE ,,RUN_ANG_LEFT_AFT,RUN_ANG_RIGHT_AFT
REAR_LS_FLAG = LS_STATE
MOTOROFF 2
MOTOROFF 3

ELSEIF k < AD_OFF AND REAR_LS_FLAG = LS_STATE THEN

MOTOR 2
MOTOR 3
SPEED RETURN_SPEED
MOVE ,,RUN_ANG_LEFT_AFT,RUN_ANG_RIGHT_AFT
REAR_LS_FLAG = LF_STATE
MOTOROFF 2
MOTOROFF 3

ENDIF

--------------

I also have the timer interrupt running, and this is a hog of processing. This means I also have large chunks of time lost especially if motors are enabled. So far I have got around this by turning off interrupts for short time critical code ( the normal C3024 code also does this). I also put code in the timer interrupt at 4 or 16 ms if I need to perform it regularily.

One drawback I found from using the FOR NEXT loop to reduce the REMOCON reads is that I find I have to accept reduced responsiveness to the remote in order to get fast timing. Hopefully by disabling the motors, I can get the additional responsiveness I need from both the controller and the remote. If I can't get acceptable results, is there anything else I can do from within RoboBasic?

Thanks!

[i-Bot]

Sory the way I structured the example was not so good.

I should have put the motor command before the loop thus.

DIM A AS BYTE
'MOTOR G6A
TIGHT: TOGGLE 23
'REMOCON(1)
GOTO TIGHT

Having any motor enabled will make the increase in the time used for the interrupt routine. Interrupt delay time is independant of number of servos

Motoroff will stop any group moves that are in progress at that time. And the servo will freeze the position at that time assuming the move command has had time to send at least one pulse to the servo. WAIT is normally used to check the move is ended, but this a loop where nothing else can be executed. I created a new command in C to test motion complete. You might acheive the same by using a peek to location 0x4E3 which has the bits which show if a servo is motion is done for the first 8 servos. I haven't tried this though.

The MOTOR and MOTOROFF for a group should be faster than doing individual servos.

A digital servo will hold the position it last received with active full torque drive. The 8498 servo can be turned off, so the the resistance is just the gearing, and some resistance from the motor but this cannot be done on the 5995

[DanAlbert]

I had the same response issue when setting up the 8 RNs to play soccer for RoboGames 2006

I wanted to send a string of ascii data as the controls on the PlayStation controller changed. Ouch I was bit by the crappy code inside the 3024.

I have rewritten the ATmega 128 code in C and assembly code on my RN (after changing out the chip) and almost had it working when I decided to move to CA. Well I have arrived and should be picking up the project again very soon. The biggest issue is how to reload the code into an existing RN board.

As has been posted in this forum the firmware is locked so that the JTAG port cannot talk to a JTAG programmer. However it is very unlikely that Hitec has completely locked everything. There is a way to reprogram the OS and I may suprise everyone with the answer.

Of course you would be required to program in C to use the code.

[cp]

So I'm now doing some timing tests using the TOGGLE command and a scope. I am running the code I posted in my second post.

The timing is awful. With the TOGGLE command inside the FOR loop, I am getting ~90ms to go though the two IF statements, and a total of 520ms when the REMOTE command is invoked.

If I comment out the second IF statement and A/D read, the cycle time drops to ~51ms, with the REMOTE command taking ~470ms

Commenting out both IF statements (and thus having only the TOGGLE command), I get 13ms(!) for the cycle time, and ~415ms for the REMOTE command.

If I never enable the motors at all, the cycle time drops to 4ms, and the REMOTE read takes ~110ms.

On the other hand, if I set the motors to a really slow speed, it becomes obvious that the code execution waits until a motor move is complete before continuing. When the MOVE command is invoked, the cycle time will increase to ~400ms while the motors move to their new position.

I wonder how the heck this system is supposed to work with a gyro when it is nowhere near real-time?

[i-Bot]

The move command waits before execuitng if there is already a move in progress. So you can do other instructions whilst moving. a wait will stop other commands being executed till motion is complete.

The gyros are read within a phase of the interrupt, and the corrections made directly to the servo positions. This is performed every 16 ms. However you do have to be careful, since some commands do disable interrupts for time critical code

[cp]

After doing more tests and much deliberation, I am biting the bullet and dumping the HR-C3024 in favour of a custom board with an SiLabs C8051F120. The code will be written in C.

The C8051F120 is a much faster processor, memory, and more I/O. My hope is to be able to eliminate the problems with the built-in delays experienced with the Hitec boards.

This will be a huge undertaking for me, as writing coding for micros is a new area for me. My intent is to create the basic functionality I need for my current project, but I would consider making a version of the board available for people wanting to do advanced development with their projects.

Wish me luck....