tnFlags __updateHeadingPosition(tnFlags __flags__, short timeout)
{
//calculate positions and distances in local coordinates
float degL =
(MotorTachoCount(__flags__.leftMotor) -
__flags__.tacL);
float degR =
(MotorTachoCount(__flags__.rightMotor) -
__flags__.tacR);
float distL = PI * __flags__.WheelDiameter * degL / 360.0;
float distR = PI * __flags__.WheelDiameter * degR / 360.0;
TextOut(0, LCD_LINE4, NumToStr(distL) + " " , DRAW_OPT_NORMAL);
TextOut(0, LCD_LINE5, NumToStr(distR) + " ", DRAW_OPT_NORMAL);
//determine the current wheel position in local coors
Vector2f TCur, TPrev, L0, L1, R0, R1, C0;
L0 = v2New(__flags__.WheelBaseWidth / -2.0, 0);
R0 = v2New(__flags__.WheelBaseWidth / 2.0, 0);
L1 = v2New(L0.X, distL);
R1 = v2New(R0.X, distR);
C0 = __flags__.position;
//theta is in RADIANS
float theta = -(distL - distR) / __flags__.WheelBaseWidth;
if((theta < PI / 18.0)//2.5 degrees
{
Vector2f C1 = v2Midpoint(L1,R1);
__flags__.position += v2RotateAbout(
v2Zero, C1, 90 * theta / PI);
__flags__.heading += 180 * theta / PI;
__flags__.tacL = MotorTachoCount(__flags__.leftMotor);
__flags__.tacR = MotorTachoCount(__flags__.rightMotor);
return __flags__;
}
int main()
{
int i;
printf("hello world\n");
printf("start of out_test\n");
Wait(SEC_1);
// initialize
if (!OutputInit())
printf("output init returned false\n");
ResetAllTachoCounts(OUT_ABCD);
// OutputSetType(OUT_A, DEVICE_TYPE_TACHO);
// OutputSetType(OUT_B, DEVICE_TYPE_TACHO);
// OutputSetType(OUT_C, DEVICE_TYPE_MINITACHO);
// OutputSetTypes(DEVICE_TYPE_TACHO, DEVICE_TYPE_TACHO, DEVICE_TYPE_TACHO, DEVICE_TYPE_TACHO);
SetPower(OUT_A, 90);
SetSpeed(OUT_B, 40);
SetPower(OUT_C, 60);
SetPower(OUT_D, -60);
On(OUT_ALL);
bool isBusy = false;
for(i=0; i < 10; i++)
{
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("OUT_C: %d %d %d\n", MotorRotationCount(OUT_C), MotorTachoCount(OUT_C), MotorActualSpeed(OUT_C));
printf("OUT_D: %d %d %d\n", MotorRotationCount(OUT_D), MotorTachoCount(OUT_D), MotorActualSpeed(OUT_D));
Wait(SEC_1);
isBusy = MotorBusy(OUT_ALL);
printf("Any output isBusy = %d\n", (byte)isBusy);
}
// Wait(SEC_5);
printf("Float(OUT_ALL)\n");
Float(OUT_ALL);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("ResetAllTachoCounts(OUT_ALL)\n");
ResetAllTachoCounts(OUT_ALL);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("OUT_C: %d %d %d\n", MotorRotationCount(OUT_C), MotorTachoCount(OUT_C), MotorActualSpeed(OUT_C));
printf("OUT_D: %d %d %d\n", MotorRotationCount(OUT_D), MotorTachoCount(OUT_D), MotorActualSpeed(OUT_D));
printf("Wait(SEC_1)\n");
Wait(SEC_1);
printf("RotateMotorNoWait(OUT_AB, 75, 7200)\n");
RotateMotorNoWait(OUT_AB, 75, 7200);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
isBusy = MotorBusy(OUT_AB);
printf("A or B isBusy = %d\n", (byte)isBusy);
printf("Wait(SEC_20)\n");
Wait(SEC_20);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("resetting all tacho counters\n");
ResetAllTachoCounts(OUT_ALL);
printf("Wait(SEC_1)\n");
Wait(SEC_1);
printf("OnForSync(OUT_AB, SEC_10, 75)\n");
OnForSync(OUT_AB, SEC_10, 75);
for(i=0; i < 10; i++)
{
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
isBusy = MotorBusy(OUT_AB);
printf("A or B isBusy = %d\n", (byte)isBusy);
isBusy = MotorBusy(OUT_CD);
printf("C or D isBusy = %d\n", (byte)isBusy);
Wait(SEC_1);
}
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
// synchronized motor movement without tacho or time limitation
printf("OnFwdSync(OUT_AB, 75)\n");
OnFwdSync(OUT_AB, 75);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("Off(OUT_AB)\n");
Off(OUT_AB); // stop with braking
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("Wait(SEC_1)\n");
Wait(SEC_1);
/*
* Turn ratio is how tight you turn and to what direction you turn
* - 0 value is moving straight forward
* - Negative values turns to the left
* - Positive values turns to the right
* - Value -100 stops the left motor
* - Value +100 stops the right motor
* - Values less than -100 makes the left motor run the opposite
* direction of the right motor (Spin)
* - Values greater than +100 makes the right motor run the opposite
* direction of the left motor (Spin)
*/
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("OnFwdSyncEx(OUT_AB, 75, -20, RESET_NONE)\n");
OnFwdSyncEx(OUT_AB, 75, -20, RESET_NONE);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("OnFwdSync(OUT_AB, 50, -50, RESET_NONE)");
OnFwdSyncEx(OUT_AB, 50, -50, RESET_NONE);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("OnFwdSync(OUT_AB, 20, -100, RESET_NONE)\n");
OnFwdSyncEx(OUT_AB, 20, -100, RESET_NONE);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("OnFwdSync(OUT_AB, 80, -150, RESET_NONE)\n");
OnFwdSyncEx(OUT_AB, 80, -150, RESET_NONE);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("OnFwdSync(OUT_AB, 30, -200, RESET_NONE)\n");
OnFwdSyncEx(OUT_AB, 30, -200, RESET_NONE);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("Off(OUT_AB)\n");
Off(OUT_AB);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
printf("ResetAllTachoCounts(OUT_AB)\n");
ResetAllTachoCounts(OUT_AB);
printf("Wait(SEC_2)\n");
Wait(SEC_2);
printf("OUT_A: %d %d %d\n", MotorRotationCount(OUT_A), MotorTachoCount(OUT_A), MotorActualSpeed(OUT_A));
printf("OUT_B: %d %d %d\n", MotorRotationCount(OUT_B), MotorTachoCount(OUT_B), MotorActualSpeed(OUT_B));
// a blocking version of RotateMotor (function call does not return
// until the motor rotation is complete)
printf("RotateMotor(OUT_AB, 75, 1800)");
RotateMotor(OUT_AB, 75, 1800); // 5 rotations
// by the time the function above returns the motors are no longer busy
isBusy = MotorBusy(OUT_AB);
printf("A or B isBusy = %d\n", isBusy);
// this call starts the motors running Forward without regulation or
// synchronization and the function call does not return until the
// specified amount of time has elapsed.
// at the end of the elapsed time the motors are stopped with braking.
printf("OnFor(OUT_AB, SEC_3)\n");
OnFor(OUT_AB, SEC_3);
printf("Off(OUT_AB)\n");
Off(OUT_AB);
printf("Wait(SEC_5)\n");
Wait(SEC_5);
OutputClose();
OutputExit();
printf("end of out_test\n");
return 0;
}
TCur = l2Intercept(l2NewPtPt(L0,R0), l2NewPtPt(L1,R1));
TPrev = v2New(0,100);
float radL, radR;
radL = distL * __flags__.WheelBaseWidth / (distL - distR);
radR = distR * __flags__.WheelBaseWidth / (distL - distR);
//take the last itteration's end points as final
//and update our flags
Vector2f C1 = v2Midpoint(L1,R1);
__flags__.position += v2RotateAbout(v2Zero,
C1, __flags__.heading);
__flags__.heading += v2AngleBetween(
v2Dif(L1,R1), v2Dif(L0,R0));
__flags__.tacL = MotorTachoCount(__flags__.leftMotor);
__flags__.tacR = MotorTachoCount(__flags__.rightMotor);
return __flags__;
}
task NavigateByTachometer()
{
tnFlags flags = tnFlagsReadOnly;
ResetAllTachoCounts(flags.leftMotor);
ResetAllTachoCounts(flags.rightMotor);
unsigned int time;
unsigned int delta;
while(true)
{
TextOut(0, flags.posLine,
"P: " + v2AsString(flags.position) +
tnFlags __updateHeadingPosition(tnFlags __flags__, short timeout)
{
//calculate positions and distances in local coordinates
float degL =
(MotorTachoCount(__flags__.leftMotor) -
__flags__.tacL);
float degR =
(MotorTachoCount(__flags__.rightMotor) -
__flags__.tacR);
float distL = PI * __flags__.WheelDiameter * degL / 360.0;
float distR = PI * __flags__.WheelDiameter * degR / 360.0;
TextOut(0, LCD_LINE4, NumToStr(distL) + " " , DRAW_OPT_NORMAL);
TextOut(0, LCD_LINE5, NumToStr(distR) + " ", DRAW_OPT_NORMAL);
//determine the current wheel position in local coors
Vector2f TCur, TPrev, L0, L1, R0, R1, C0;
L0 = v2New(__flags__.WheelBaseWidth / -2.0, 0);
R0 = v2New(__flags__.WheelBaseWidth / 2.0, 0);
L1 = v2New(L0.X, distL);
R1 = v2New(R0.X, distR);
C0 = __flags__.position;
//In the special case where wheels barely move,
//or the wheels move in sync so there is minimal turning,
//we're simply going to add the distance and convert back to globals
bool leftNearZero = abs(degL) <= 2.0;
bool rightNearZero = abs(degR) <= 2.0;
bool isTurning = (distL - distR) > (distL + distR) / 256.0;
if((leftNearZero && rightNearZero) || !isTurning)
{
Vector2f C1 = v2Midpoint(L1,R1);
__flags__.heading += v2AngleBetween(
v2Dif(L1,R1),
v2Dif(L0,R0));
__flags__.position += v2RotateAbout(
v2Zero, C1,__flags__.heading);
__flags__.tacL = MotorTachoCount(__flags__.leftMotor);
__flags__.tacR = MotorTachoCount(__flags__.rightMotor);
return __flags__;
}
//special case where one wheel is stationary
//we only need one itteration, with the stationary wheel as pivot
if(leftNearZero || rightNearZero)
{
if(leftNearZero)
{
TCur = L0;
L1 = L0;
float RadR = v2Dist(R0, TCur);
R1 = v2Mult(v2FromAngle(180.0 * distR / (PI * RadR)), RadR);
if(TCur.X >= R0.X)
R1 = v2ReflectY(R1);
R1 = v2Dif(TCur, R1);
}
if(rightNearZero)
{
TCur = R0;
R1 = R0;
float RadL = v2Dist(L0, TCur);
L1 = v2Mult(v2FromAngle(180.0 * distL / (PI * RadL)), RadL);
if(TCur.X >= L0.X)
L1 = v2ReflectY(L1);
L1 = v2Dif(TCur, L1);
}
Vector2f C1 = v2Midpoint(L1,R1);
__flags__.position += v2RotateAbout(v2Zero,
C1, __flags__.heading);
__flags__.heading += v2AngleBetween(
v2Dif(L1,R1),
v2Dif(L0,R0));
__flags__.tacL = MotorTachoCount(__flags__.leftMotor);
__flags__.tacR = MotorTachoCount(__flags__.rightMotor);
return __flags__;
}
//Primary case where both wheels move a different non-zero amount
//T is the turn pivot
TCur = l2Intercept(l2NewPtPt(L0,R0), l2NewPtPt(L1,R1));
TPrev = v2New(0,100);
//Itterate until our turn point is accurate to within a small margin
while(v2Dist(TCur,TPrev) > 1)
{
if(CurrentTick() > timeout)
{
__flags__.convergenceFailure = true;
return __flags__;
}
TPrev = TCur;
//circle radius to project distance onto
float RadL = v2Dist(L0, TCur);
float RadR = v2Dist(R0, TCur);
//new endpoints from distance over arc,
//rather than straight line
L1 = v2Mult(v2FromAngle(180.0 * distL / (PI * RadL)), RadL);
R1 = v2Mult(v2FromAngle(180.0 * distR / (PI * RadR)), RadR);
if(TCur.X >= L0.X)
L1 = v2ReflectY(L1);
if(TCur.X >= R0.X)
R1 = v2ReflectY(R1);
L1.X += TCur.X;
R1.X += TCur.X;
TCur = l2Intercept(l2NewPtPt(L0,R0), l2NewPtPt(L1,R1));
}
//take the last itteration's end points as final
//and update our flags
Vector2f C1 = v2Midpoint(L1,R1);
__flags__.position += v2RotateAbout(v2Zero,
C1, __flags__.heading);
__flags__.heading += v2AngleBetween(
v2Dif(L1,R1), v2Dif(L0,R0));
__flags__.tacL = MotorTachoCount(__flags__.leftMotor);
__flags__.tacR = MotorTachoCount(__flags__.rightMotor);
return __flags__;
}
