View unanswered posts | View active topics It is currently Fri Nov 28, 2014 3:10 am






Reply to topic  [ 3 posts ] 
dead reckoning program - can you help me, please? 
Author Message
Rookie

Joined: Sun Nov 06, 2011 2:03 pm
Posts: 2
Post dead reckoning program - can you help me, please?
Hi, i'm trying to run with ROBOC 3.51 a very interesting program of dead reckoning published by Mikep on his blog [url][/url]http://www.spiked3.com/?p=410#comment-185[url][/url]:

Code:
#pragma config(Sensor, S2, COMPASS, sensorI2CCustom9V)
#pragma config(Motor, motorB, LEFT, tmotorNormal, PIDControl, encoder)
#pragma config(Motor, motorC, RIGHT, tmotorNormal, PIDControl, encoder)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//

// Xander's driver suite
#include "hitechnic-compass.h"

//**********************************************************//
//* *//
//* PoseNXC - LEGO NXT Robotics with Pose *//
//* *//
//**********************************************************//

#define __POSE_DEBUG__
#include "pose.h"

// shorthand for the typing lazy
intrinsic void _T(const string sFormatString, ...)
asm(opcdStringOps, strOpDebugStreamFormat, variableRefString(sFormatString), varArgList);

intrinsic void _TL(const string sFormatString, ...)
asm(opcdStringOps, strOpDebugStreamFormatWithNewLine, variableRefString(sFormatString), varArgList);

task DisplayPose()
{
while (true)
{
nxtDisplayTextLine(3, "# Pose@ %d", PoseClock);
nxtDisplayTextLine(4, "#-x,y %1.1f, %1.1f", PoseX, PoseY);
nxtDisplayTextLine(5, "#-hdg %d", PoseHdg);
nxtDisplayTextLine(6, "#-spd %1.1f", PoseVelocity);
wait1Msec(500);
}
}

void ShowPose(string t)
{
#ifdef _DEBUG
_TL("-%s", t);
_TL("--x,y %1.1f, %1.1f", PoseX, PoseY);
_TL("--hdg %d, %d", PoseHdg, HTMCreadRelativeHeading(COMPASS));
#endif
}

//**********************************************************//
//* *//
//* Main task *//
//* *//
//**********************************************************//

task main()
{
eraseDisplay();
nxtDisplayCenteredBigTextLine(0, "PoseNXT1");
_TL("");
_TL("***PoseNXT1");

time1[T1] = 0;

// should help to actually measure the tire, LEGO specs it at 81.6
// (float wheelDiameter, float wheelBase, float finalGearRatio, tMotor left, tMotor right, TSynchedMotors useForSynch, int movePower, int turnPower, int updateRate = 5)
PoseStartTask(81.6, 120.0, (float)(16/16), LEFT, RIGHT, synchBC, 50, 30);

StartTask(DisplayPose);
HTMCsetTarget(COMPASS);
PlaySound(soundBeepBeep);

// PilotMove(250.0);
// wait1Msec(500);
// PilotMove(-250.0);

// a lap done with moves and turns
for (int i = 0; i < 4; i++)
{
ShowPose("*move");
PilotMove(200.0);
PilotWaitDrivesIdle();
ShowPose("#move");

// simple
// PilotTurn(90);

// using corrected Turn
// PilotTurn(PilotQuickestTurnTo(PoseHdg, (i+1)*90));

// using TurnTo (which will internally correct)
// correct PoseHdg before we turn if compass available (will work with or without)
PoseHdg = PilotHdg360(HTMCreadRelativeHeading(COMPASS));
PilotTurnTo((i+1)*90);

wait1Msec(50); // give it as little time to update Pose
ShowPose("#turn");
}

// another lap done with GoTo
// PilotGoTo(0.0, 200.0);
// PilotGoTo(200.0, 200.0);
// PilotGoTo(200.0, 0.0);
// PilotGoTo(0.0, 0.0);
// PilotTurnTo(0);

PlaySound(soundBeepBeep);

bFloatDuringInactiveMotorPWM = true;
motor[LEFT] = 0;
motor[RIGHT] = 0;

ShowPose("final");
StopTask(PoseUpdate);

while (bSoundActive)
wait1Msec(100);

_TL("###PoseNXT");

while(true)
EndTimeSlice(); // use dark grey button to exit
}

The program was written for a previous version of ROBOTC and mikep kindly modified the library pose.h in this way to work under 3.5 version:

/changes
// 9/12/12 - update to work with robotc 3.5

bool _hasBeenInitialized = false;

// geometry info

float _wheelDiameter; // in some global unit
float _finalGearRatio; // generally spur / pinion
float _wheelBase;
int _PoseUpdateRate; // times per second
int _movePower, _turnPower;
tMotor _left, _right;
TSynchedMotors _useForSynch;

float _wheelTravelPerRevolution;
int _ticksPerWheelRevolution;
float _wheelBaseDiameter;
int _ticksForFullPilotTurn;

// Global Pose Variables
long PoseClock = 0; /*!< time (in ticks) the last update occurred */
short PoseHdg = 0; /*!< current heading in degrees */
float PoseX = 0.0, /*!< current X location */
PoseY = 0.0, /*!< current Y location */
PoseVelocity; /*!< current velocity */

task PoseUpdate();

/*!
* Initialize geomtery variables, Performs one time calculations, and starts the task PoseUpdate().
* All measurements must be in the same units, angles are in degrees. The example uses MM.
* @param wheelDiameter wheel diameter
* @param wheelBase wheel base
* @param finalGearRatio usually calculated as spur (the gear on the wheel) / pinion (the gear on the motor)
* @param left the motor that is driving the left wheel
* @param right the motor that is driving the right wheel
* @param useForSynch the syncMode to use for robotc, typically synchBC
* @param movePower the power to apply for a straight line move
* @param turnPower the power to apply when rotating to a new heading
* @param updateRate times per second the pose should be updated, the default is 5
*/
void PoseStartTask(float wheelDiameter, float wheelBase, float finalGearRatio, tMotor left, tMotor right, TSynchedMotors useForSynch, int movePower, int turnPower, int updateRate = 5)
{
_wheelDiameter = wheelDiameter;
_wheelBase = wheelBase;
_finalGearRatio = finalGearRatio;
_useForSynch = useForSynch;
_movePower = movePower;
_turnPower = turnPower;
_PoseUpdateRate = updateRate; // times per second
//_CompassInterval = compassInterval;
_left = left;
_right = right;

// one time calculated values
_wheelTravelPerRevolution = _wheelDiameter * PI;
_ticksPerWheelRevolution = 360 * _finalGearRatio;
_wheelBaseDiameter = _wheelBase * PI;
_ticksForFullPilotTurn = _wheelBaseDiameter * _ticksPerWheelRevolution / _wheelTravelPerRevolution;

#ifdef __POSE_DEBUG__
writeDebugStreamLine("PoseStartTask");
writeDebugStreamLine("wd %1.1f", _wheelDiameter);
writeDebugStreamLine("wb %1.1f", _wheelBase);
writeDebugStreamLine("fgr %1.1f", _finalGearRatio);

writeDebugStreamLine("wtpr %1.1f", _wheelTravelPerRevolution);
writeDebugStreamLine("tpwr %d", _ticksPerWheelRevolution);
writeDebugStreamLine("wbd %1.1f", _wheelBaseDiameter);
writeDebugStreamLine("tfft %d", _ticksForFullPilotTurn);
#endif

_hasBeenInitialized = true;
StartTask(PoseUpdate);
}

void PoseException()
{
hogCPU();
eraseDisplay();
PlaySound(soundException);
nxtDisplayCenteredTextLine(0, "Missing call to");
nxtDisplayCenteredTextLine(1, "PoseStartTask");
nxtDisplayCenteredTextLine(2, "Program aborted");
writeDebugStreamLine("Missing call to");
writeDebugStreamLine("PoseStartTask");
writeDebugStreamLine("Program aborted");
wait1Msec(10000);
StopAllTasks();
}

/*!
* This task is called by the NXT OS and will update the global pose variables to their current value.
* PoseStartTask() must be called before this task is started, and is automatically initially started by it.
* This task may be stopped and restarted by the user, and it will re-initialize Pose information.
*/
task PoseUpdate()
{
long lastEncoderLeft = 0;
long lastEncoderRight = 0;
long nowEncoderLeft;
long nowEncoderRight;
float leftDistance;
float rightDistance;
float poseTheta = 0.0;
float distance;
float xChange, yChange;
long deltaLeft, deltaRight;

if (!_hasBeenInitialized)
PoseException();

nSchedulePriority = 10; // we are pretty important

PoseX = 0.0;
PoseY = 0.0;
PoseHdg = 0;
PoseVelocity = 0.0;
nSyncedMotors = synchNone; // so we can reset both encoders
nMotorEncoder[_left] = 0;
nMotorEncoder[_right] = 0;
nSyncedMotors = _useForSynch; // back to 'normal' mode

while (true)
{
long elapsedTime;

hogCPU();
long nowTime = time1[T1];
elapsedTime = nowTime - PoseClock;
PoseClock = nowTime;

nowEncoderLeft = nMotorEncoder[_left];
nowEncoderRight = nMotorEncoder[_right];

deltaLeft = nowEncoderLeft - lastEncoderLeft;
deltaRight = nowEncoderRight - lastEncoderRight;

leftDistance = deltaLeft * _wheelTravelPerRevolution / (float)_ticksPerWheelRevolution;
rightDistance = deltaRight * _wheelTravelPerRevolution / (float)_ticksPerWheelRevolution;

distance = (leftDistance + rightDistance) / 2.0;
PoseVelocity = distance / elapsedTime / 1000;

//float poseTheta1 = MSIMUreadGyroZAxis(AIMU)/ 100.0; // using IMU gyro in radians
float poseTheta2 = ((leftDistance - rightDistance ) / _wheelBase); // using odemetry, in radians
poseTheta += poseTheta2;

// these (trig functions) are reversed from normal thinking,
// compass direction of 0 indicates North = a movement up the Y axis
xChange = distance * sin(poseTheta);
yChange = distance * cos(poseTheta);

PoseX += xChange;
PoseY += yChange;
PoseHdg = poseTheta * 180.0 / PI; // in degrees

lastEncoderLeft = nowEncoderLeft;
lastEncoderRight = nowEncoderRight;

releaseCPU();
wait1Msec(1000 / _PoseUpdateRate);
}
}

void PilotChangePower(int movePower, int turnPower)
{
_movePower = movePower;
_turnPower = turnPower;
}

void PilotChangePower(int movePower)
{
_movePower = movePower;
}

int PilotHdg360(int degrees)
{
while (degrees < 0)
degrees += 360;
return degrees % 360;
}

int PilotHdg180(int degrees)
{
degrees = PilotHdg360(degrees);
return degrees > 180 ? degrees - 360: degrees;
}

int PilotQuickestTurnTo(int hdgNow, int hdgNew)
{
hdgNow = PilotHdg360(hdgNow);
hdgNew = PilotHdg360(hdgNew);
if (hdgNow < hdgNew)
hdgNow += 360;
int left = hdgNow - hdgNew;
return (left < 181 ? -left : 360 - left);
}

void PilotWaitDrivesIdle()
{
if (!_hasBeenInitialized)
PoseException();

wait1Msec(20);
bool done = false;
while(!done)
{
done = (nMotorRunState[_left] == runStateIdle && nMotorRunState[_right] == runStateIdle);
EndTimeSlice();
}
wait1Msec(20);
}

void PilotMoveAsync(float distance)
{
long ticks;
if (!_hasBeenInitialized)
PoseException();

ticks = abs(distance * (float)_ticksPerWheelRevolution / _wheelTravelPerRevolution);

#ifdef __POSE_DEBUG__
writeDebugStreamLine("PilotMove:ticks %d", ticks);
#endif

nSyncedTurnRatio = 100;
nSyncedMotors = _useForSynch;
nMotorEncoderTarget[_left] = ticks;
motor[_left] = _movePower * sgn(distance);
}

void PilotTurnAsync(int degrees)
{
// this function is sensitive to which motor pairs we are using and in their order
// degrees may be flipped for BC or BA

long delta;

if (!_hasBeenInitialized)
PoseException();

// test this before uncommenting
// if (_useForSynch == synchBA || _useForSynch == synchCA || _useForSynch == synchCB)
// degrees = -degrees; ??

delta = (degrees * _ticksForFullPilotTurn / 360.0);

#ifdef __POSE_DEBUG__
writeDebugStreamLine("PilotTurn:%d", degrees);
writeDebugStreamLine(" LRe %d %d", nMotorEncoder[_left], nMotorEncoder[_right]);
writeDebugStreamLine(" ticks %d", delta);
#endif

nSyncedMotors = _useForSynch;
nSyncedTurnRatio = -100; // always turn opposite

nMotorEncoderTarget[_left] = abs(delta);
motor[_left] = _turnPower * sgn(delta);
}

void PilotTurnToAsync(int heading)
{
if (!_hasBeenInitialized)
PoseException();
PilotTurnAsync(PilotQuickestTurnTo(PoseHdg, heading));
}

void PilotGoToAsync(float x, float y)
{
float distance, hdgRadians;
int hdg;

distance = sqrt( (PoseX - x) * (PoseX - x) + (PoseY - y) * (PoseY - y) );
hdgRadians = atan2((y - PoseY), (x - PoseX)); // relative to 0,0
hdg = (int)(hdgRadians * 180.0 / PI);

#ifdef __POSE_DEBUG__
writeDebugStreamLine("GoTo: %1.1f, %1.1f", x, y);
writeDebugStreamLine(" frm: %1.1f, %1.1f", PoseX, PoseY);
writeDebugStreamLine(" hdgR: %1.1f", hdgRadians);
writeDebugStreamLine("dst hdg: %1.1f, %d", distance, hdg);
#endif

PilotTurnToAsync(hdg);
PilotWaitDrivesIdle(); //go to wait before we start moving
PilotMoveAsync(distance);
}

void PilotMove(float distance)
{
PilotMoveAsync(distance);
PilotWaitDrivesIdle();
}

void PilotTurn(int degrees)
{
PilotTurnAsync(degrees);
PilotWaitDrivesIdle();
}

void PilotTurnTo(int heading)
{
PilotTurnToAsync(heading);
PilotWaitDrivesIdle();
}

void PilotGoTo(float x, float y)
{
PilotGoToAsync(x,y);
PilotWaitDrivesIdle();
}



I tryed to run the program but i received these errors:
Code:
**Error**:Undefined variable 'strOpDebugStreamFormat'. 'short' assumed.
**Error**:Undefined variable 'varArgList'. 'short' assumed.
**Error**:Undefined variable 'strOpDebugStreamFormatWithNewLine'. 'short' assumed.
**Error**:Calling procedure 'ShowPose'. 'const' expressions does not fit. Parameter: 'string & t'. Expression: '"*move"'. Type: 'char *'
**Error**:Calling procedure 'ShowPose'. 'const' expressions does not fit. Parameter: 'string & t'. Expression: '"#move"'. Type: 'char *'
**Error**:Calling procedure 'ShowPose'. 'const' expressions does not fit. Parameter: 'string & t'. Expression: '"#turn"'. Type: 'char *'
**Error**:Calling procedure 'ShowPose'. 'const' expressions does not fit. Parameter: 'string & t'. Expression: '"final"'. Type: 'char *'

Can you help me to fix the problem?
I'd like to test my robot!!

Thank you in advance
flatino


Sat Oct 06, 2012 2:11 am
Profile
Expert

Joined: Tue Feb 28, 2012 3:10 pm
Posts: 195
Post Re: dead reckoning program - can you help me, please?
I made some changes to it today, and realized it is likely to change again in the next day or two for another build of robotC. I was waiting for the new version to settle in a bit before I updated the code on my website.

The errors you listed are very basic to fix though. Perhaps you could spend a little more time experimenting with RobotC and learn more about programming in general, it would be very helpful to you in the long run. Although I will say that the transition between old 'work-a-rounds' and the new can sometimes be confusing to those just starting right this moment. AnsiC, which robotC follows as closely as possible (most of the time), does not have a 'string' variable. It looks like the new RobotC prefers to use char *. So, to fix the last 4 errors, simply change the argument list on the function from 'string' to 'char *'.

Code:
void ShowPose(string t)  <- replace this with void ShowPose(char *t)
{
...
}



To get rid of the other errors, in the macro _TL and _T - use writeDebugStreamLine instead of asm(....), or wait a few days.

_________________
Mike aka Spiked3
http://www.spiked3.com


Sat Oct 06, 2012 4:53 am
Profile
Rookie

Joined: Sun Nov 06, 2011 2:03 pm
Posts: 2
Post Re: dead reckoning program - can you help me, please?
Tank you very much Mike!
I'm just starting to learn, so please forgive me for my brainless questions...
flatino


Sat Oct 06, 2012 5:09 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 3 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.