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) +
" " , DRAW_OPT_NORMAL);
TextOut(0, flags.headLine, "H: " +
NumToStr(flags.heading) +
" " , DRAW_OPT_NORMAL);
time = CurrentTick();
tnFlags temp = __updateHeadingPosition(flags, time + 50);
if(temp.convergenceFailure)
PlayTone(2000, 100);
else
flags = temp;
delta = CurrentTick() - time;
tnFlagsReadOnly = flags;
if(delta < 25)
Wait(25 - delta);
}
}
//---------------------------------------------------------------------
// void CalcInterval(long cLoop)
//
// Calculate the interval time from one iteration of the loop to the next.
// Note that first time through, cLoop is 0, and has not gone through
// the body of the loop yet. Use it to save the start time.
// After the first iteration, take the average time and convert it to
// seconds for use as interval time.
inline void CalcInterval(long cLoop)
{
if (cLoop == 0) {
// First time through, set an initial tInterval time and
// record start time
tInterval = 0.0055;
tCalcStart = CurrentTick();
} else {
// Take average of number of times through the loop and
// use for interval time.
tInterval = (CurrentTick() - tCalcStart)/(cLoop*1000.0);
}
}
static int TryAcquireRead(
_Inout_ ReadWriteLock* self
)
{
volatile LockFields* lock = (LockFields*)self;
size_t oldState, state, swapState;
for(;;)
{
oldState = ReadLockState(lock);
state = oldState + 1;
/* Skipped when owners is the only active field
* and we did not try to add too many shared owners. */
if (state >= OWN_EXCLUSIVE)
{
/* Detect whether adding another shared owner is impossible. */
if (LockOwners(oldState) >= OWN_MAXSHARED)
return 0;
/* If writer == exit, no writers are waiting. */
if ((LockWriter(state) ^ LockExit(state)) != 0)
{
/* Writers are waiting. Acquiring would jump the queue. */
if (((CurrentTick() - LockUnfair(oldState)) & 14) != 0)
return 0;
}
}
/* Ready to take shared ownership. */
swapState = Atomic_CompareAndSwap(LockState(lock), oldState, state);
if (swapState == oldState)
return 1;
}
}
void ReadWriteLock_ReleaseWrite(
_Inout_ ReadWriteLock* self)
{
volatile LockFields* lock = (LockFields*)self;
size_t state, key;
state = Atomic_Add(LockState(lock), -OWN_EXCLUSIVE);
if (state != 0)
{
/* There is a queue.
* Threads may be blocked waiting for us to leave. */
key = (size_t)lock ^ LockExit(state);
CondLock_Broadcast(key);
//if (((LockEntry(state) - LockExit(state)) & (FIELD_SIZE - 1)) == 2 &&
if (LockEntry(state) - LockExit(state) >= 2 &&
((CurrentTick() - LockUnfair(state)) & 14) == 0)
{
/* Under certain conditions, encourage the last group of threads in
* line to stop spinning and acquire unfairly. */
if (LockEntry(state) == LockWriter(state))
key = (size_t)lock ^ (LockEntry(state) - 1);
else
key = (size_t)lock ^ LockWriter(state);
CondLock_BroadcastSpinners(key);
}
}
}
static int TryAcquireWrite(
_Inout_ ReadWriteLock* self
)
{
volatile LockFields* lock = (LockFields*)self;
size_t oldState, state, swapState;
for(;;)
{
oldState = ReadLockState(lock);
state = oldState | OWN_EXCLUSIVE;
/* Skipped when the lock is empty. */
if (oldState != 0)
{
/* Detect whether there are existing owners. */
if (LockOwners(oldState) != 0)
return 0;
/* Someone must be waiting. Acquiring would jump the queue. */
if (((CurrentTick() - LockUnfair(oldState)) & 14) != 0)
return 0;
}
/* Ready to take exclusive ownership. */
swapState = Atomic_CompareAndSwap(LockState(lock), oldState, state);
if (swapState == oldState)
return 1;
}
}
static void QueueAcquireWrite(
_Inout_ ReadWriteLock* self
)
{
volatile LockFields* lock = (LockFields*)self;
size_t oldState, state, swapState, preQueuedState;
size_t waitFor, key, spinState, spinCount;
for (;;)
{
oldState = ReadLockState(lock);
state = oldState;
/* If there is no queue, we are the first one to wait;
* allow unfairness for the current timer tick. */
if (state <= OWN_EXCLUSIVE)
LockUnfair(state) = CurrentTick();
/* Wait for the most recent thread to enter the queue. */
waitFor = LockEntry(state);
if (++LockEntry(state) == LockExit(state))
{
/* The queue arithmetic will wrap if we continue. */
Thread_Yield();
continue;
}
/* Make reader threads coming in wait for us. */
LockWriter(state) = LockEntry(state);
swapState = Atomic_CompareAndSwap(LockState(lock), oldState, state);
if (swapState == oldState)
break;
}
/* This thread now has a place in the queue.
* Threads behind us may be depending on us to wake them up. */
preQueuedState = oldState;
key = (size_t)lock ^ waitFor;
spinState = LockSpin(oldState);
for (;;)
{
/* Avoid write prefetching since we expect to wait. */
oldState = *(ptrdiff_t*)lock;
if (LockExit(oldState) != waitFor || LockOwners(oldState) != 0)
{
/* The thread ahead of us still hasn't acquired,
* or some reader or writer owns the lock right now. */
if (((CurrentTick() - LockUnfair(oldState)) & 14) == 0 &&
LockEntry(oldState) - LockExit(oldState) >= 2 &&
LockEntry(oldState) == LockEntry(preQueuedState) + 1 &&
LockOwners(oldState) == 0)
{
/* Under certain conditions, we can acquire immediately if we
* are the last thread in line and undo joining the queue. */
if (preQueuedState <= OWN_EXCLUSIVE)
state = OWN_EXCLUSIVE;
else
{
state = oldState + OWN_EXCLUSIVE;
LockEntry(state) = LockEntry(preQueuedState);
LockWriter(state) = LockWriter(preQueuedState);
}
/* Atomically de-queue and acquire unfairly. */
swapState = Atomic_CompareAndSwap(LockState(lock), oldState, state);
if (swapState == oldState)
return;
continue;
}
/* spinState being low means spinning usually works.
* Use a high count if it has been working recently. */
spinCount = (spinState & SPIN_SIGN) ?
CONDLOCK_LOW_SPINCOUNT :
CONDLOCK_HIGH_SPINCOUNT;
/* Spin and/or block until something changes.
* Adjust the spin field based on whether spinning worked. */
if (CondLock_Wait(key, (ptrdiff_t*)lock, oldState, spinCount))
spinState = (spinState > 2) ? (spinState - 2) : 0;
else
spinState = (spinState < SPIN_MAX) ? (spinState + 1) : spinState;
continue;
}
state = oldState + OWN_EXCLUSIVE;
/* Bump the exit ticket number. We're leaving the queue. */
LockExit(state)++;
/* Zero the top 4 fields if the queue is now empty. */
if (LockExit(state) == LockEntry(state))
state = LockOwners(state);
else
{
/* Not empty, but we just acquired fairly.
* Allow unfairness for a while. */
LockUnfair(state) = CurrentTick();
LockSpin(state) = spinState;
}
/* Ready to take exclusive ownership. */
swapState = Atomic_CompareAndSwap(LockState(lock), oldState, state);
if (swapState == oldState)
return;
}
}
static void QueueAcquireRead(
_Inout_ ReadWriteLock* self
)
{
volatile LockFields* lock = (LockFields*)self;
size_t oldState, state, swapState, preQueuedState;
size_t waitFor, diff, key, spinState, spinCount;
for (;;)
{
oldState = ReadLockState(lock);
state = oldState;
/* If there is no queue, we are the first one to wait;
* allow unfairness for the current timer tick. */
if (state <= OWN_EXCLUSIVE)
LockUnfair(state) = CurrentTick();
/* Insert a barrier every half revolution.
* This stops writer arithmetic from wrapping. */
if ((LockEntry(state) & ~FIELD_SIGN) == 0)
LockWriter(state) = LockEntry(state);
if (++LockEntry(state) == LockExit(state))
{
/* The queue arithmetic will wrap if we continue. */
Thread_Yield();
continue;
}
swapState = Atomic_CompareAndSwap(LockState(lock), oldState, state);
if (swapState == oldState)
break;
}
/* This thread now has a place in the queue.
* Threads behind us may be depending on us to wake them up. */
/* Wait for the most recent writer to enter the queue. */
waitFor = LockWriter(state);
key = (size_t)lock ^ waitFor;
preQueuedState = oldState;
spinState = LockSpin(oldState);
for (;;)
{
/* Avoid write prefetching since we expect to wait. */
oldState = *(ptrdiff_t*)lock;
diff = LockExit(oldState) - waitFor;
if ((diff & FIELD_SIGN) == 0)
{
/* The writer ahead of us in line already acquired.
* Someone could have beat us unfairly.
* Just wait for the current owner. */
waitFor = LockExit(oldState);
key = (size_t)lock ^ waitFor;
}
if ((diff & FIELD_SIGN) != 0 || (LockOwners(oldState) == OWN_EXCLUSIVE))
{
/* The writer ahead of us still hasn't acquired,
* or someone owns the lock exclusively right now. */
if (((CurrentTick() - LockUnfair(oldState)) & 14) == 0 &&
LockEntry(oldState) - LockExit(oldState) >= 2 &&
LockEntry(oldState) == LockEntry(preQueuedState) + 1 &&
(LockOwners(oldState) < OWN_MAXSHARED))
{
/* Under certain conditions, we can acquire immediately if we
* are the last thread in line and undo joining the queue. */
if (preQueuedState <= OWN_EXCLUSIVE)
state = LockOwners(oldState) + 1;
else
{
state = oldState + 1;
LockEntry(state) = LockEntry(preQueuedState);
LockWriter(state) = LockWriter(preQueuedState);
}
/* Atomically de-queue and acquire unfairly. */
swapState = Atomic_CompareAndSwap(LockState(lock), oldState, state);
if (swapState == oldState)
return;
continue;
}
/* spinState being low means spinning usually works.
* Use a high count if it has been working recently. */
spinCount = (spinState & SPIN_SIGN) ?
CONDLOCK_LOW_SPINCOUNT :
CONDLOCK_HIGH_SPINCOUNT;
/* Spin and/or block until something changes.
* Adjust the spin field based on whether spinning worked. */
if (CondLock_Wait(key, (ptrdiff_t*)lock, oldState, spinCount))
spinState = (spinState > 2) ? (spinState - 2) : 0;
else
spinState = (spinState < SPIN_MAX) ? (spinState + 1) : spinState;
continue;
}
if (LockOwners(oldState) == OWN_MAXSHARED)
{
/* The owner arithmetic will overflow if we continue. */
Thread_Yield();
continue;
}
state = oldState + 1;
/* Bump the exit ticket number. We're leaving the queue. */
LockExit(state)++;
/* Zero the top 4 fields if the queue is now empty. */
if (LockExit(state) == LockEntry(state))
state = LockOwners(state);
else
{
/* Not empty, but we just acquired fairly.
* Allow unfairness for a while. */
LockUnfair(state) = CurrentTick();
LockSpin(state) = spinState;
}
/* Ready to take shared ownership. */
swapState = Atomic_CompareAndSwap(LockState(lock), oldState, state);
if (swapState == oldState)
break;
}
if ((LockExit(state) & ~FIELD_SIGN) == 0)
{
/* Wakes those waiting on the artificial barrier inserted each half
* revolution (see above). */
key = (size_t)lock ^ LockExit(state);
CondLock_Broadcast(key);
}
}
//---------------------------------------------------------------------
// taskBalance
// This is the main balance task for the HTWay robot.
//
// Robot is assumed to start leaning on a wall. The first thing it
// does is take multiple samples of the gyro sensor to establish and
// initial gyro offset.
//
// After an initial gyro offset is established, the robot backs up
// against the wall until it falls forward, when it detects the
// forward fall, it start the balance loop.
//
// The main state variables are:
// gyroAngle This is the angle of the robot, it is the results of
// integrating on the gyro value.
// Units: degrees
// gyroSpeed The value from the Gyro Sensor after offset subtracted
// Units: degrees/second
// motorPos This is the motor position used for balancing.
// Note that this variable has two sources of input:
// Change in motor position based on the sum of
// MotorRotationCount of the two motors,
// and,
// forced movement based on user driving the robot.
// Units: degrees (sum of the two motors)
// motorSpeed This is the speed of the wheels of the robot based on the
// motor encoders.
// Units: degrees/second (sum of the two motors)
//
// From these state variables, the power to the motors is determined
// by this linear equation:
// power = KGYROSPEED * gyro +
// KGYROANGLE * gyroAngle +
// KPOS * motorPos +
// KSPEED * motorSpeed;
//
task taskBalance()
{
Follows(main);
float gyroSpeed, gyroAngle;
float motorSpeed;
int power, powerLeft, powerRight;
long tMotorPosOK;
long cLoop = 0;
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(0, LCD_LINE3, "Ich beginne");
TextOut(0, LCD_LINE4, "mich nun");
TextOut(0, LCD_LINE4, "selbst zu");
TextOut(0, LCD_LINE6, "Stabilisieren!");
tMotorPosOK = CurrentTick();
// Reset the motors to make sure we start at a zero position
ResetRotationCount(LEFT_MOTOR);
ResetRotationCount(RIGHT_MOTOR);
while(true) {
CalcInterval(cLoop++);
GetGyroData(gyroSpeed, gyroAngle);
GetMotorData(motorSpeed, motorPos);
// Apply the drive control value to the motor position to get robot
// to move.
motorPos -= motorControlDrive * tInterval;
// This is the main balancing equation
power = (KGYROSPEED * gyroSpeed + // Deg/Sec from Gyro sensor
KGYROANGLE * gyroAngle) / ratioWheel + // Deg from integral of gyro
KPOS * motorPos + // From MotorRotaionCount of both motors
KDRIVE * motorControlDrive + // To improve start/stop performance
KSPEED * motorSpeed; // Motor speed in Deg/Sec
if (abs(power) < 100)
tMotorPosOK = CurrentTick();
SteerControl(power, powerLeft, powerRight);
// Apply the power values to the motors
OnFwd(LEFT_MOTOR, powerLeft);
OnFwd(RIGHT_MOTOR, powerRight);
// Check if robot has fallen by detecting that motorPos is being limited
// for an extended amount of time.
if ((CurrentTick() - tMotorPosOK) > TIME_FALL_LIMIT) {
break;
}
Wait(WAIT_TIME);
}
Off(MOTORS);
ClearScreen();
TextOut(0, LCD_LINE1, "Bluetooth-Segway");
TextOut(0, LCD_LINE3, "Ich bin");
TextOut(0, LCD_LINE4, "hingefallen!");
TextOut(0, LCD_LINE5, "Neustart");
TextOut(0, LCD_LINE5, "erforderlich!");
}
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__;
}
