/* -- simpwalk.h --
 */
#define LEGDOWN_CYCLE_TIME                      50
#define ALPHA_ADVANCE_CYCLE_TIME        10
#define GAIT_CYCLE_TIME                         500
 
typedef enum { ALPHA_DECR = -1, NEUTRAL, ALPHA_INCR } POSITION;
#define BETA_UP         127
#define BETA_DOWN       -128
 
#define IS_BETA_UP(x)   !IS_BETA_DOWN(x)
#define IS_BETA_DOWN(x) ((x) < 0)
 
typedef struct
        {
        char    *inName;
        char    *outName;
        int             servoName;
        } IO_NAME;
 
extern IO_NAME alphaData[], betaData[];
void alphaPos(void), betaPos(void);
 
/* -- standup.c --
 */
#include <stdio.h>
#include "exec.h"
#include "simpwalk.h"
 
#define alpha0Servo 0
#define alpha1Servo 1
#define alpha2Servo 2
#define alpha3Servo 3
#define alpha4Servo 4
#define alpha5Servo 5
#define beta0Servo  6
#define beta1Servo  7
#define beta2Servo  8
#define beta3Servo  9
#define beta4Servo  10
#define beta5Servo  11
 
IO_NAME alphaData[] = {
        { "alpha0Pos.in", "alpha0Pos.out", alpha0Servo },
        { "alpha1Pos.in", "alpha1Pos.out", alpha1Servo },
        { "alpha2Pos.in", "alpha2Pos.out", alpha2Servo },
        { "alpha3Pos.in", "alpha3Pos.out", alpha3Servo },
        { "alpha4Pos.in", "alpha4Pos.out", alpha4Servo },
        { "alpha5Pos.in", "alpha5Pos.out", alpha5Servo }
        };
 
IO_NAME betaData[] = {
        { "beta0Pos.in", "beta0Pos.out", beta0Servo },
        { "beta1Pos.in", "beta1Pos.out", beta1Servo },
        { "beta2Pos.in", "beta2Pos.out", beta2Servo },
        { "beta3Pos.in", "beta3Pos.out", beta3Servo },
        { "beta4Pos.in", "beta4Pos.out", beta4Servo },
        { "beta5Pos.in", "beta5Pos.out", beta5Servo }
        };
 
/*      alpha position.  One per leg.
 */
void alphaPos(void)
        {
        IO_NAME *pData = &alphaData[ExecArg0];
        PORT portValue, position;
        STRING_ATOM output = StringToAtom(pData->outName);
 
        MsgOpenPort(output, &portValue);
        MsgWriteToPort(output, 
                        servoWrite(pData->servoName, NEUTRAL));
        MsgWatchPort(StringToAtom(pData->inName), &position);
        while (MsgGetMsg())
                {
                MsgWriteToPort(output, 
                                servoWrite(pData->servoName, position));
                }
        }
 
/*      beta position.  One per leg.
 */
void betaPos(void)
        {
        IO_NAME *pData = &betaData[ExecArg0];
        EXEC_ID id, timerID;
        PORT currentPos, portValue, position;
        STRING_ATOM output = StringToAtom(pData->outName);
 
        MsgOpenPort(output, &portValue);
        MsgWriteToPort(output, BETA_DOWN);
 
        servoWrite(pData->servoName, position = BETA_DOWN);
        while (servoReadWrite(pData->servoName, position) != BETA_DOWN)
                ;
        MsgWatchPort(StringToAtom(pData->inName), &position);
        timerID = MsgWatchTimer(1);
 
        while (id = MsgGetMsg())
                {
                if (id == timerID)
                        MsgWriteToPort(output,
servoReadWrite(pData->servoName, position));
                else
                        servoWrite(pData->servoName, position);
                }
        }
 
/* -- simpwalk.c --
 */
#include <stdlib.h>
#include "exec.h"
#include "simpwalk.h"
 
/*      interface names
 */
IO_NAME legDownData[] = {
        { "leg0Down.in", "leg0Down.out" },
        { "leg1Down.in", "leg1Down.out" },
        { "leg2Down.in", "leg2Down.out" },
        { "leg3Down.in", "leg3Down.out" },
        { "leg4Down.in", "leg4Down.out" },
        { "leg5Down.in", "leg5Down.out" }
        };
 
IO_NAME advanceData[] = {
        { "alpha0Advance.in", "alpha0Advance.out" },
        { "alpha1Advance.in", "alpha1Advance.out" },
        { "alpha2Advance.in", "alpha2Advance.out" },
        { "alpha3Advance.in", "alpha3Advance.out" },
        { "alpha4Advance.in", "alpha4Advance.out" },
        { "alpha5Advance.in", "alpha5Advance.out" }
        };
 
IO_NAME uplegData[] = {
        { "upleg0Trigger.in", "upleg0Trigger.out" },
        { "upleg1Trigger.in", "upleg1Trigger.out" },
        { "upleg2Trigger.in", "upleg2Trigger.out" },
        { "upleg3Trigger.in", "upleg3Trigger.out" },
        { "upleg4Trigger.in", "upleg4Trigger.out" },
        { "upleg5Trigger.in", "upleg5Trigger.out" }
        };
 
/*      put the leg down if it's up.  One per leg.
 */
void legDown(void)
        {
        IO_NAME *pData = &legDownData[ExecArg0];
        PORT legPosition;
        STRING_ATOM betaOut, output;
 
        betaOut = StringToAtom(betaData[ExecArg0].outName);
        output = StringToAtom(pData->outName);
        /*      connect our output to the beta position's input
         */
        MsgConnectPorts(output, StringToAtom(betaData[ExecArg0].inName));
        MsgWatchTimer(LEGDOWN_CYCLE_TIME);
 
        while (MsgGetMsg())
                {
                MsgPeekPort(betaOut, &legPosition);
                if (IS_BETA_UP(legPosition))
                        MsgWriteToPort(output, BETA_DOWN);
                }
        }
 
/*      if the total alpha positions are positive, then move them back.
 *      If it is negative, move them forward.  One for all legs.
 */
void alphaBalance(void)
        {
        STRING_ATOM alpha0, alpha1, alpha2, alpha3, alpha4, alpha5;
        STRING_ATOM alphaOut;
        PORT totalAlphaPosition;
 
        alpha0 = StringToAtom(alphaData[0].outName);
        alpha1 = StringToAtom(alphaData[1].outName);
        alpha2 = StringToAtom(alphaData[2].outName);
        alpha3 = StringToAtom(alphaData[3].outName);
        alpha4 = StringToAtom(alphaData[4].outName);
        alpha5 = StringToAtom(alphaData[5].outName);
        alphaOut = StringToAtom("alphaBalance.out");
        /*      connect our output to the alpha motors' input
         */
        MsgConnectPorts(alphaOut, StringToAtom(alphaData[0].inName));
        MsgConnectPorts(alphaOut, StringToAtom(alphaData[1].inName));
        MsgConnectPorts(alphaOut, StringToAtom(alphaData[2].inName));
        MsgConnectPorts(alphaOut, StringToAtom(alphaData[3].inName));
        MsgConnectPorts(alphaOut, StringToAtom(alphaData[4].inName));
        MsgConnectPorts(alphaOut, StringToAtom(alphaData[5].inName));
 
        MsgWatchTimer(ALPHA_ADVANCE_CYCLE_TIME);
        while (MsgGetMsg())
                {
                totalAlphaPosition =
                        MsgPeekPort(alpha0, NULL) +
                        MsgPeekPort(alpha1, NULL) +
                        MsgPeekPort(alpha2, NULL) +
                        MsgPeekPort(alpha3, NULL) +
                        MsgPeekPort(alpha4, NULL) +
                        MsgPeekPort(alpha5, NULL);
                if (totalAlphaPosition < 0)
                        MsgWriteToPort(alphaOut, ALPHA_INCR);
                else if (0 < totalAlphaPosition)
                        MsgWriteToPort(alphaOut, ALPHA_DECR);
                }
        }
 
/*      if the leg is up, then move leg forward.  One per leg.
 */
void alphaAdvance(void)
        {
        PORT betaPosition;
        STRING_ATOM input, output, alphaPosInput;
 
        /*      wakes up whenever the beta position changes
         */
        input = StringToAtom(advanceData[ExecArg0].inName);
        MsgWatchPort(input, &betaPosition);
 
        alphaPosInput = StringToAtom(alphaData[ExecArg0].inName);
        output = StringToAtom(advanceData[ExecArg0].outName);
 
        /*      change alpha position input to connect to our output
         */
        MsgInhibitPorts(output, alphaPosInput);
        MsgConnectPorts(StringToAtom(betaData[ExecArg0].outName), input);
 
        while (MsgGetMsg())
                {
                if (IS_BETA_UP(betaPosition))
                        MsgWriteToPort(output, ALPHA_INCR);
                }
        }
 
/*      if the leg is down, move it up
 */
void uplegTrigger(void)
        {
        PORT betaPosition;
        STRING_ATOM betaInput, betaOutput, output;
 
        betaInput = StringToAtom(betaData[ExecArg0].inName);
        betaOutput = StringToAtom(betaData[ExecArg0].outName);
 
        output = StringToAtom(uplegData[ExecArg0].outName);
        MsgSuppressPorts(output, betaInput);
        MsgWatchTrigger(StringToAtom(uplegData[ExecArg0].inName));
 
        while (MsgGetMsg())
                {
                MsgPeekPort(betaOutput, &betaPosition);
                if (IS_BETA_DOWN(betaPosition))
                        MsgWriteToPort(output, BETA_UP);
                }
        }
 
void TripodGait(void)
        {
        STRING_ATOM evenCycleID, oddCycleID;
        int cycle;
 
        evenCycleID = StringToAtom("walk.even");
        oddCycleID = StringToAtom("walk.odd");
 
        /*      alternately raise the legs in a tripod gait fashion
         */
        MsgConnectPorts(evenCycleID, StringToAtom(uplegData[0].inName));
        MsgConnectPorts(evenCycleID, StringToAtom(uplegData[2].inName));
        MsgConnectPorts(evenCycleID, StringToAtom(uplegData[4].inName));
        MsgConnectPorts(oddCycleID, StringToAtom(uplegData[1].inName));
        MsgConnectPorts(oddCycleID, StringToAtom(uplegData[3].inName));
        MsgConnectPorts(oddCycleID, StringToAtom(uplegData[5].inName));
 
        MsgWatchTimer(GAIT_CYCLE_TIME);
        while (MsgGetMsg())
                {
                if (cycle)
                        MsgWriteToPort(oddCycleID, 0);
                else
                        MsgWriteToPort(evenCycleID, 0);
                cycle ^= 1;
                }
        }
 
#ifndef HC11
void nil(void)
        {
        MsgWatchTimer(4);
        while (MsgGetMsg())
                if (kbhit())
                        {
                        if (getchar() == 'q')
                                exit(0);
                        }
        }
#endif
 
#define SSIZE   100
#define XSIZE    100
#define BSIZE   100
 
int main()
        {
        void printServos();
 
        ExecInit();
        ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 0, 0);
        ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 1, 0);
        ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 2, 0);
        ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 3, 0);
        ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 4, 0);
        ExecCreateThread(NORMAL_PROC, alphaPos, XSIZE, 5, 0);
 
        ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 0, 0);
        ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 1, 0);
        ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 2, 0);
        ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 3, 0);
        ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 4, 0);
        ExecCreateThread(NORMAL_PROC, betaPos, BSIZE, 5, 0);
 
        ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 0, 0);
        ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 1, 0);
        ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 2, 0);
        ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 3, 0);
        ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 4, 0);
        ExecCreateThread(NORMAL_PROC, legDown, SSIZE, 5, 0);
 
        ExecCreateThread(NORMAL_PROC, alphaBalance, BSIZE, 0, 0);
 
        ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 0, 0);
        ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 1, 0);
        ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 2, 0);
        ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 3, 0);
        ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 4, 0);
        ExecCreateThread(NORMAL_PROC, alphaAdvance, SSIZE, 5, 0);
 
        ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 0, 0);
        ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 1, 0);
        ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 2, 0);
        ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 3, 0);
        ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 4, 0);
        ExecCreateThread(NORMAL_PROC, uplegTrigger, SSIZE, 5, 0);
 
        ExecCreateThread(NORMAL_PROC, TripodGait, SSIZE, 0, 0);
#ifndef HC11
        ExecCreateThread(NORMAL_PROC, nil, 200, 0, 0);
        ExecCreateThread(NORMAL_PROC, printServos, 600, 0, 0);
#endif
        ExecStart();
        }
 
- - --- end ---

