The Problem

Suppose a BrainStem must perform a sequence of motions with servo motors.  The completion of one motion must trigger the next.

The Solution

The BrainStem GP 1.0 can perform speed-controlled motions with servos.  When servos operate in this mode they can trigger a reflex once a motion is completed.  If a TEA program executes a command to start the motion, that program can stall until its semaphore is written.  The servo reflex can write to the TEA program's semaphore to make the program continue its execution.

The Details

We need to describe this logic in terms that can be directly translated into BrainStem commands or TEA program statements.  Each servo may be configured to issue a reflex once it completes a motion using either the cmdSRV_RFLX command or aServo_SetReflex TEA subroutine.  The cmdDEV_VAL command may be used to write to a semaphore from within a reflex.  This is one way for TEA programs and reflexes to communicate.  Any unused raw input ID can be assigned to a servo reflex.  An ID of 100 is used in this example.  The reflex consists of a single vector and single command:

A sample TEA program illustrates how the servo reflex can control a sequence of motions.  The program implements a simple back-and-forth servo loop for servo 0.  First, it tells the servo to move to position 0 at maximum speed.  It waits for a half second to ensure that the first motion is completed.  Then it configures the servo to remain enabled, but have a speed of 4.  Then it enters an endless loop of servo motions.

The "move" subroutine configures the reflex for servo 0.  Then it tells the servo to begin its motion.  It waits until the program's semaphore is written, then returns.  The reflex writes to the semaphore once the motion is complete.  Each call to "move" must reset the reflex for servo 0 so that it will be triggered again when the new motion is completed.

This sample program assumes the TEA program is running as process 0.  The aPortSemaphore identifier is equivalent to 0x0100 which is the control port for the semaphore for process 0.

ex7_srvrflx.tea

Download "ex7_srvrflx.tea" here

#include <aCore.tea> #include <aPrint.tea> #include <aServo.tea> void move(unsigned char p) {   aServo_SetReflex(0, 0xE400);   aServo_SetAbsolute(0,p);   asm   {     pushms aPortSemaphore     pops   } } void main () {   aServo_SetAbsolute(0,0);   aCore_Sleep(5000);   aServo_SetConfig(0, (char)(SRV_ENA | 4));   while (1)   {     aPrint_String("Back...\n");     move((unsigned char)255);     aPrint_String("and forth...\n");     move((unsigned char)0);   } }

The Commands

In the reflexive routine described above, there is only one command.  This command, complete with address byte and size byte, is listed below.  The value of 53 is the device identifier associated with the semaphore for process 0.

This command goes in reflex command slot 0.  The cmdMSG_WR command listed below will save the command data into the appropriate location in the EEPROM.

The Vectors

Now that the reflex command is in the EEPROM, we need to define the reflex vector data for reflex 100.  The binary contents of the reflex vector along with a description of what the contents mean is listed below:

This vector data goes into reflex vector slot 100.  The cmdVEC_WR command listed below will save the vector data into the appropriate location in the EEPROM.

The Results

The reflexive control routine is now completely defined.  A text batch file can store all the commands for downloading the reflex data.  Copy and paste the following lines of numbers to a file called "srvctrl.txt" in your "aUser" directory.  The Console can execute these commands automatically.  Just enter batch "srvctrl.txt" at the prompt.

2 11 128 100 128 128
2 12 128 0 2 3 4 53 0

To test the routine, copy and paste the source of the "ex7_srvrflx.tea" program to your aUser directory.  Run the Console and compile the TEA program with the steep command.  Download the program into file slot 0 with the load command.

Run program 0 by entering "launch 2 0" at the Console prompt.  Servo 0 should move quickly to one side.  Then the message "Back..." should appear while the servo is moving to the other side.  Once that motion is complete, the message "And forth..." should appear while the servo is moving to the other side.  This motion will continue until the program is halted.  While the program is still running, try the following command:

2 31 0 143

That keeps the enable bit set for servo 0, but sets the speed to 15.  The motion will be much quicker, but the servo will still move to the same positions.

2 31 0 129

That keeps the enable bit set for servo 0, but sets the speed to 1.  The motion will be much slower.

This example demonstrates how reflexes can be used to coordinate speed-controlled servo motions.  It also demonstrates how reflexes and TEA programs can communicate via semaphores.

Here is a LEAF source file for the reflex routine in this example:

ex7_srvrflx.leaf

Download "ex7_srvrflx.leaf" here

#include <aCmd.tea> #include <aGPReflexes.tea> /* assume the BrainStem has address 2 */ #define MODULE 2 #define SEMAPHORE0 53 module[MODULE] {   /************* REFLEX COMMANDS *************/   message[0] {     MODULE, cmdDEV_VAL, SEMAPHORE0, 0   }   /************* REFLEX VECTORS *************/   vector[100] {     0   } }

Here is the BAG output from the LEAF compiler:

ex7_srvrflx.bag

Download "ex7_srvrflx.bag" here

// module 2 (1 messages, 1 vectors) // message 0 2 12 128 0 2 3 4 53 0 // vector 100 2 11 128 100 128 128

This batch file contains the same commands for downloading the reflex that were listed for the "srvctrl.txt" file, but in a different order.