 | BrainStem Moto Features Example Last Modified: 2006-11-03 | | |
| Acroname Robotics | PDF webpage version  | ||
| | BrainStem Moto Features Example Last Modified: 2006-11-03 | | |
| Acroname Robotics | PDF webpage version | ||
| Introdution This example shows how to use several different Moto board features in a TEA program. The code was developed for Garcia, a robot that uses the Moto board and a prototype base that has two motors with built-in quadrature encoders. The program takes advantage of the free digital input and free analog input on the Moto board. A switch attached to the digital input selects either a ramp demo or wall-hug demo. A GP2D12 sensor on the analog input is used for wall-hugging. Encoders and Distance To measure distance, it is necessary to calculate the distance that corresponds to one encoder tick. Each motor on the robot has a 19:1 gearbox and a 16 counts-per-revolution quadrature encoder. There is a secondary 3:1 gear reduction stage between the motor output shaft and wheels. This yields a final gear reduction factor of 57. Due to the nature of the quadrature input, the encoders create a number of "ticks" per revolution that is 4 times the counts-per-revolution. One complete revolution of the wheel will create 3648 (16x4x57) encoder ticks. The wheels are 3.975" or 100.965mm in diameter. Using (pi * diameter) / (0.0869 ticks/mm) yields 0.0869mm per tick. To travel one foot, the robot must go 12x25.4mm. This is a total of 3508 ticks. A common move for a 2-wheel drive robot is to rotate in place by applying opposite speeds to the motors. In order to rotate a precise amount, some extra calculations are needed. The wheel base, or distance between the points where the two wheels touch the ground, is 177mm. When rotating, the wheels will follow a circular path with a diameter equal to the wheel base. So the number of ticks for a 360 degree rotation is (pi * 177) / (0.0869 ticks/mm) which is very close to 6400. A 90 degree rotation will be equivalent to 1600 ticks. The constants in the calculations above are specific to the Garcia robot. The methods apply to any 2-wheel drive robot with quadrature encoders. Ramp Demo In the ramp demo, the robot travels forward 4 feet, turns left 90 degrees, turns right 90 degrees, then travels backward 4 feet to end up back at its starting position. Each motion is performed using dual trapezoidal velocity ramps with synchronization and a final nudge maneuver for each wheel. A ramp reflex paces the series of motions. On a tile floor, the robot routinely returns to a point less than an inch from where it started. Wall Hug Demo In the wall-hug demo, the robot performs a right wall-hug while traveling at about 80% of its maximum speed. The robot uses velocity damping to smooth its response. Jitter in the sensor readings can cause large changes in the velocity and make the robot's motion too jerky. Source Code - Moto Configuration The first file is a batch file of commands. They are used to initialize and save all the motion control parameters for the two motors. The file is a series of cmdMO_CFG commands followed by a cmdMO_SAV command. These settings work well for both the ramp demo and wall-hug demo. If the robot only performed one of these tasks, different settings might work better. /* filename: motocfg.bag */
4 63 0 0 7 1 /* cmdMO_CFG, CH0, ENCVEL mode, invert PID input */
4 63 0 1 1 0 /* cmdMO_CFG, CH0, PID P term, value=1.0 */
4 63 0 2 0 0 /* cmdMO_CFG, CH0, PID I term, value=0.0 */
4 63 0 3 1 80 /* cmdMO_CFG, CH0, PID D term, value=10.5 */
4 63 0 4 0 0 /* cmdMO_CFG, CH0, COFFSET, value=0 */
4 63 0 5 127 255 /* cmdMO_CFG, CH0, PWMRAIL, value=32767 */
4 63 0 6 0 50 /* cmdMO_CFG, CH0, PERIOD, value=5usec */
4 63 0 7 0 0 /* cmdMO_CFG, CH0, LATENCY, value=0 */
4 63 0 8 0 255 /* cmdMO_CFG, CH0, PWMFREQ, 10-bit, 40kHz */
4 63 1 0 7 2 /* cmdMO_CFG, CH1, ENCVEL mode, invert motor output */
4 63 1 1 1 0 /* cmdMO_CFG, CH1, PID P term, value=1.0 */
4 63 1 2 0 0 /* cmdMO_CFG, CH1, PID I term, value=0.0 */
4 63 1 3 1 80 /* cmdMO_CFG, CH1, PID D term, value=10.5 */
4 63 1 4 0 0 /* cmdMO_CFG, CH1, COFFSET, value=0 */
4 63 1 5 127 255 /* cmdMO_CFG, CH1, PWMRAIL, value=32767 */
4 63 1 6 0 50 /* cmdMO_CFG, CH1, PERIOD, value=5usec */
4 63 1 7 0 0 /* cmdMO_CFG, CH1, LATENCY, value=0 */
4 63 1 8 0 255 /* cmdMO_CFG, CH1, PWMFREQ, 10-bit, 40kHz */
4 64 /* cmdMO_SAV */
Source Code - Reflex to Inidicate Demo Completion The next file is a leaf file that defines reflex 100. This reflex writes to the semaphore for process 0 and dumps a debug message. When performing the ramp demo, the TEA program configures motion channel 1 to trigger reflex 100 when it finishes ramp motions. The TEA program can launch a ramp motion and wait for its semaphore to be written. After the semaphore is written, the TEA program continues. This provides a convenient means for pacing the motions. /* filename: garcia.leaf */
#include <aCmd.tea>
#include <aMotoReflexes.tea>
/* Moto has address 4 */
#define MODULE 4
#define SEMAPHORE0 53
module[MODULE] {
/************* REFLEX COMMANDS *************/
message[0] {
MODULE, cmdDEV_VAL, SEMAPHORE0, 0
}
message[1] {
MODULE, cmdDEBUG, 1, 2
}
/************* REFLEX VECTORS *************/
vector[100] {
0,
1
}
}
Building the leaf file listed above with the leaf Console command produces the following bag file. // filename: garcia.bag
// module 4 (2 messages, 1 vectors)
// message 0
4 12 128 0 4 3 4 53 0
// message 1
4 12 128 1 4 3 23 1 2
// vector 100
4 11 128 100 0 128 129 128
Source Code - TEA Demonstration The final file is the TEA code for the two demos. Most of the code is configuration commands for built-in Moto board functions. The wall-hug code has a simple proportional control loop that calculates a steering correction to keep the robot a set distance from the wall. The aMotion routines are documented in the TEA section of the online BrainStem Reference . /* filename: garcia.tea */
/* code for Moto board testing platform */
#include <aCore.tea>
#include <aPrint.tea>
#include <aMulti.tea>
#include <aA2D.tea>
#include <aDig.tea>
#include <aMotion.tea>
/* Wheel diamteter = 100.965mm (3.975") */
/* Wheel base = 177mm */
/* Gearbox has 3648 ticks per revolution */
/* 100.965*pi/3648 yields 0.0869mm per tick */
/* 1 ft = 304.8mm, 304.8/0.0869=3508 ticks per foot */
/* pi*177/0.0869 yields 6400 ticks per rotation */
/* 1600 ticks for rotating 90 degrees */
#define NINETY 1600
#define FOURFT 14032
void initvd()
{
int nACCT=5;
aMotion_SetRampVel(0,0);
aMotion_SetRampVel(1,0);
aMotion_SetRampAccStepTime(0,nACCT);
aMotion_SetRampAccStepTime(1,nACCT);
aMotion_SetRampFlags(0,1);
aMotion_SetRampFlags(1,1);
aMotion_RampEnable(aMOTION_CHANNEL_ALL,1);
}
void inittr()
{
int nACCT=160;
aMotion_SetRampAccStepTime(0,nACCT);
aMotion_SetRampAccStepTime(1,nACCT);
aMotion_SetRampFlags(0,0x000A);
aMotion_SetRampFlags(1,0xE40A);
}
void move()
{
aMotion_RampEnable(aMOTION_CHANNEL_ALL,1);
aMulti_Wait();
aCore_Sleep(100);
}
void rotate(char x, int n, int v)
{
char x0;
char x1;
x0=x;
x1=1-x;
aMotion_SetRampVel(x0, v);
aMotion_SetRampTarget(x0, 0,n);
aMotion_SetRampVel(x1,-v);
aMotion_SetRampTarget(x1,-1,-n);
move();
}
void straight(int a, int b, int v)
{
aMotion_SetRampVelocity(v,v);
aMotion_SetRampTarget(0,a,b);
aMotion_SetRampTarget(1,a,b);
move();
}
void demo_hug()
{
int i,j;
int err;
int set=250;
initvd();
while(1)
{
j=aDig_ReadInt(0);
i=aA2D_ReadInt(4);
err=(set-i)/10;
aMotion_RampVelSteer(30,15,err);
if (j==0) break;
}
}
void demo_ramp()
{
inittr();
{
straight(0,FOURFT,15);
aCore_Sleep(15000);
rotate(1,NINETY,5);
aCore_Sleep(15000);
rotate(0,NINETY,5);
aCore_Sleep(15000);
straight(-1,-FOURFT,-15);
aCore_Sleep(15000);
}
}
void main()
{
int i;
i=aDig_ReadInt(0);
if (i==0)
{
demo_ramp();
}
else
{
demo_hug();
aMotion_SetRampVelocity(0,0);
}
}
Revision History:
| |||
| 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. |
