 | Controlling an RC Car Example Last Modified: 2006-11-08 | | |
| Acroname Robotics | PDF webpage version  | ||
| | Controlling an RC Car Example Last Modified: 2006-11-08 | | |
| Acroname Robotics | PDF webpage version | ||
|  Introduction In this example, a BrainStem controls the behaviors of Acroname's Squishy Robot that is made from a RC truck chasis. This is an early version of Squishy's code that demonstrates his motion control algorithm. His motion control routines can not fit in a single TEA program. A second TEA program stores several of his subroutines. This example shows how to make a single application that uses multiple TEA programs. It is not intended to be a detailed guide on how to build a robot.
Squishy always rolls in curves. He alternates curving left and right. As he rolls around, he uses a Devantech Compass to monitor his progress. If his motion is unobstructed, the compass heading will change noticeably as he turns. If the compass heading does not change very much, then it probably means Squishy hit an obstacle and should back up, turn, and go in some other direction. A new addition to Squishy is a pressure sensor on his back. It replaces a microswitch that was previously used as a start-stop button. With the microswitch, Squishy had to be picked up in order to get access to the button. The pressure sensor is much more convenient. Now when Squishy rolls around, he can be stopped by pushing on his back. He can be restarted with another push.  Insides of the Squishy robot where the controll, power and sensors are housed. Source Code - Overview and TEA Include File The programming uses some of the techniques from the One Program with Multiple TEA Files example. The program consists of three source files. The "squishydef.tea" is the "glue" that holds the two programs together. It defines common values for both programs and has wrapper functions that enable the squishy0.tea program to call subroutines in the squishy1.tea program. The squishy0.tea program is stored in TEA file slot 0. The squishy1.tea program is stored in TEA file slot 1. The BrainStem is configured to run squishy0.tea as its bootstrap program. This version of the code for Squishy uses the BrainStem libraries released with the Build 6 download and later. /* filename: squishydef.tea */
/* Squishy's defines and multi-tasking glue */
#include <aMulti.tea>
/* CFG LMT */
#define SRVSTEER 0 /* 0xCF, (0x1A,0x1F) */
#define SRVSPEED 1 /* 0xC0, (0x17,0x18) */
/* servo 0 is for steering */
/* servo 1 is motor controller */
#define DIGBEEP 4 /* digital output for beeper */
#define APAT 4 /* analog input for pat sensor */
#define NSTEERMIN 1 /* steering limits */
#define NSTEERCTR 128
#define NSTEERMAX 254
/* scooting parameters */
#define NSPDON 208
#define NSPDOFF 128
#define NSPDREV 48
#define NBRAKE 123
#define TMRAMP 55
#define SPDOFFTIME 2000
#define SCOOTPROC 1
#define SCOOTFILE 1
/* cscoot function wrapper */
int cscoot(int nspd, int nstr, int nbrk)
{
return aMulti_Run13(SCOOTFILE,SCOOTPROC,0,nspd,nstr,nbrk);
}
/* evade function wrapper */
int evade(int ndir)
{
return aMulti_Run13(SCOOTFILE,SCOOTPROC,1,ndir,0,0);
}
/* steer function wrapper */
int steer(int nstr)
{
return aMulti_Run13(SCOOTFILE,SCOOTPROC,2,nstr,0,0);
}
Source Code - TEA Main Calling Routine The "squishy0.tea" program contains the top-level logic that controls his rolling pattern and course corrections. /* filename: squishy0.tea */
/* Squishy's main program */
#include <aCore.tea>
#include <aServo.tea>
#include <aDig.tea>
#include <aA2D.tea>
#include "squishydef.tea"
void init()
{
aDig_Config(DIGBEEP,ADIG_OUTPUT);
aServo_SetLimits(SRVSTEER,0x1A1F);
aServo_SetLimits(SRVSPEED,0x1718);
aServo_SetConfig(SRVSTEER,(unsigned char)0xCF);
aServo_SetConfig(SRVSPEED,(unsigned char)0xC0);
}
/* emit several 50ms beeps */
void beep(int k, int ndur)
{
int i;
for (i=0; i<k; i++)
{
aDig_Write(DIGBEEP,1);
aCore_Sleep(ndur);
aDig_Write(DIGBEEP,0);
aCore_Sleep(ndur);
}
}
/* toggle a between values b and c (a must equal b or c) */
int toggle(int a, int b, int c)
{
if (a==b) return c;
return b;
}
/* loop to beep and check input switch */
/* (beep indicates robot is turned on) */
void poll_and_beep()
{
int i;
int nflag=1;
int nback=0;
while (nflag)
{
/* quit if back is pushed */
for (i = 0; i < 200; i++)
{
nback = aA2D_ReadInt(APAT);
if (nback>650)
{
nflag=0;
break;
}
}
beep(1,50);
}
}
void mainloop()
{
int i;
int r;
int nback=0;
int niter=0;
int ndir=0;
/* initialize direction and scoot count */
ndir = NSTEERMIN;
niter = 0;
while (1)
{
/* steer if scoot has been reset */
if (niter == 0)
{
steer(ndir);
aCore_Sleep(2000);
}
/* quit if back is pushed */
/* (loop also provides additional steering delay) */
for (i = 0; i < 25; i++)
{
nback = aA2D_ReadInt(APAT);
if (nback>800) return;
}
/* scoot and see if direction changed */
/* if direction hardly changed, back up and turn */
/* (perform evasive maneuver) */
/* then start scooting again */
r = cscoot(NSPDON,ndir,NBRAKE);
if (r < 2)
{
beep(1,200);
beep(40,20);
beep(60,40);
evade(ndir);
niter=0;
ndir=toggle(ndir,NSTEERMIN,NSTEERMAX);
}
else
{
/* change direction after a couple of scoots */
niter++;
if (niter==2)
{
niter=0;
ndir=toggle(ndir,NSTEERMIN,NSTEERMAX);
}
}
}
}
void main()
{
int i;
int r;
int nflag=1;
int nback=0;
int niter=0;
int ndir=0;
init();
while (nflag)
{
/* wait for back to be pushed */
poll_and_beep();
/* acknowledge start and pause for 1 second */
beep(5,500);
aCore_Sleep(10000);
/* scoot until back is pushed again */
mainloop();
/* buzz then start over */
beep(100,20);
}
}
Source Code - TEA Control Routines The "squishy1.tea" program contains the lower level routines that control his different motions. /* filename: squishy1.tea */
/* Squishy's control routines */
#include <aCore.tea>
#include <aServo.tea>
#include <aDig.tea>
#include "squishydef.tea"
/* return absolute value of an int */
int abs(int a)
{
int val;
if (a >= 0)
{
val = a;
}
else
{
val = -a;
}
return val;
}
/* read 1 byte from obsolete version of Devantech compass */
/* (not compatible with latest version of compass) */
int read_compass()
{
int val=0;
asm
{
/* byte 1 is dir (0-255) */
pushlb 0 /* pad byte to short */
pushlb (unsigned char)0xC1 /* IIC address */
pushlb 1 /* bytes to read */
popsm aPortIICRead
popss 2 /* store as int */
}
return val;
}
/* send positioning signal to steering servo */
void steer_abs(int npos)
{
asm
{
pushss 4
popsm aPortServo+SRVSTEER*aPortServoBlockSize+aOffsetServoAbsolute
}
}
/* send speed value to motor controller */
void speed_abs(int nspd)
{
asm
{
pushss 4
popsm aPortServo+SRVSPEED*aPortServoBlockSize+aOffsetServoAbsolute
}
}
/* perform a ramp acceleration */
/* (increases or decreases speed in timed increments) */
void acc(int nspd1, int nspd2, int tmramp)
{
int i;
if (nspd2>nspd1)
{
for (i=nspd1; i<nspd2; i++)
{
aCore_Sleep(tmramp);
speed_abs(i);
}
}
else
{
for (i=nspd1; i>=nspd2; i--)
{
aCore_Sleep(tmramp);
speed_abs(i);
}
}
}
/* apply reverse speed to stop quickly */
void brake(int nbrk)
{
acc(128,nbrk,(int)10);
acc(nbrk,128,(int)10);
}
/* IMPLEMENTATION OF EVADE FUNCTION */
/* do some three-point turns in an attempt */
/* to get around an obstacle */
int evade_internal(int ndir)
{
int i;
int tog;
aCore_Sleep(500);
steer_abs(NSTEERCTR);
aCore_Sleep(8000);
for (i = 0; i < 2; i++)
{
acc(NSPDOFF, NSPDREV, 10);
acc(NSPDREV, NSPDOFF, 10);
aCore_Sleep(500);
}
tog=ndir;
for (i = 0; i < 2; i++)
{
steer_abs(tog);
aCore_Sleep(8000);
acc(NSPDOFF, NSPDREV, 10);
acc(NSPDREV, NSPDOFF, 10);
tog=255-tog;
steer_abs(tog);
aCore_Sleep(8000);
acc(NSPDOFF, NSPDON, 10);
acc(NSPDON, NSPDOFF, 10);
tog=255-tog;
}
return 0;
}
/* IMPLEMENTATION OF CSCOOT FUNCTION */
/* roll in a curve and check compass */
/* no curve (a stall) returns small value */
/* right curve returns positive */
/* left curve returns negative */
int cscoot_internal(int nspd, int nstr, int nbrk)
{
int n1;
int n2;
int d;
/* take initial heading, drive for a bit, then brake */
n1 = read_compass();
acc(128, nspd, TMRAMP);
brake(nbrk);
aCore_Sleep(SPDOFFTIME);
/* check final heading and calculate direction change */
/* (must handle possible wrap-around with direction) */
n2 = read_compass();
d=(n2-n1);
if (d > 128)
{
d=d-256;
}
if (d < -128)
{
d=d+256;
}
d=abs(d);
return d;
}
/* IMPLEMENTATION OF STEER FUNCTION */
int steer_internal(int nstr)
{
/* send steering command */
steer_abs(nstr);
return 0;
}
/* CASE STATEMENT TO CHOOSE APPROPRIATE FUNCTION */
void main(char callingProc, char c1, int i1, int i2, int i3)
{
int retval=0;
switch (c1)
{
case 0: retval = cscoot_internal(i1,i2,i3); break;
case 1: retval = evade_internal(i1); break;
case 2: retval = steer_internal(i1); break;
}
aMulti_Halt(callingProc,retval);
}
Revision History:
| |||||||
Related Examples: Adding Multithreaded Behaviors to an RC Car based Robot Example | ||||||||
| voice: 720-564-0373, email: sales@acroname.com, address: 4822 Sterling Dr., Boulder CO, 80301-2350, privacy © Copyright 1994-2010 Acroname, Inc., Boulder, Colorado. All rights reserved. |
