//------------------------------------------------------------------------------
// transmit() -- send radar emissions
//------------------------------------------------------------------------------
void IrSensor::transmit(const double dt)
{
BaseClass::transmit(dt);
// In transmit (request IR) mode and have a IrSeeker
const auto seeker = dynamic_cast<IrSeeker*>( getSeeker() );
if (seeker != nullptr && isQuerying()) {
// Send the emission to the other player
const auto irQuery = new IrQueryMsg();
if (irQuery != nullptr) {
irQuery->setLowerWavelength(getLowerWavelength());
irQuery->setUpperWavelength(getUpperWavelength());
irQuery->setInstantaneousFieldOfView(getIFOV());
irQuery->setSendingSensor(this);
irQuery->setNEI(getNEI());
irQuery->setMaxRangeNM(getMaximumRange()* base::distance::M2NM);
seeker->irRequestSignature(irQuery);
irQuery->unref();
} // If irQuery not null
else {
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "Error: IrQueryMsg memory failure in IrSensor.cpp in IrSensor::transmit" << std::endl;
}
}
}
}
//------------------------------------------------------------------------------
// Process players of interest -- This will work with the function in Gimbal to create
// a filtered list of players that we plan to send emission packets to.
//------------------------------------------------------------------------------
void IrSystem::processPlayersOfInterest()
{
// ---
// Do we have a seeker?
// ---
if (getSeeker() != nullptr) {
base::PairStream* poi = nullptr;
WorldModel* sim = getWorldModel();
if ( sim != nullptr && !areQueriesDisabled() )
poi = getWorldModel()->getPlayers();
// Pass our players of interest to the seeker for processing
getSeeker()->processPlayersOfInterest(poi);
if (poi != nullptr) { poi->unref(); poi = nullptr; }
}
}
////////////////////////////////////////////////////////////////////////////// TASK MAIN!!! \(^0^)/
task main()
{
clearDebugStream();
gyroCal();
getSeeker();
int left = 0;
int delay = 0;
int endDump = 0;
int ramp = 0;
int* switches = getButtons();
//asign meaning to the touch sensors
if(switches[3])
left = 1;
else
left = 0;
if(switches[2])
delay = 1;
else
delay = 0;
if(switches[1])
endDump = 1;
else
endDump = 0;
if(switches[0])
ramp = 1;
else
ramp = 0;
//Round starts here
waitForStart();
nMotorEncoder[BR] = 0;
nMotorEncoder[wrist1] = 0;
nMotorEncoder[arms] = 0;
if(endDump)
EndDump(left, delay);
else if(ramp)
Ramp(left, delay);
else
IRramp(left , delay);
}
//------------------------------------------------------------------------------
// updateData() -- update background data here
//------------------------------------------------------------------------------
void IrSystem::updateData(const double dt)
{
// ---
// Do we need to find the seeker?
// ---
if (getSeeker() == nullptr && getSeekerName() != nullptr && getOwnship() != nullptr) {
// We have a name of the seeker, but not the seeker itself
const char* name = *getSeekerName();
// Get the named seeker from the player's list of gimbals
const auto p = dynamic_cast<IrSeeker*>( getOwnship()->getGimbalByName(name) );
if (p != nullptr) {
setSeeker( p );
// FAB - not needed - esp if multiple sensors on a seeker.
//getSeeker()->setSystem(this);
}
if (getSeeker() == nullptr) {
// The assigned seeker was not found!
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "IrSystem::update() ERROR -- seeker: " << name << ", was not found!" << std::endl;
}
setSlotSeekerName(nullptr);
}
}
// ---
// Process our players of interest
// ---
processPlayersOfInterest();
// ---
// Our base class methods
// ---
BaseClass::updateData(dt);
}
//------------------------------------------------------------------------------
// reset() -- Reset parameters
//------------------------------------------------------------------------------
void IrSystem::reset()
{
// FAB - sensor needs to know its seeker without waiting for updateData
if (getSeeker() == nullptr && getSeekerName() != nullptr && getOwnship() != nullptr) {
// We have a name of the seeker, but not the seeker itself
const char* name = *getSeekerName();
// Get the named seeker from the player's list of gimbals
const auto p = dynamic_cast<IrSeeker*>( getOwnship()->getGimbalByName(name) );
if (p != nullptr) {
setSeeker( p );
}
if (getSeeker() == nullptr) {
// The assigned seeker was not found!
if (isMessageEnabled(MSG_ERROR)) {
std::cerr << "IrSystem::update() ERROR -- seeker: " << name << ", was not found!" << std::endl;
}
setSlotSeekerName(nullptr);
}
}
BaseClass::reset();
}
task main()
{
gyroCal();
//waitForStart();
clearDebugStream();
writeDebugStreamLine("starting!");
nMotorEncoder[FrontR] = 0;
nMotorEncoder[grabber1] = 0;
nMotorEncoder[grabber2] = 0;
int beac = getSeeker();
if(beac == -1)
{
writeDebugStreamLine("This no good");
Stop(0);
}
else if(beac == 1)
{
writeDebugStreamLine("orientation 1 is running");
turn(-41, 25);
move(50, 30, 1, 0);
turn(82, 25);
move (40, 40, 1, 0);
}
else if(beac == 2)
{
writeDebugStreamLine("orientation 2 is running");
turn(-19, 25);
move(60, 30, 1, 0);
turn(30, 25);
move(40, 40, 1, 0);
}
else if(beac == 3)
{
writeDebugStreamLine("orientation 3 is running");
move(60, 30, 1, 0);
turn(-36, 25);
move(40, 40, 1, 0);
}
else
{
writeDebugStreamLine("the sensor thought it was in region %d", beac);
}
//move(-10 , 30, 1, 1);
//writeDebugStreamLine("finished enitial move");
//wait10Msec(100);
//turn(82, 25);
//writeDebugStreamLine("finished first turn");
/*move(-40 , 50, 1, 0);
turn(-45, 50);
move(-10, 30, 1, 0);
ClearTimer(T1);
while(nMotorEncoder[grabber1] > -90 && time1[T1] < 1500)
{
if(abs(nMotorEncoder[grabber1]-nMotorEncoder[grabber2]) > 10)
writeDebugStreamLine("the grabbers aren't moving with each other");
motor[grabber1] = -30;
motor[grabber2] = -30;
}
motor[grabber1] = 0;
motor[grabber2] = 0;*/
}
// isQuerying() -- Returns true if the IR system is currently observing targets
bool IrSystem::isQuerying() const
{
// Default: if we're enabled and have a seeker, we're transmitting.
return ( !areQueriesDisabled() && getSeeker() != nullptr && getOwnship() != nullptr );
}
void Curve::addMarker() {
float n=marker->nnew2new(getSeeker());
marker->add(marker->new2old(n),n);
selMarker=marker->getAreaNew(n);
}
void IRramp(int left , int delay)
{
int mult = 1;
int beacon = 0;
int boost = 0;
if(left) //account for starting position
mult = - 1;
else
mult = 1;
if(delay) // wait for other team but go faster
{
wait1Msec(5000);
boost = 20;
}
else
boost = 0;
if(!left)
{
// get intp initial position
move(mult * 10, 20 + boost);
turn(mult * -42, 40 + boost);
//Raise the arm
while(nMotorEncoder[arms] > -4900)
{
motor[arms] = -30;
}
motor[arms] = 0;
wait10Msec(25);
}
else
{
//get into initial postion
move(mult * 15, 20 + boost);
turn(mult * -44, 40 + boost);
move(-25, 20);
wait10Msec(25);
//raise the arm
while(nMotorEncoder[arms] > -4900)
{
motor[arms] = -30;
}
motor[arms] = 0;
}
//check at each position
if(getSeeker())
{
beacon = 1;
writeDebugStreamLine("found becon at 1");
dump(left);
}
if(!beacon)
{
adjustedMove(mult * 25, 20 + boost);
wait1Msec(250);
if(getSeeker() && !beacon)
{
beacon = 2;
writeDebugStreamLine("found becon at 2");
dump(left);
}
if(!beacon)
{
adjustedMove( mult * 55, 20 + boost);
wait10Msec(25);
if(getSeeker() && !beacon)
{
beacon = 3;
writeDebugStreamLine("found becon at 3");
dump(left);
}
if(!beacon)
{
adjustedMove(mult * 25, 20 + boost);
wait10Msec(25);
beacon = 4;
writeDebugStreamLine("found becon at 4");
//arived at last basket w/o sensing beacon, dump regardless
dump(left);
}
}
}
wait10Msec(50);
//decide how far to go based on position
int add = 0;
if(beacon == 1)
add = 100;
else if(beacon == 2)
add = 75;
else if(beacon == 3)
add = 25;
else
add = 0;
if(left)
add += 25;
//lift the wrist back up
while(nMotorEncoder[wrist1] < -5)
{
motor[wrist1] = 20;
motor[wrist2] = 20;
}
motor[wrist1] = 0;
motor[wrist2] = 0;
//drive to end of baskets
if(left)
{
turn(-90, 50 + boost);
move(mult * (15+add), 50 + boost);
while(nMotorEncoder[arms] < -250)
{
motor[arms] = 40;
}
motor[arms] = 0;
}
else
{
turn(-90, 50 + boost);
while(nMotorEncoder[arms] < -250)
{
motor[arms] = 40;
}
motor[arms] = 0;
move(mult * (15+add), 50 + boost);
}
//two-point turn in tight space
turn(mult * 45, 50 );
move(mult * 25, 50 + boost);
turn(mult * 30, 50 );
move(mult * 80, 50 + boost);
turn(-90, 50 + boost);
//Drive onto ramp
move(-50, 70 + boost);
move(-40, 90);
}
