Shooter::LatchPosition Shooter::getRawSLatch() {
Shooter::LatchPosition pos = !sLatchSensor->Get() ? Shooter::kLatched
: Shooter::kUnlatched;
if (sLatchPatternBuffer.lastState != pos && getTurns() < 0.25) {
// Something happened
Logger::log(Logger::kFinest, "Shooter",
"Shooter latch changed from %s to %s",
sLatchPatternBuffer.lastState == Shooter::kLatched ? "Latched"
: "Unlatched", pos == Shooter::kLatched ? "Latched"
: "Unlatched");
if (pos) {
sLatchPatternBuffer.lastRisingEdge = getCurrentMillis();
} else {
sLatchPatternBuffer.lastFallingEdge = getCurrentMillis();
}
if (sLatchPatternBuffer.solenoidChangeTime + SHOOTER_PATTERN_KILLZONE
> getCurrentMillis()) {
sLatchPatternBuffer.lastFallingEdge = -1;
sLatchPatternBuffer.lastRisingEdge = -1;
Logger::log(Logger::kFinest, "Shooter",
"Cleared shooter latch pattern buffer due to a recent change");
}
}
sLatchPatternBuffer.lastState = pos;
return pos;
}
void SBMLParser::inputSBMLDocumentFromString(const char *str)
{
SBMLReader *reader;
unsigned long start, stop;
start = getCurrentMillis();
reader = new SBMLReader();
document = reader->readSBMLFromString(str);
stop = getCurrentMillis();
unsigned int errors = document->getNumErrors();
if (verbose)
{
cout << endl;
cout << " filename: <stdin>" << endl;
cout << " file size: "<< strlen(str) << endl;
cout << " read time (ms): " << stop - start << endl;
cout << " validation error(s): " << errors << endl;
cout << endl;
}
document->printErrors(cerr);
delete reader;
}
int MessageProcessor::receiveMessage(NETWORK_MSG *msg, struct sockaddr_in *source) {
socklen_t socklen = sizeof(struct sockaddr_in);
// assume we don't care about the value of socklen
//cout << "Waiting for message from server" << endl;
int ret = recvfrom(sock, (char*)msg, sizeof(NETWORK_MSG), 0, (struct sockaddr *)source, &socklen);
//cout << "Returned from wait" << endl;
if (ret == -1)
return ret;
// add id to the NETWORK_MSG struct
if (msg->type == MSG_TYPE_ACK) {
if (!sentMessages[msg->id][source->sin_addr.s_addr].getAcked()) {
sentMessages[msg->id][source->sin_addr.s_addr].setAcked(true);
sentMessages[msg->id][source->sin_addr.s_addr].setTimeAcked(getCurrentMillis());
if (outputLog)
(*outputLog) << "Received ack for message id " << msg->id << endl;
}
return -1; // don't do any further processing
}else {
bool isDuplicate = false;
map<unsigned int, unsigned long long>& ackedPlayerMessages = ackedMessages[source->sin_addr.s_addr];
if (ackedPlayerMessages.find(msg->id) != ackedPlayerMessages.end()) {
isDuplicate = true;
cout << "Got duplicate of type " << msg->type << endl;
if (outputLog)
(*outputLog) << "Received duplicate (id " << msg->id << ") of type " << MessageContainer::getMsgTypeString(msg->type) << endl;
}else {
cout << "Got message of type " << msg->type << endl;
if (outputLog)
(*outputLog) << "Received message (id " << msg->id << ") of type " << MessageContainer::getMsgTypeString(msg->type) << endl;
}
ackedPlayerMessages[msg->id] = getCurrentMillis();
NETWORK_MSG ack;
ack.id = msg->id;
ack.type = MSG_TYPE_ACK;
sendto(sock, (char*)&ack, sizeof(NETWORK_MSG), 0, (struct sockaddr *)source, sizeof(struct sockaddr_in));
if (isDuplicate)
return -1;
}
return ret;
}
void BeagleBone::parse(char *data) {
int area;
float x, y, level1, level2;
int id, total;
double time = getCurrentMillis();
sscanf(data, "%d/%d\t%d\t%f\t%f\t%f\t%f", &id, &total, &area, &x, &y,
&level1, &level2);
if (id == 1337 && total == 1337) {
//Gyro
gyro.yaw = y;
gyro.pitch = level1;
gyro.roll = level2;
lastGyroUpdate = time;
} else {
vision.x = x;
vision.y = y;
vision.level1 = level1;
vision.level2 = level2;
vision.id = id;
vision.total = total;
vision.area = area;
lastVisionUpdate = time;
}
}
void WLatch::Initialize() {
bTime = getCurrentMillis();
if (shooter->getWLatch() == state) {
bTime = 0;
}
shooter->setWLatch(state);
}
bool AngelChange::IsFinished() {
#if ASYNC_BRAKE
return brakeEngagedTime> 0 && brakeEngagedTime + 100 < getCurrentMillis();
#else
return stability>13 || IsTimedOut();
#endif
}
void AngelChange::Execute() {
pterodactyl->setOutputRange();
// Let the PID run.
if (pterodactyl->isPIDFinished(true) || (target <= 0
&& pterodactyl->getAngle() <= 0.25)) {
stability++;
} else {
stability = 0;
}
if (tmpTarget == 45 && target < 45 && shooter->isReallyDrawnBack()) {
tmpTarget = target;
pterodactyl->setTarget(tmpTarget+(tmpTarget>0 ? 0.5 : 0));
}
#if ASYNC_BRAKE
if ((stability> 13 && (tmpTarget != 45)) || IsTimedOut()) {
if (brakeEngagedTime <= 0) {
brakeEngagedTime = getCurrentMillis();
}
if (target> 0) {
pterodactyl->setBrakeState(Pterodactyl::kActive);
}
}
#endif
}
void WLatch::Execute() {
shooter->setWLatch(state);
time = getCurrentMillis() - bTime;
if (state == Shooter::kLatched&& shooter->getWLatch() == state && time
< SHOOTER_WLATCH_WAIT - SHOOTER_WLATCH_UNLOCK_DRIVE - 100) {
bTime = getCurrentMillis()
- (SHOOTER_WLATCH_WAIT - SHOOTER_WLATCH_UNLOCK_DRIVE - 100) + 1;
}
if (state == Shooter::kUnlatched && time
> (SHOOTER_WLATCH_WAIT - SHOOTER_WLATCH_UNLOCK_DRIVE)) {
shooter->setWenchMotor(SHOOTER_WENCH_MOTOR_FORCE_UNLATCH);
}
if (state == Shooter::kLatched && time
> (SHOOTER_WLATCH_WAIT - SHOOTER_WLATCH_LOCK_DRIVE)) {
shooter->setWenchMotor(SHOOTER_WENCH_MOTOR_FORCE_LATCH);
}
}
void Shooter::resetShooter() {
Logger::log(Logger::kInfo, "Shooter", "RESET ALL THE THINGS");
sLatchPatternBuffer.lastFallingEdge = -1;
sLatchPatternBuffer.lastRisingEdge = -1;
if (getRawProximity() && pullBackSwitchPatternBuffer.lastRisingEdge < 0) {
pullBackSwitchPatternBuffer.lastRisingEdge = getCurrentMillis();
}
}
int
main (int argc, char *argv[])
{
const char *filename;
#ifdef __BORLANDC__
unsigned long start, stop;
#else
unsigned long long start, stop;
#endif
unsigned long size;
unsigned int errors = 0;
SBMLDocument_t *d;
if (argc != 2)
{
printf("Usage: validateSBML filename\n");
return 2;
}
filename = argv[1];
start = getCurrentMillis();
d = readSBML(filename);
stop = getCurrentMillis();
errors = SBMLDocument_getNumErrors(d);
errors += SBMLDocument_checkConsistency(d);
size = getFileSize(filename);
printf( "\n" );
printf( " filename: %s\n" , filename );
printf( " file size: %lu\n", size );
printf( " read time (ms): %llu\n", stop - start );
printf( " error(s): %u\n" , errors );
if (errors > 0) SBMLDocument_printErrors(d, stdout);
printf("\n");
SBMLDocument_free(d);
return errors;
}
bool Shooter::isLatchedByPattern() {
Shooter::LatchPosition pos = getRawSLatch();
return pos && (sLatchPatternBuffer.lastFallingEdge > 0)
&& (sLatchPatternBuffer.lastRisingEdge > 0)
&& (sLatchPatternBuffer.lastFallingEdge
< sLatchPatternBuffer.lastRisingEdge)
&& sLatchPatternBuffer.lastRisingEdge
+ SHOOTER_SLATCH_PATTERN_DELAY < getCurrentMillis();
}
bool Shooter::getRawProximity() {
bool state = !pullBackSwitchLeft->Get() || !pullBackSwitchRight->Get();
if (pullBackSwitchPatternBuffer.lastState != state) {
pullBackSwitchPatternBuffer.lastState = state;
if (state) {
pullBackSwitchPatternBuffer.lastRisingEdge = getCurrentMillis();
} else {
pullBackSwitchPatternBuffer.lastFallingEdge = getCurrentMillis();
}
if (pullBackSwitchPatternBuffer.solenoidChangeTime
+ SHOOTER_PATTERN_KILLZONE > getCurrentMillis()) {
pullBackSwitchPatternBuffer.lastFallingEdge = -1;
pullBackSwitchPatternBuffer.lastRisingEdge = -1;
if (sLatch->Get() == Shooter::kLatched) {
pullBackSwitchPatternBuffer.lastState = true;
} else {
pullBackSwitchPatternBuffer.lastState = false;
}
// Logger::log(Logger::kFinest, "Shooter",
// "Cleared proximity pattern buffer due to a recent change");
}
}
if (sLatch->Get() != pullBackSwitchPatternBuffer.lastRequestedState) {
pullBackSwitchPatternBuffer.solenoidChangeTime = getCurrentMillis();
pullBackSwitchPatternBuffer.lastFallingEdge = -1;
pullBackSwitchPatternBuffer.lastRisingEdge = -1;
pullBackSwitchPatternBuffer.lastRequestedState = sLatch->Get();
if (sLatch->Get() == Shooter::kLatched) {
pullBackSwitchPatternBuffer.lastState = true;
} else {
pullBackSwitchPatternBuffer.lastState = false;
}
// Logger::log(Logger::kFinest, "Shooter",
// "Cleared proximity pattern buffer internally");
}
if (state && pullBackSwitchPatternBuffer.lastRisingEdge<0) {
pullBackSwitchPatternBuffer.lastRisingEdge = getCurrentMillis();
}
return state;
}
void MessageProcessor::cleanAckedMessages() {
map<unsigned int, map<unsigned long, MessageContainer> >::iterator it = sentMessages.begin();
map<unsigned long, MessageContainer>::iterator it2;
while (it != sentMessages.end()) {
it2 = it->second.begin();
while (it2 != it->second.end()) {
if (it2->second.getAcked()) {
if ((getCurrentMillis() - it2->second.getTimeAcked()) > 1000) {
if (outputLog)
(*outputLog) << "Removing id " << it2->second.getMessage()->id << " from the acked record" << endl;
it->second.erase(it2++);
}else
it2++;
}else
it2++;
}
if (it->second.size() == 0)
sentMessages.erase(it++);
else
it++;
}
map<unsigned long, map<unsigned int, unsigned long long> >::iterator it3 = ackedMessages.begin();
map<unsigned int, unsigned long long>::iterator it4;
// somehow want to delete the inner map once that player logs out
while (it3 != ackedMessages.end()) {
it4 = it3->second.begin();
while (it4 != it3->second.end()) {
if ((getCurrentMillis() - it4->second) > 500)
it3->second.erase(it4++);
else
it4++;
}
it3++;
}
}
Shooter::Shooter() :
Subsystem("Shooter") {
wenchPot = new AnalogPot(SHOOTER_CAT_POT);
wenchPot->setVoltageToAngle(SHOOTER_POT_TO_DRAW_COEFF);
wenchMotor = new StallableMotor(new Talon(SHOOTER_MOTOR_WENCH),
SHOOTER_MOTOR_STALL_SPEED, SHOOTER_MOTOR_STALL_TIME, 7500);
wenchMotor->setPotSource(wenchPot);
wenchMotor->setName("Winch Motor");
LiveWindow::GetInstance()->AddActuator("Shooter", "Wench Motor", new DualLiveSpeed(wenchMotor));
LiveWindow::GetInstance()->AddSensor("Shooter", "Wench Potentiometer", wenchPot);
wLatch = new SolenoidPair(SHOOTER_PNEUMATIC_W_LATCH);
LiveWindow::GetInstance()->AddActuator("Shooter", "Wench Latch", wLatch);
sLatch = new SolenoidPair(SHOOTER_PNEUMATIC_S_LATCH);
LiveWindow::GetInstance()->AddActuator("Shooter", "Shooter Latch", sLatch);
#if SHOOTER_LIMITSWITCH
pullBackSwitchLeft = new DigitalInput(
SHOOTER_LIMITSWITCH_LEFT_PULLBACK_CHECK);
pullBackSwitchRight = new DigitalInput(
SHOOTER_LIMITSWITCH_RIGHT_PULLBACK_CHECK);
#endif
sLatchSensor = new DigitalInput(SHOOTER_S_LATCH_SENSOR);
LiveWindow::GetInstance()->AddSensor("Shooter", "Shooter Latch Sensor", sLatchSensor);
wLatchSensor = new DigitalInput(SHOOTER_W_LATCH_SENSOR);
LiveWindow::GetInstance()->AddSensor("Shooter", "Winch Latch Sensor", wLatchSensor);
// Icky Icky. This code is repeated
sLatchPatternBuffer.lastState = !sLatchSensor->Get() ? Shooter::kLatched
: Shooter::kUnlatched;
pullBackSwitchPatternBuffer.lastRequestedState = !pullBackSwitchLeft->Get()
|| !pullBackSwitchRight->Get();
pullBackSwitchPatternBuffer.lastState
= !pullBackSwitchPatternBuffer.lastRequestedState;
if (getRawProximity()) {
pullBackSwitchPatternBuffer.lastRisingEdge = getCurrentMillis();
}
lastReleasePosition = 0;
setSLatch(Shooter::kLatched);
wenchMotor->Set(0);
shootDelay = 0.4;
}
/**
* This program asks the user to enter an infix formula, translates it to
* an Abstract Syntax tree using the function:
*
* ASTNode_t *SBML_parseFormula(char *)
*
* evaluates the formula and returns the result. See comments for
* double evalAST(ASTNode_t *n) for further information.
*/
int
main()
{
char *line;
unsigned long long start, stop;
ASTNode_t *n;
double result;
printf( "\n" );
printf( "This program evaluates math formulas in infix notation.\n" );
printf( "Typing 'enter' triggers evaluation.\n" );
printf( "Typing 'enter' on an empty line stops the program.\n" );
printf( "\n" );
while (1)
{
printf( "Enter an infix formula (empty line to quit):\n\n" );
printf( "> " );
if ( strlen(line = trim_whitespace(get_line( stdin ))) == 0 ) break;
n = SBML_parseFormula(line);
start = getCurrentMillis();
result = evalAST(n);
stop = getCurrentMillis();
printf("\n%s\n= %.10g\n\n", SBML_formulaToString(n), result);
printf("evaluation time: %llu ms\n\n", stop - start);
free(line);
ASTNode_free(n);
}
return 0;
}
void Shooter::setSLatch(Shooter::LatchPosition state) {
sLatch->Set(state);
if (state == Shooter::kUnlatched) {
#if !SHOOTER_FANCY_LETOUT
lastReleasePosition = 0.0;
#endif
}
int sLatchState = state;
if (sLatchState != sLatchPatternBuffer.lastRequestedState) {
sLatchPatternBuffer.solenoidChangeTime = getCurrentMillis();
sLatchPatternBuffer.lastFallingEdge = -1;
sLatchPatternBuffer.lastRisingEdge = -1;
sLatchPatternBuffer.lastRequestedState = sLatchState;
Logger::log(Logger::kFinest, "Shooter",
"Cleared shooter latch pattern buffer internally");
}
}
bool BeagleBone::isGyroActive() {
return lastGyroUpdate + 5000 > getCurrentMillis();
}
bool Shooter::isLatchedByProximity() {
bool state = getRawProximity();
return state && pullBackSwitchPatternBuffer.lastRisingEdge >= 0
&& pullBackSwitchPatternBuffer.lastRisingEdge
+ SHOOTER_LIMITSWITCH_DELAY < getCurrentMillis();
}
int main (int argc, char* argv[])
{
char parent_process_path_buf[128];
sprintf(parent_process_path_buf, "/proc/%d/cmdline", getppid());
std::ifstream parent_cmdline;
parent_cmdline.open(parent_process_path_buf);
std::string parent_cmdline_str;
std::getline(parent_cmdline, parent_cmdline_str);
parent_cmdline.close();
std::cout <<(char *)getauxval(AT_EXECFN) << " pid " << getpid() << " Parent PID is " << getppid() << " " <<parent_cmdline_str << " " <<currentDateTime() << std::endl;
usleep(1000000* (rand()%10+1));
std::cout <<get_process_name(getpid()) << " pid " << getpid() << " Parent PID is " << getppid() << " " <<parent_cmdline_str << " " <<currentDateTime() << std::endl;
std::cout << "Resource size is " << (size_t)&_binary_main_cpp_size<<std::endl;
std::string src(&_binary_main_cpp_start,(size_t)( &_binary_main_cpp_end-&_binary_main_cpp_start));
ACE_Argv_Type_Converter to_tchar (argc, argv);
ACE_Get_Opt get_opt (argc,
to_tchar.get_TCHAR_argv (),
ACE_TEXT ("msad:"), 1, 0);
//ACE_Get_Opt get_opt (argc,argv, ACE_TEXT ("P"), 1, 0);
for (int c; (c = get_opt ()) != -1; )
switch (c)
{
case 's': // print source code
std::cout <<src << std::endl;
break;
case 'm': // print free memory information
{
std::cout << "Parent process is " << get_process_name(getppid()) << std::endl;
std::string ss=runcmd("free -m");
std::cout << ss <<std::endl;
}
break;
case 'a': // test ACE_Argv_Type_Converter
{
ACE_ARGV_T<char> argslist;
argslist.add("abc def ghi");
argslist.add("4th");
argslist.add("5th");
int newargc=argslist.argc();
ACE_Argv_Type_Converter cmdline(newargc, argslist.argv());
char** newargs=cmdline.get_ASCII_argv();
std::cout << "arg count " << argslist.argc()<<std::endl;
for(int i=0; i<newargc;i++)
std::cout<< "argv["<<i<<"] " << newargs[i]<<std::endl;
}
break;
case 'd': // compare performance of itoa and sprintf
{
int cnt = ACE_OS::atoi(get_opt.opt_arg ());
uint64_t perf1start= getCurrentMillis();
char buf[16];
for(int i=0; i<cnt; i++)
itoa(i, buf, 10);
uint64_t perf2start= getCurrentMillis();
for(int i=0; i<cnt; i++)
sprintf(buf, "%d", i);
uint64_t perf2stop= getCurrentMillis();
std::cout << "convert "<<cnt<<" int to string by itoa in "
<< (perf2start-perf1start) << " milliseconds, sprintf in "
<< (perf2stop-perf2start) << " milliseconds\n";
}
break;
default:
break;
}
return 0;
}
