static bool
GenerateVux_generatePlanets (SOLARSYS_STATE *solarSys)
{
COUNT angle;
GenerateDefault_generatePlanets (solarSys);
if (CurStarDescPtr->Index == MAIDENS_DEFINED)
{
GenerateDefault_generatePlanets (solarSys);
// XXX: this is the second time that this function is
// called. Is it safe to remove one, or does this change
// the RNG so that the outcome is different?
solarSys->PlanetDesc[0].data_index = REDUX_WORLD;
solarSys->PlanetDesc[0].radius = EARTH_RADIUS * 212L / 100;
angle = ARCTAN (solarSys->PlanetDesc[0].location.x,
solarSys->PlanetDesc[0].location.y);
solarSys->PlanetDesc[0].location.x =
COSINE (angle, solarSys->PlanetDesc[0].radius);
solarSys->PlanetDesc[0].location.y =
SINE (angle, solarSys->PlanetDesc[0].radius);
ComputeSpeed(&solarSys->PlanetDesc[0], FALSE, 1);
}
else
{
if (CurStarDescPtr->Index == VUX_DEFINED)
{
solarSys->PlanetDesc[0].data_index = REDUX_WORLD;
solarSys->PlanetDesc[0].NumPlanets = 1;
solarSys->PlanetDesc[0].radius = EARTH_RADIUS * 42L / 100;
angle = HALF_CIRCLE + OCTANT;
ComputeSpeed(&solarSys->PlanetDesc[0], FALSE, 1);
}
else /* if (CurStarDescPtr->Index == VUX_BEAST_DEFINED) */
{
memmove (&solarSys->PlanetDesc[1], &solarSys->PlanetDesc[0],
sizeof (solarSys->PlanetDesc[0])
* solarSys->SunDesc[0].NumPlanets);
++solarSys->SunDesc[0].NumPlanets;
angle = HALF_CIRCLE - OCTANT;
solarSys->PlanetDesc[0].data_index = WATER_WORLD;
solarSys->PlanetDesc[0].radius = EARTH_RADIUS * 110L / 100;
solarSys->PlanetDesc[0].NumPlanets = 0;
ComputeSpeed(&solarSys->PlanetDesc[0], FALSE, 1);
}
solarSys->PlanetDesc[0].location.x =
COSINE (angle, solarSys->PlanetDesc[0].radius);
solarSys->PlanetDesc[0].location.y =
SINE (angle, solarSys->PlanetDesc[0].radius);
solarSys->PlanetDesc[0].rand_seed = MAKE_DWORD (
solarSys->PlanetDesc[0].location.x,
solarSys->PlanetDesc[0].location.y);
}
return true;
}
static bool
GenerateTrap_generatePlanets (SOLARSYS_STATE *solarSys)
{
COUNT angle;
int planetArray[] = { PRIMORDIAL_WORLD, WATER_WORLD, TELLURIC_WORLD };
solarSys->SunDesc[0].NumPlanets = (BYTE)~0;
solarSys->SunDesc[0].PlanetByte = 0;
if(!PrimeSeed){
solarSys->SunDesc[0].NumPlanets = (RandomContext_Random (SysGenRNG) % (9 - 1) + 1);
}
FillOrbits (solarSys, solarSys->SunDesc[0].NumPlanets, solarSys->PlanetDesc, FALSE);
GeneratePlanets (solarSys);
solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].data_index = TELLURIC_WORLD;
solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].alternate_colormap = NULL;
solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].NumPlanets = 1;
if(!PrimeSeed){
solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].data_index = planetArray[RandomContext_Random (SysGenRNG) % 2];
} else {
solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].radius = EARTH_RADIUS * 203L / 100;
angle = ARCTAN (solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].location.x,
solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].location.y);
solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].location.x =
COSINE (angle, solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].radius);
solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].location.y =
SINE (angle, solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte].radius);
ComputeSpeed(&solarSys->PlanetDesc[solarSys->SunDesc[0].PlanetByte], FALSE, 1);
}
return true;
}
static bool
GenerateDruuge_generatePlanets (SOLARSYS_STATE *solarSys)
{
COUNT angle;
GenerateDefault_generatePlanets (solarSys);
memmove (&solarSys->PlanetDesc[1], &solarSys->PlanetDesc[0],
sizeof (solarSys->PlanetDesc[0])
* solarSys->SunDesc[0].NumPlanets);
++solarSys->SunDesc[0].NumPlanets;
solarSys->PlanetDesc[0].data_index = DUST_WORLD;
solarSys->PlanetDesc[0].radius = EARTH_RADIUS * 50L / 100;
solarSys->PlanetDesc[0].NumPlanets = 0;
angle = HALF_CIRCLE - OCTANT;
solarSys->PlanetDesc[0].location.x =
COSINE (angle, solarSys->PlanetDesc[0].radius);
solarSys->PlanetDesc[0].location.y =
SINE (angle, solarSys->PlanetDesc[0].radius);
solarSys->PlanetDesc[0].rand_seed = MAKE_DWORD (
solarSys->PlanetDesc[0].location.x,
solarSys->PlanetDesc[0].location.y);
ComputeSpeed(&solarSys->PlanetDesc[0], FALSE, 1);
return true;
}
static bool
GenerateSpathi_generateMoons (SOLARSYS_STATE *solarSys, PLANET_DESC *planet)
{
COUNT angle;
GenerateDefault_generateMoons (solarSys, planet);
if (matchWorld (solarSys, planet, 0, MATCH_PLANET))
{
#ifdef NOTYET
utf8StringCopy (GLOBAL_SIS (PlanetName),
sizeof (GLOBAL_SIS (PlanetName)),
"Spathiwa");
#endif /* NOTYET */
solarSys->MoonDesc[0].data_index = PELLUCID_WORLD;
solarSys->MoonDesc[0].alternate_colormap = NULL;
solarSys->MoonDesc[0].radius = MIN_MOON_RADIUS + MOON_DELTA;
angle = NORMALIZE_ANGLE (LOWORD (RandomContext_Random (SysGenRNG)));
solarSys->MoonDesc[0].location.x =
COSINE (angle, solarSys->MoonDesc[0].radius);
solarSys->MoonDesc[0].location.y =
SINE (angle, solarSys->MoonDesc[0].radius);
ComputeSpeed(&solarSys->MoonDesc[0], TRUE, 1);
}
return true;
}
void Asserv::Compute()
{
if (running) {
float actualSpeed = GetSpeed();
float newSpeed = actualSpeed;
d_Remaining = finalTarget - (target * meterPerStep);
int sens = 1;
if (d_Remaining < 0) sens = -1;
if (initialSens != sens) { // target reachead
running = false;
if (finalSpeed == 0.0) SetSpeed(finalSpeed);
return;
}
/* distance theorique (temps continu) pour annuler la vitesse avec un freinage maximum et constant*/
d_freinage = (actualSpeed*actualSpeed - finalSpeed*finalSpeed) * .5 / dec;
if (abs(d_Remaining) > d_freinage ) {
if (abs(actualSpeed) < maxSpeed ) {
newSpeed += (sens * acc * sampleTime);
}
} else {
if (emergencyStop) {
newSpeed -= (sens * decEmergency * sampleTime);
} else {
newSpeed -= (sens * dec * sampleTime);
}
}
SetSpeed(newSpeed);
}
ComputeSpeed();
}
static bool
GenerateBurvixese_generatePlanets (SOLARSYS_STATE *solarSys)
{
COUNT angle;
GenerateDefault_generatePlanets (solarSys);
solarSys->PlanetDesc[0].data_index = REDUX_WORLD;
solarSys->PlanetDesc[0].NumPlanets = 1;
solarSys->PlanetDesc[0].radius = EARTH_RADIUS * 39L / 100;
angle = ARCTAN (solarSys->PlanetDesc[0].location.x,
solarSys->PlanetDesc[0].location.y);
solarSys->PlanetDesc[0].location.x =
COSINE (angle, solarSys->PlanetDesc[0].radius);
solarSys->PlanetDesc[0].location.y =
SINE (angle, solarSys->PlanetDesc[0].radius);
ComputeSpeed(&solarSys->PlanetDesc[0], FALSE, 1);
return true;
}
static bool
GenerateAndrosynth_generatePlanets (SOLARSYS_STATE *solarSys)
{
COUNT angle;
GenerateDefault_generatePlanets (solarSys);
solarSys->PlanetDesc[1].data_index = TELLURIC_WORLD;
solarSys->PlanetDesc[1].radius = EARTH_RADIUS * 204L / 100;
angle = ARCTAN (solarSys->PlanetDesc[1].location.x,
solarSys->PlanetDesc[1].location.y);
solarSys->PlanetDesc[1].location.x =
COSINE (angle, solarSys->PlanetDesc[1].radius);
solarSys->PlanetDesc[1].location.y =
SINE (angle, solarSys->PlanetDesc[1].radius);
ComputeSpeed(&solarSys->PlanetDesc[1], FALSE, 1);
return true;
}
static bool
GenerateBurvixese_generateMoons (SOLARSYS_STATE *solarSys, PLANET_DESC *planet)
{
GenerateDefault_generateMoons (solarSys, planet);
if (matchWorld (solarSys, planet, 0, MATCH_PLANET))
{
COUNT angle;
DWORD rand_val;
solarSys->MoonDesc[0].data_index = SELENIC_WORLD;
solarSys->MoonDesc[0].radius = MIN_MOON_RADIUS
+ (MAX_MOONS - 1) * MOON_DELTA;
rand_val = TFB_Random ();
angle = NORMALIZE_ANGLE (LOWORD (rand_val));
solarSys->MoonDesc[0].location.x =
COSINE (angle, solarSys->MoonDesc[0].radius);
solarSys->MoonDesc[0].location.y =
SINE (angle, solarSys->MoonDesc[0].radius);
ComputeSpeed(&solarSys->MoonDesc[0], TRUE, 1);
}
return true;
}
static bool
GenerateChmmr_generateMoons (SOLARSYS_STATE *solarSys, PLANET_DESC *planet)
{
GenerateDefault_generateMoons (solarSys, planet);
if (matchWorld (solarSys, planet, 1, MATCH_PLANET))
{
COUNT angle;
DWORD rand_val;
solarSys->MoonDesc[0].data_index = HIERARCHY_STARBASE;
solarSys->MoonDesc[0].radius = MIN_MOON_RADIUS;
rand_val = TFB_Random ();
angle = NORMALIZE_ANGLE (LOWORD (rand_val));
solarSys->MoonDesc[0].location.x =
COSINE (angle, solarSys->MoonDesc[0].radius);
solarSys->MoonDesc[0].location.y =
SINE (angle, solarSys->MoonDesc[0].radius);
ComputeSpeed(&solarSys->MoonDesc[0], TRUE, 1);
}
return true;
}
static bool
GenerateSpathi_generatePlanets (SOLARSYS_STATE *solarSys)
{
PLANET_DESC *pMinPlanet;
COUNT angle;
pMinPlanet = &solarSys->PlanetDesc[0];
solarSys->SunDesc[0].NumPlanets = 1;
FillOrbits (solarSys,
solarSys->SunDesc[0].NumPlanets, pMinPlanet, FALSE);
pMinPlanet->radius = EARTH_RADIUS * 1150L / 100;
angle = ARCTAN (pMinPlanet->location.x, pMinPlanet->location.y);
pMinPlanet->location.x = COSINE (angle, pMinPlanet->radius);
pMinPlanet->location.y = SINE (angle, pMinPlanet->radius);
pMinPlanet->data_index = WATER_WORLD;
pMinPlanet->alternate_colormap = NULL;
if (GET_GAME_STATE (SPATHI_SHIELDED_SELVES))
pMinPlanet->data_index |= PLANET_SHIELDED;
pMinPlanet->NumPlanets = 1;
ComputeSpeed(pMinPlanet, FALSE, 1);
return true;
}
static bool
GenerateUtwig_generatePlanets (SOLARSYS_STATE *solarSys)
{
COUNT angle;
GenerateDefault_generatePlanets (solarSys);
if (CurStarDescPtr->Index == UTWIG_DEFINED)
{
solarSys->PlanetDesc[0].data_index = WATER_WORLD;
solarSys->PlanetDesc[0].NumPlanets = 1;
solarSys->PlanetDesc[0].radius = EARTH_RADIUS * 174L / 100;
angle = ARCTAN (solarSys->PlanetDesc[0].location.x,
solarSys->PlanetDesc[0].location.y);
solarSys->PlanetDesc[0].location.x =
COSINE (angle, solarSys->PlanetDesc[0].radius);
solarSys->PlanetDesc[0].location.y =
SINE (angle, solarSys->PlanetDesc[0].radius);
ComputeSpeed(&solarSys->PlanetDesc[0], FALSE, 1);
}
return true;
}
static bool
GenerateSaMatra_generateMoons (SOLARSYS_STATE *solarSys, PLANET_DESC *planet)
{
GenerateDefault_generateMoons (solarSys, planet);
if (matchWorld (solarSys, planet, 4, MATCH_PLANET))
{
COUNT angle;
DWORD rand_val;
solarSys->MoonDesc[0].data_index = SA_MATRA;
solarSys->MoonDesc[0].radius = MIN_MOON_RADIUS + (2 * MOON_DELTA);
rand_val = RandomContext_Random (SysGenRNG);
angle = NORMALIZE_ANGLE (LOWORD (rand_val));
solarSys->MoonDesc[0].location.x =
COSINE (angle, solarSys->MoonDesc[0].radius);
solarSys->MoonDesc[0].location.y =
SINE (angle, solarSys->MoonDesc[0].radius);
ComputeSpeed(&solarSys->MoonDesc[0], TRUE, 1);
}
return true;
}
void ProcessData(void) {
int i, j, k, l;
bool bMarkerBreakFlag;
// eliminating "crossovers" and computing brake distance.
for (i = 1, j = 1, k = 0; i < rSegment.cnt; i++, k++, j=i) {
while (rSegment.state[j] != MARKER)
j++;
pSegment.state[k] = rSegment.state[i];
for (; i < j; i++) {
pSegment.length[k] += rSegment.length[i];
pSegment.angle[k] += rSegment.angle[i];
if (rSegment.state[i] == JUNCTION) {
pSegment.juncSegCnt[k]++;
}
}
// marker state
bMarkerBreakFlag = (rSegment.length[i] > LINESEN_SIDESEN_DIST);
if (bMarkerBreakFlag) {
l = k;
pSegment.syncLength[k] = SYNC_DISTANCE;
pSegment.bMarkerBreakFlag[k] = TRUE;
} else {
l = k + 1;
pSegment.syncLength[k] = rSegment.length[i];
pSegment.bMarkerBreakFlag[k] = FALSE;
}
pSegment.length[l] += rSegment.length[i];
pSegment.angle[l] += rSegment.angle[i];
pSegment.brakeDist[k] = bMarkerBreakFlag ? MAX_BRAKE_DIST : MIN_BRAKE_DIST;
// if (pSegment.length[k] > 2500) {
// pSegment.brakeDist[k] += 250;
// }
}
// differentiating straight from curve with huge radius.
for (i = 0; i < k; i++) {
if (pSegment.state[i] == STRAIGHT) {
if (ABS(pSegment.angle[i]) > ANGLE_THRESHOLD)
pSegment.state[i] = (pSegment.angle[i] > 0) ? LEFT : RIGHT;
}
}
// correct segment states that were incorrectly recorded.
for (i = 0; i < k; i++) {
if (pSegment.angle[i] < -(ANGLE_THRESHOLD))
pSegment.state[i] = RIGHT;
else if (pSegment.angle[i] > ANGLE_THRESHOLD)
pSegment.state[i] = LEFT;
else
pSegment.state[i] = STRAIGHT;
}
// compute the radius (if any) of a segment & the total recorded displacement.
for (i = 0; i < k; i++) {
if (pSegment.state[i] != STRAIGHT) {
// arc length = radius * theta (rads)
pSegment.radius[i] = (pSegment.length[i] * 180) / (ABS(pSegment.angle[i]) * _PI_);
}
pSegment.totalDisp += pSegment.length[i];
}
// check which segments to synchronise fast run.
for (i = 0; i < k; i++) {
pSegment.bSyncFlag[i] = ( ( (pSegment.state[i] == STRAIGHT) && (pSegment.length[i] > 200) ) ||
( ( (pSegment.state[i] == LEFT) || (pSegment.state[i] == RIGHT) )
&& (pSegment.radius[i] > 400) ) );
}
pSegment.cnt = k;
ComputeSpeed(0);
}
void FastRun(int fastType) {
sSeg segment;
int index, *i = &segmentIndex;
char s[4];
if (pSegment.cnt == 0) {
DispDotMatrixWait("No data recorded");
return;
}
StartBeep();
DispDotMatrix("GOGO");
DelaymSec(1500);
bFastFlag = TRUE;
ComputeSpeed(fastType);
segment = pSegment;
segPtr = &segment;
segmentIndex = 0;
StartRun();
// SetRobotSpeedX(segment.topSpeed[*i]);
// SetRobotAccX(ACCELERATION);
SetMoveCommand(XSPEED, 1000, 0, segment.topSpeed[*i], segment.topSpeed[*i], ACCELERATION);
while (markerCnt[RIGHT_SENSOR] != 1) {
if (bSWFlag) {
bSWFlag = FALSE;
StopRun();
return;
}
}
StartRunTimer();
for (; *i < segment.cnt; (*i)++) {
sprintf(s, "%4d", *i);
DispDotMatrix(s);
WaitSyncFastRun();
if (MoveRobotFastRun(XSPEED, segment.length[*i], segment.brakeDist[*i], segment.topSpeed[*i],
segment.endSpeed[*i], ACCELERATION))
break; // switch break
// if (segment.bSyncFlag[*i]) {
// *i = markerCnt[LEFT_SENSOR] - 1;
// }
segment.curDisp += segment.length[*i];
if (bStopFlag) {
(*i)++;
break;
}
// StartBeep();
}
StopRunTimer();
StopRun();
align_Kp = 4;
align_Kd = 40;
sprintf(s, "%04d", runTime);
// DispDotMatrixWait(s);
WaitSW();
clrscr();
printf("segment cnt: %d i: %d\n", segment.cnt, *i);
printf("No. State Top Min Max Dist Length Actual Break Marker\n");
printf("--- ----- --- --- --- ---- ------ ------ ----- ------\n");
for (index = 0; index < *i; index++) {
printf("%3d %-9s %4dmm/s %4dmm/s %4dmm/s %4dmm %4dmm %4dmm %d %d\n", index, run_state[segment.state[index]],
segment.topSpeed[index], segment.actMinSpeed[index], segment.actMaxSpeed[index], segment.dist[index], segment.length[index],
segment.actDisp[index], segment.bActBreakFlag[index], segment.bMarkerBreakFlag[index]);
}
DispDotMatrixWait("Done");
}
////////////////////////////////////////////////////////////////////////////
// ProcessANTCBSCRXEvents
//
// CBSC Reciever event processor
//
// Processes events recieved from CBSC module.
//
// \return: N/A
///////////////////////////////////////////////////////////////////////////
void ProcessANTCBSCRXEvents(ANTPLUS_EVENT_RETURN* pstEvent_)
{
switch (pstEvent_->eEvent)
{
case ANTPLUS_EVENT_CHANNEL_ID:
{
// Can store this device number for future pairings so that
// wild carding is not necessary.
printf("Device number is %d\n", pstEvent_->usParam1);
printf("Transmission type is %d\n\n", pstEvent_->usParam2);
break;
}
case ANTPLUS_EVENT_PAGE:
{
// Get data correspinding to the page. Only get the data you
// care about.
switch(pstEvent_->usParam1)
{
case CBSCRX_PAGE_0:
{
CBSCPage0_Data* pstPage0Data = CBSCRX_GetPage0();
CBSCPage0_Data* pstPrev0Data = CBSCRX_GetPastPage0();
printf("Combined Bike Speed and Cadence Page 0\n\n");
ComputeSpeed(pstPage0Data, pstPrev0Data);
ComputeCadence(pstPage0Data, pstPrev0Data);
//update accumulated values, handles rollovers
ulBSAccumRevs += (ULONG)((pstPage0Data->usCumSpeedRevCount - pstPrev0Data->usCumSpeedRevCount) & MAX_USHORT);
ulBCAccumCadence += (ULONG)((pstPage0Data->usCumCadenceRevCount - pstPrev0Data->usCumCadenceRevCount) & MAX_USHORT);
#if 0
printf("Current speed time: %u", (ULONG)(pstPage0Data->usLastTime1024 / 1024));
printf(".%03u s\n", (ULONG)((((pstPage0Data->usLastTime1024 % 1024) * CBSC_PRECISION)+(ULONG)512)/(ULONG)1024));
printf("Delta speed time: %u", (ULONG)(stSpeedData.usDeltaValue / 1024));
printf(".%03u s\n", (ULONG)((((stSpeedData.usDeltaValue % 1024) * CBSC_PRECISION)+(ULONG)512)/(ULONG)1024));
printf("Instantaneous time inverse (x circumference for inst speed): %u", (ULONG)(stSpeedData.ulIntValue));
printf(".%03u 1/s\n", stSpeedData.usFracValue);
printf("Accumulated revs (x circumference for total distance): 0x%04X", (USHORT)((ulBSAccumRevs >> 16) & MAX_USHORT));
printf("%04X\n", (USHORT)(ulBSAccumRevs & MAX_USHORT)); //display limited by 16-bit CPU
printf("Current cadence time: %u", (ULONG)(pstPage0Data->usLastCadence1024 / 1024));
printf(".%03u s\n", (ULONG)((((pstPage0Data->usLastCadence1024 % 1024) * CBSC_PRECISION)+(ULONG)512)/(ULONG)1024));
printf("Delta cadence time: %u", (ULONG)(stCadenceData.usDeltaValue / 1024));
printf(".%03u s\n", (ULONG)((((stCadenceData.usDeltaValue % 1024) * CBSC_PRECISION)+(ULONG)512)/(ULONG)1024));
printf("Instantaneous cadence: %u", (ULONG)(stCadenceData.ulIntValue));
printf(".%03u RPM\n", stCadenceData.usFracValue);
printf("Accumulated cadence: 0x%04X", (USHORT)((ulBCAccumCadence >> 16) & MAX_USHORT));
printf("%04X\n\n\n", (USHORT)(ulBCAccumCadence & MAX_USHORT)); //display limited by 16-bit CPU
#endif
window_postmessage(EVENT_SPORT_DATA, SPORTS_SPEED, (void*)(window_readconfig()->circumference * (stSpeedData.ulIntValue * CBSC_PRECISION + stSpeedData.usFracValue)));
window_postmessage(EVENT_SPORT_DATA, SPORTS_CADENCE, (void*)stCadenceData.ulIntValue);
//move current data to the past
pstPrev0Data->usCumCadenceRevCount = pstPage0Data->usCumCadenceRevCount;
pstPrev0Data->usLastCadence1024 = pstPage0Data->usLastCadence1024;
pstPrev0Data->usCumSpeedRevCount = pstPage0Data->usCumSpeedRevCount;
pstPrev0Data->usLastTime1024 = pstPage0Data->usLastTime1024;
break;
}
default:
{
// ASSUME PAGE 0
printf("Invalid or undefined page\n\n");
break;
}
}
break;
}
case ANTPLUS_EVENT_UNKNOWN_PAGE: // Decode unknown page manually
case ANTPLUS_EVENT_NONE:
default:
{
break;
}
}
}
