P2 and the Tank

iseries · 2026-02-17 20:05

I found this tank on Amazon and just had to get one.
Blue Tank
I got it with the encoders so I could measure the output of the motors under load.

After getting the unit I first tested the motors with a MDD3A board I had laying around. This inexpensive board works very nicely.
MDD3A

I could have used the Parallax BLDC board I also had laying around but it was not plug and play setup for my P2 board.

The motors came with Quadrature encoders so setup a smart pin to record the motor turns was a snap.
XR25-370

_pinstart(H1A, P_PLUS1_B | P_QUADRATURE, 0, 0);

The motor came with a cable with both ends attached so I removed the one end and cut off the quadrature pins and installed new pins to fit in Harwin connector. The other two were inserted in the motor drive screws.

Assembling the tank was simple enough but at last a problem arose. One of the tracks was too small to fit and the extra track they supplied didn't have enough pins to make it work. I contacted them and they should be fixing my track issue. I had enough track to just make it fit on the one side.
Tank Robot
I found a copy of my LOGO Program so I could make the tank move about the floor under command.

Using the WX module I can bring it online and give it instructions to have it move about the floor.

I like this tank because it's big enough to mount thinks on it. Now where is that huskylens...

#define DEBUG

#include <stdbool.h>
#include <memory.h>
#include <stdio.h>
#include <string.h>
#include <propeller.h>
#include "json.h"
#include "smartpins.h"
#include "mdd3a.h"

void DoPosition(void *);
void Position_Wait(void);


void DoLogo(void);
bool DoParse(void);
int FindW(char *);
int FindT(char *);
void DoForward(void);
void DoBack(void);
void DoLeft(void);
void DoRight(void);
void DoRepeat(void);
void DoBye(void);
void DoSave(void);
void DoLoad(void);
void DoTo(void);
void DoSleep(void);
void DoRunTo(int);
bool Evaluate(void);
void DoMake(void);


#define M1A 20
#define M1B 21
#define M2A 23
#define M2B 22

#define H1A 36
#define H1B 9
#define H2A 8
#define H2B 37

#define SPEED 400
#define STEPS 10
#define DEGREE 830


int Stack[1024];
int M1, M2;
int m1, m2;


const char WORDS[] = "FORWARD   BACK      LEFT      RIGHT     REPEAT    "
                     "BYE       SAVE      LOAD      TO        WAIT      "
                     "MAKE      ";
char Buffer[4096];
char Commands[1024];
char Repeat[1024];
char Variables[10][11];
char Values[10][10];
char Word[11];
void *ToObject[25];
char To[128];
int Items;
int Pos;
int Left;
int Right;


int main(int argc, char** argv)
{
    Items = 0;
    To[0] = 0;
    ToObject[0] = NULL;

    cogstart(DoPosition, NULL, Stack, 50);

    _waitms(5000);


    json_init(Buffer);
    Left = 0;
    Right = 0;


    while (1)
    {
        printf("Left: %d, Right: %d\n", M1, M2);
        printf("ready>\n");
        gets(Commands);
        DoLogo();
    }
}

void DoLogo()
{
    int i;

    while (true)
    {
        if (!DoParse())
            return;

        i = FindT(Word);
        if (i >= 0)
            DoRunTo(i);

        i = FindW(Word);

        switch (i)
        {
        case 0:
            DoForward();
            break;
        case 1:
            DoBack();
            break;
        case 2:
            DoLeft();
            break;
        case 3:
            DoRight();
            break;
        case 4:
            DoRepeat();
            break;
        case 5:
            DoBye();
            break;
        case 6:
            DoSave();
            break;
        case 7:
            DoLoad();
            break;
        case 8:
            DoTo();
            break;
        case 9:
            DoWait();
            break;
        case 10:
            DoMake();
            break;

        default:
            printf("I don't know how to: %s\n", Word);
            break;
        }
        _waitms(500);
    }
}

bool DoParse()
{
    int Pos;
    int i;
    bool yes = false;

    Pos = 0;
    Word[0] = 0;
    while (Commands[Pos] == ' ')
        Pos++;
    i = 0;
    while (Commands[Pos] != 0)
    {
        if (Commands[Pos] == ' ')
            break;
        Word[i++] = toupper(Commands[Pos++]);
        Word[i] = 0;
        yes = true;
    }
    strcpy(Commands, &Commands[Pos]);
    return yes;
}

int FindW(char *f)
{
    char *w;
    int i;

    f = strcat(f, " ");
    w = strstr(WORDS, f);
    if (w == NULL)
        return -1;
    i = (w - WORDS) / 10;

    return i;
}

int FindT(char *f)
{
    char *w;
    int i;

    f = strcat(f, " ");
    w = strstr(To, f);
    if (w == NULL)
        return -1;
    i = (w - To) / 10;
    printf("To: %d\n", i);
    return i;
}

void DoForward()
{
    int i;
    int l, r;

    if (!DoParse())
        return;
    Evaluate();
    i = atoi(Word);
    i = i * STEPS; // inch

    l = M1 + i;
    r = M2 + i;
    M1 = l;
    M2 = r;
    Position_Wait();
}

void DoBack()
{
    int i;
    int l, r;

    if (!DoParse())
        return;
    Evaluate();
    i = atoi(Word);
    i = i * STEPS; // inches

    l = M1 - i;
    r = M2 - i;
    M1 = l;
    M2 = r;
    Position_Wait();
}

void DoLeft()
{
    int i;
    int l, r;

    if (!DoParse())
        return;
    Evaluate();
    i = atoi(Word);
    i = DEGREE * i / 360;

    l = M1 + i;
    r = M2 - i;
    M1 = l;
    M2 = r;
    Position_Wait();
}

void DoRight()
{
    int i;
    int l, r;

    if (!DoParse())
        return;
    Evaluate();
    i = atoi(Word);
    i = DEGREE * i / 360;

    l = M1 - i;
    r = M2 + i;
    M1 = l;
    M2 = r;
    Position_Wait();
}

void DoRepeat()
{
    int i;
    int j;
    char *w;

    if (!DoParse())
        return;
    Evaluate();
    i = atoi(Word);

    DoParse();
    if (Word[0] == '[')
    {
        w = strchr(Commands, ']');
        *w = ' ';
        strcpy(Buffer, w);
        *w = 0;
        strcpy(Repeat, Commands);
    }

    for (int t=0;t < i;t++)
    {
        strcpy(Commands, Repeat);
        DoLogo();
    }
    *w = ' ';
    strcpy(Commands, w);
}

void DoSave()
{
    int i;
    char save[25];

    if (!DoParse())
        return;
    json_init(Buffer);
    json_putStr("TO", To);
    json_putArray("Items");
    for (i=0;i<Items;i++)
        json_putStr("ITEMS", ToObject[i]);
    json_putArray(NULL);
}

void DoLoad()
{
    int i;
    char *x;
    char *t;

    if (!DoParse())
        return;
    Items = 0;
    json_init(Buffer);
    x = json_find("TO");
    strcpy(To, x);
    x = json_find("ITEMS");
    while (x != NULL)
    {
        i = strlen(x);
        t = malloc(i+1);
        strcpy(t, x);
        ToObject[Items++] = t;
    }
}

void DoBye()
{
    mdd3a_Stop();
    printf("Done\n");
    while (1)
    {
        _waitms(1000);
    }
}

void DoTo()
{
    int i;
    bool done;
    char *func;

    if (!DoParse())
        return;
    strcpy(Buffer, Word);
    strcat(Buffer, "          ");
    Buffer[10] = 0;
    strcat(To, Buffer);

    Buffer[0] = 0;
    while (DoParse())
    {
        strcat(Buffer, Word);
        strcat(Buffer, " ");
    }

    done = false;
    while (!done)
    {
        printf(">");
        gets(Commands);
        if (strlen(Commands) == 0)
        {
            i = strlen(Buffer);
            func = malloc(i+1);
            strcpy(func, Buffer);
            ToObject[Items++] = func;
            done = true;
        }
        strcat(Buffer, Commands);
        strcat(Buffer, " ");
    }
    printf(Buffer);
}

void DoWait()
{
    int i;

    DoParse();
    if (Evaluate())
        i = atoi(Word);
    else
        i = 1;

    _waitms(i * 1000);
}

void DoRunTo(int i)
{
    int p;

    strcpy(Buffer, ToObject[i]);
    i = 0;
    while (Buffer[i] == ' ')
        i++;
    while (Buffer[i] == ':')
    {
        p = 0;
        i++;
        while (Buffer[i] != 0)
        {
            if (Buffer[i] == ' ')
                break;
            Word[p++] = Buffer[i++];
            Word[p] = 0;
        }
        strcpy(Variables[Items], Word);
        DoParse();
        strcpy(Values[Items++], Word);
        while (Buffer[i] == ' ')
            i++;
    }
    strcpy(Buffer, &Buffer[i]);
    strcat(Buffer, Commands);
    strcpy(Commands, Buffer);
    DoParse();
}

bool Evaluate()
{
    if (Word[0] == ':')
    {
        for (int i=0;i<Items;i++)
        {
            if (strcmp(&Word[1], Variables[i]) == 0)
            {
                strcpy(Word, Values[i]);
                return true;
            }
        }
    }
    return false;
}

void DoMake()
{
    if (!DoParse())
        return;

    if (Word[0] == ':')
    {
        for (int i=0;i<Items;i++)
        {
            if (strcmp(&Word[1], Variables[i]) == 0)
            {
                DoParse();
                strcpy(Values[i], Word);
                return;
            }
        }
        strcpy(Variables[Items], &Word[1]);
        DoParse();
        strcpy(Values[Items++], Word);
    }
}

void DoPosition(void *par)
{
    int s1, s2;

    _pinstart(H1A, P_PLUS1_B | P_QUADRATURE, 0, 0);
    _pinstart(H2A, P_PLUS1_B | P_QUADRATURE, 0, 0);

    mdd3a_Init(M1A, M1B, M2A, M2B);

    while (1)
    {
        m1 = -(_rdpin(H1A) >> 3);
        m2 = _rdpin(H2A) >> 3;

        s1 = 0;
        s2 = 0;

        if (M1 > m1)
            s1 = SPEED;
        if (M1 < m1)
            s1 = -SPEED;
        if (M2 > m2)
            s2 = SPEED;
        if (M2 < m2)
            s2 = -SPEED;

        mdd3a_SetPWM(1500 - s1, 1500 + s2);

    }
}

void Position_Wait()
{
    while ((M1 != m1) || (M2 != m2))
        _waitms(100);
}

Mike