int main(int argc, char *argv[])
{
int id = 0;
int seed_rd_fd = STDIN_FILENO;
int score_wr_fd = STDOUT_FILENO;
if (argc == 2)
id = strtol(argv[1], NULL, 10);
shooter(id, seed_rd_fd, score_wr_fd);
return 0;
}
int main(int argc, char *argv[])
{
int id = 0;
int seed_rd_fd = STDIN_FILENO;
int score_wr_fd = STDOUT_FILENO;
if (argc == 2){
id = strtol(argv[1], NULL, 10);
}
shooter(id, seed_rd_fd, score_wr_fd);
puts("Shooter exiting properly.");
return 0;
}
/*
* Runs the user autonomous code. This function will be started in its own task with the default
* priority and stack size whenever the robot is enabled via the Field Management System or the
* VEX Competition Switch in the autonomous mode. If the robot is disabled or communications is
* lost, the autonomous task will be stopped by the kernel. Re-enabling the robot will restart
* the task, not re-start it from where it left off.
*
* Code running in the autonomous task cannot access information from the VEX Joystick. However,
* the autonomous function can be invoked from another task if a VEX Competition Switch is not
* available, and it can access joystick information if called in this way.
*
* The autonomous task may exit, unlike operatorControl() which should never exit. If it does
* so, the robot will await a switch to another mode or disable/enable cycle.
*/
void autonomous() {
//lfilterClear();
int8_t shooterSpeed = shooterSpeedPresets[2];
int8_t n = 0;
while (true) {
if (abs(motorGet(SHOOTER_MOTOR_CHANNEL)) == shooterSpeed && abs(motorGet(SHOOTER_MOTOR_CHANNEL2)) == shooterSpeed) {
if (n < 75) {
++n;
} else {
lifter(60);
takeInInternal(60);
}
}
shooter(shooterSpeed);
delay(20);
}
}
task usercontrol()
{
startTask(pid);
clearDebugStream();
while (true)
{
//++++++++++++++++++++Shooter++++++++++++++++++++
if (vexRT[Btn5DXmtr2]){
if (vexRT[Btn8UXmtr2]) targetRPM = full;
else if (vexRT[Btn8LXmtr2])targetRPM = skills;
else if (vexRT[Btn8RXmtr2])targetRPM = half;
else if (vexRT[Btn8DXmtr2])targetRPM = close;
}
else targetRPM = 0;
if (vexRT[Btn6DXmtr2]) targetRPM = 0;
//pid overwrite
if (vexRT[Btn6UXmtr2]) shooter(vexRT[Ch2Xmtr2]);
//------------------End Shooter------------------
//+++++++++++++++++++++Drive+++++++++++++++++++++
if (vexRT[Btn5U]) drive(vexRT[Ch2]/2+vexRT[Ch1]/2,vexRT[Ch2]/2-vexRT[Ch1]/2);
else if (vexRT[Btn5D]) drive(vexRT[Ch2]/4+vexRT[Ch1]/4,vexRT[Ch2]/4-vexRT[Ch1]/4);
else drive(vexRT[Ch2]+vexRT[Ch1], vexRT[Ch2]-vexRT[Ch1]);
/*tank drive
runLeft(vexRT[Ch3]);
runRight(vexRT[Ch2]);
*/
//--------------------End Drive--------------------
//++++++++++++++++++++++Intake++++++++++++++++++++++
if (vexRT[Btn6U]) intake(127);
else if (vexRT[Btn6D]) intake(-127);
else intake(0);
//--------------------End Intake--------------------
//++++++++++++++++++++Solenoids+++++++++++++++++++++
//------------------End Solenoids------------------
}//while
}//task
/* User Control */
task usercontrol()
{
initMotor(&motorLeftShooter, leftShooter1, leftShooter2, leftShooterSensor);
initMotor(&motorRightShooter, rightShooter1, rightShooter2, rightShooterSensor);
clearAll(actOnSensors);
clearTimer(T1);
clearTimer(T2);
while(true)
{
drive();
intake();
shooter(&motorLeftShooter, &motorRightShooter);
velocidade_motor_esquerdo=motorLeftShooter.speedRead;
potencia_motor_esquerdo=motorLeftShooter.controller_output;
velocidade_motor_direito=motorRightShooter.speedRead;
potencia_motor_direito=motorRightShooter.controller_output;
}
}
task main()
{
clearTimer(T1);
while(true)
{
drive();
intake();
shooter();
timenow = time1[T1];
if(timenow>=30){
encoderr = SensorValue[rightShooterSensor];
speedReadr = abs((1000.0*(encoderr - lastr))/timenow); //that is not a unit. You should multiply for a constant. I will leave this way by now.
lastr = encoderr;
checktime=timenow;
clearTimer(T1);
}
}
}
void App::create_shooter( const InputEvent &key, const std::string &str, bool use_red, bool use_green, bool use_blue)
{
// Set the firing line
Vec3f vector(0.0f, 0.0f, 20.0f);
float width = (float) canvas.get_width();
float height = (float) canvas.get_height();
vector.x = ((key.mouse_pos.x - width/2)* 10.0f) / width;
vector.y = -((key.mouse_pos.y - height/2) * 10.0f) / height;
// Create the text offset
TextShooter shooter(vector_font, str);
shooter.set_start_time(System::get_time());
shooter.set_duration(2 * 1000);
shooter.set_initial_white_time(1000);
shooter.set_end_fade_time(1000);
shooter.set_start_position(Vec3f(0.0f, 0.0f, 0.2f));
shooter.set_end_position(vector);
shooter.set_color_component(use_red, use_green, use_blue);
text_shooter.push_back(shooter);
}
task pid(){ //PID task
while(true){
if (!vexRT[Btn6UXmtr2]){ //pid overwrite
//++++++++++++++++++++Current RPM++++++++++++++++++++
val = SensorValue[enSh];
diff = val - lastVal;
currentRPM = diff*10*60*25/360;
//-----------------End Current RPM ------------------
//write to debug stream
if (debug) writeDebugStreamLine("Current RPM: %d", currentRPM);
if (debug) writeDebugStreamLine("Tagrget RPM: %d", targetRPM);
//get error
error = targetRPM - currentRPM;
if (debug) writeDebugStreamLine("Error: %d", error);
//sumError = sumError + error;
//p calculations
pChange = error * pVal;
if (debug) writeDebugStreamLine("P Change: %d\n", pChange);
//i calculations
//iChange = sumError * iVal;
//d calculations
//slope = error - lastError;
//dChange = slope * dVal;
//total pid changes
tChange = pChange; //+ iChange + dChange;
//make sure motor doesnt run backwards
if(tChange<0) tChange = 0;
//update motor
shooter(tChange);
//lastError = error;
lastVal = val;
wait1Msec(100);
//led
if (vexRT[Btn5DXmtr2]){
if (currentRPM >= targetRPM)speedLed(1);
else speedLed(0);
}
else speedLed(0);
//speed leds
if (targetRPM == full) ledRGY(0,1,0);
else if(targetRPM == half) ledRGY(0,0,1);
else if (targetRPM == close) ledRGY(1,0,0);
else if(targetRPM == skills) ledRGY(1,1,1);
else ledRGY(0,0,0);
}//over write if
}//while loop
}//task
/*
* Runs the user operator control code. This function will be started in its own task with the
* default priority and stack size whenever the robot is enabled via the Field Management System
* or the VEX Competition Switch in the operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
#ifdef AUTO
autonomous();
#elif defined(TEST)
#define DEFAULT_SHOOTER_SPEED 0
#define SHOOTER_SPEED_INCREMENT 5
int8_t xSpeed, ySpeed, rotation;
int8_t lifterSpeed/*, intakeSpeed*/;
int16_t shooterSpeed = DEFAULT_SHOOTER_SPEED; //shooter is on when robot starts
int8_t frontIntakeSpeed = INTAKE_SPEED;
bool isShooterOn = true;
bool isAutoShootOn = false;
//lfilterClear();
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP); // intake forward
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT); // intake off
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT); // intake off
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN); // intake backward
toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN); // shooter on off
toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_RIGHT); // auto shoot on off
toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP); // shooter speed up
toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN); // shooter speed down
while (true) {
printf("ultra distance (in): %f\r\n", ultrasonicGet(ultra) / 2.54);
toggleBtnUpdateAll();
// drive
xSpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, STRAFE_AXIS);
ySpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, DRIVE_AXIS);
rotation = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, ROTATION_AXIS) / 2;
if (abs(ySpeed) < DIAGONAL_DRIVE_DEADBAND) {
ySpeed = 0;
}
if (abs(xSpeed) < DIAGONAL_DRIVE_DEADBAND) {
xSpeed = 0;
}
drive(xSpeed, ySpeed, rotation, false);
// lifter up down
if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_UP)) {
lifterSpeed = LIFTER_SPEED;
} else if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_DOWN)) {
lifterSpeed = -LIFTER_SPEED;
} else {
lifterSpeed = 0;
}
lifter(lifterSpeed);
takeInInternal(lifterSpeed);
if (isShooterOn) {
if (isAutoShootOn) {
shooterSpeed = calculateShooterSpeed();
} else {
// shooter increase speed
if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
shooterSpeed += SHOOTER_SPEED_INCREMENT;
if (shooterSpeed > SHOOTER_MAX_SPEED) {
shooterSpeed = SHOOTER_MAX_SPEED;
}
}
// shooter decrease speed
if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
shooterSpeed -= SHOOTER_SPEED_INCREMENT;
if (shooterSpeed < SHOOTER_MIN_SPEED) {
shooterSpeed = SHOOTER_MIN_SPEED;
}
}
}
// auto shooter on off
if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) {
isAutoShootOn = !isAutoShootOn;
}
}
// shooter on off
if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
isShooterOn = !isShooterOn;
shooterSpeed = isShooterOn ? DEFAULT_SHOOTER_SPEED : 0;
}
shooter(shooterSpeed);
// intake mode
if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT) == BUTTON_PRESSED
|| toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) {
frontIntakeSpeed = 0;
} else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
frontIntakeSpeed = -INTAKE_SPEED;
} else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
frontIntakeSpeed = INTAKE_SPEED;
}
takeInFront(frontIntakeSpeed);
delay(20);
}
#else
int8_t xSpeed, ySpeed, rotation;
int8_t lifterSpeed/*, intakeSpeed*/;
int8_t defaultPreset = 0;
int8_t currentPreset = defaultPreset;
int16_t shooterSpeed = shooterSpeedPresets[defaultPreset]; //shooter is on when robot starts
int8_t frontIntakeSpeed = INTAKE_SPEED;
bool isShooterOn = true;
//lfilterClear();
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP); // intake forward
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT); // intake off
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT); // intake off
toggleBtnInit(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN); // intake backward
toggleBtnInit(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN); // shooter on off
toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP); // shooter speed up
toggleBtnInit(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN); // shooter speed down
while (true) {
printf("ultra distance (in): %f\r\n", ultrasonicGet(ultra) / 2.54);
toggleBtnUpdateAll();
// drive
xSpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, STRAFE_AXIS);
ySpeed = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, DRIVE_AXIS);
rotation = (int8_t) joystickGetAnalog(JOYSTICK_SLOT, ROTATION_AXIS) / 2;
if (abs(ySpeed) < DIAGONAL_DRIVE_DEADBAND) {
ySpeed = 0;
}
if (abs(xSpeed) < DIAGONAL_DRIVE_DEADBAND) {
xSpeed = 0;
}
drive(xSpeed, ySpeed, rotation, false);
// lifter up down
if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_UP)) {
lifterSpeed = LIFTER_SPEED;
} else if (joystickGetDigital(JOYSTICK_SLOT, LIFTER_BUTTON_GROUP, JOY_DOWN)) {
lifterSpeed = -LIFTER_SPEED;
} else {
lifterSpeed = 0;
}
lifter(lifterSpeed);
takeInInternal(lifterSpeed);
if (isShooterOn) {
// shooter increase speed
if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
++currentPreset;
if (currentPreset >= NUM_SHOOTER_SPEED_PRESETS) {
currentPreset = NUM_SHOOTER_SPEED_PRESETS - 1;
}
}
// shooter decrease speed
if (toggleBtnGet(JOYSTICK_SLOT, SHOOTER_ADJUST_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
--currentPreset;
if (currentPreset < 0) {
currentPreset = 0;
}
}
shooterSpeed = shooterSpeedPresets[currentPreset];
}
// shooter on off
if (toggleBtnGet(JOYSTICK_SLOT, CONTROL_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
isShooterOn = !isShooterOn;
shooterSpeed = isShooterOn ? shooterSpeedPresets[defaultPreset] : 0;
}
shooter(shooterSpeed);
// intake mode
if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_LEFT) == BUTTON_PRESSED
|| toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_RIGHT) == BUTTON_PRESSED) {
frontIntakeSpeed = 0;
} else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_UP) == BUTTON_PRESSED) {
frontIntakeSpeed = -INTAKE_SPEED;
} else if (toggleBtnGet(JOYSTICK_SLOT, INTAKE_BUTTON_GROUP, JOY_DOWN) == BUTTON_PRESSED) {
frontIntakeSpeed = INTAKE_SPEED;
}
takeInFront(frontIntakeSpeed);
delay(20);
}
#endif
}
int main()
{
srand(time(0));
//variables
const int width = 700;
const int height = 600;
int points = 0;
float velEnemy = 0.2;
//font
sf::Font font;
if (!font.loadFromFile("Data/avaria.ttf"))
return 0;
//sound
sf::SoundBuffer exploImage;
if (!exploImage.loadFromFile("Data/explosion.wav"))
return 0;
sf::Sound destroy;
destroy.setBuffer(exploImage);
//enemy image
sf::Texture alienImage;
if (!alienImage.loadFromFile("Data/alien.png"))
return 0;
//enemy dead
sf::Texture alienDead;
if (!alienDead.loadFromFile("Data/alienExplo.png"))
return EXIT_FAILURE;
//ship image
sf::Texture texture;
if (!texture.loadFromFile("Data/ship.png"))
return EXIT_FAILURE;
sf::RectangleShape alien;
alien.setTexture(&alienImage);
alien.setSize(sf::Vector2f(50, 50));
alien.setPosition(150, 100);
//bullet image
sf::Texture bulletImage;
if (!bulletImage.loadFromFile("Data/bullet.png"))
return EXIT_FAILURE;
//score
sf::Text score;
score.setFont(font);
score.setPosition(10, height - 50);
score.setString("Points : 0");
//end score
sf::RenderWindow window(sf::VideoMode(width, height), "X1");
Player player1(width/2, height-50, texture);
shoot shooter(bulletImage);
shoot alienShoot(bulletImage);
while (window.isOpen())
{
sf::Event event;
while (window.pollEvent(event)) {
srand(time(nullptr));
if (event.type == sf::Event::Closed)
window.close();
if (event.key.code == sf::Keyboard::Escape)
window.close();
}
//logic alien move
if (alien.getPosition().x > width - 50)
velEnemy = -0.2;
if (alien.getPosition().x < 50)
velEnemy = 0.2;
//end logic
window.clear(sf::Color::Black);
std::stringstream text;
bool go = false;
text << "Player : " << points;
score.setString(text.str());
window.draw(score);
//logic for alien
alien.setTexture(&alienImage);
if (alien.getGlobalBounds().intersects(shooter.bullet.getGlobalBounds())) {
alien.setPosition(1 + rand() % width, 100);
points++;
destroy.play();
alien.setTexture(&alienDead);
Sleep(20);
}
//set player limits
if (player1.rect.getPosition().x < 0)
player1.rect.setPosition(0, player1.rect.getPosition().y);
if (player1.rect.getPosition().x > width - 50)
player1.rect.setPosition(width - 50, player1.rect.getPosition().y);
alien.move(velEnemy, 0);
window.draw(alien);
if (player1.update())
{
shooter.update(player1.rect.getPosition().x, player1.rect.getPosition().y, -3);
window.draw(shooter.bullet);
}
window.draw(player1.rect);
window.display();
}
return 0;
}
void RandomInlet::run(){
DemField* dem=static_cast<DemField*>(field.get());
if(!generator) throw std::runtime_error("RandomInlet.generator==None!");
if(materials.empty()) throw std::runtime_error("RandomInlet.materials is empty!");
if(collideExisting){
if(!collider){
for(const auto& e: scene->engines){ collider=dynamic_pointer_cast<Collider>(e); if(collider) break; }
if(!collider) throw std::runtime_error("RandomInlet: no Collider found within engines (needed for collisions detection with already existing particles; if you don't need that, set collideExisting=False.)");
}
if(dynamic_pointer_cast<InsertionSortCollider>(collider)) static_pointer_cast<InsertionSortCollider>(collider)->forceInitSort=true;
}
if(isnan(massRate)) throw std::runtime_error("RandomInlet.massRate must be given (is "+to_string(massRate)+"); if you want to generate as many particles as possible, say massRate=0.");
if(massRate<=0 && maxAttempts==0) throw std::runtime_error("RandomInlet.massFlowRate<=0 (no massFlowRate prescribed), but RandomInlet.maxAttempts==0. (unlimited number of attempts); this would cause infinite loop.");
if(maxAttempts<0){
std::runtime_error("RandomInlet.maxAttempts must be non-negative. Negative value, leading to meaking engine dead, is achieved by setting atMaxAttempts=RandomInlet.maxAttDead now.");
}
spheresOnly=generator->isSpheresOnly();
padDist=generator->padDist();
if(isnan(padDist) || padDist<0) throw std::runtime_error(generator->pyStr()+".padDist(): returned invalid value "+to_string(padDist));
// as if some time has already elapsed at the very start
// otherwise mass flow rate is too big since one particle during Δt exceeds it easily
// equivalent to not running the first time, but without time waste
if(stepPrev==-1 && stepPeriod>0) stepPrev=-stepPeriod;
long nSteps=scene->step-stepPrev;
// to be attained in this step;
stepGoalMass+=massRate*scene->dt*nSteps; // stepLast==-1 if never run, which is OK
vector<AlignedBox3r> genBoxes; // of particles created in this step
vector<shared_ptr<Particle>> generated;
Real stepMass=0.;
SpherePack spheres;
//
if(spheresOnly){
spheres.pack.reserve(dem->particles->size()*1.2); // something extra for generated particles
// HACK!!!
bool isBox=dynamic_cast<BoxInlet*>(this);
AlignedBox3r box;
if(isBox){ box=this->cast<BoxInlet>().box; }
for(const auto& p: *dem->particles){
if(p->shape->isA<Sphere>() && (!isBox || box.contains(p->shape->nodes[0]->pos))) spheres.pack.push_back(SpherePack::Sph(p->shape->nodes[0]->pos,p->shape->cast<Sphere>().radius));
}
}
while(true){
// finished forever
if(everythingDone()) return;
// finished in this step
if(massRate>0 && mass>=stepGoalMass) break;
shared_ptr<Material> mat;
Vector3r pos=Vector3r::Zero();
Real diam;
vector<ParticleGenerator::ParticleAndBox> pee;
int attempt=-1;
while(true){
attempt++;
/***** too many tries, give up ******/
if(attempt>=maxAttempts){
generator->revokeLast(); // last particle could not be placed
if(massRate<=0){
LOG_DEBUG("maxAttempts="<<maxAttempts<<" reached; since massRate is not positive, we're done in this step");
goto stepDone;
}
switch(atMaxAttempts){
case MAXATT_ERROR: throw std::runtime_error("RandomInlet.maxAttempts reached ("+lexical_cast<string>(maxAttempts)+")"); break;
case MAXATT_DEAD:{
LOG_INFO("maxAttempts="<<maxAttempts<<" reached, making myself dead.");
this->dead=true;
return;
}
case MAXATT_WARN: LOG_WARN("maxAttempts "<<maxAttempts<<" reached before required mass amount was generated; continuing, since maxAttemptsError==False"); break;
case MAXATT_SILENT: break;
default: throw std::invalid_argument("Invalid value of RandomInlet.atMaxAttempts="+to_string(atMaxAttempts)+".");
}
}
/***** each maxAttempts/attPerPar, try a new particles *****/
if((attempt%(maxAttempts/attemptPar))==0){
LOG_DEBUG("attempt "<<attempt<<": trying with a new particle.");
if(attempt>0) generator->revokeLast(); // if not at the beginning, revoke the last particle
// random choice of material with equal probability
if(materials.size()==1) mat=materials[0];
else{
size_t i=max(size_t(materials.size()*Mathr::UnitRandom()),materials.size()-1);;
mat=materials[i];
}
// generate a new particle
std::tie(diam,pee)=(*generator)(mat,scene->time);
assert(!pee.empty());
LOG_TRACE("Placing "<<pee.size()<<"-sized particle; first component is a "<<pee[0].par->getClassName()<<", extents from "<<pee[0].extents.min().transpose()<<" to "<<pee[0].extents.max().transpose());
}
pos=randomPosition(diam,padDist); // overridden in child classes
LOG_TRACE("Trying pos="<<pos.transpose());
for(const auto& pe: pee){
// make translated copy
AlignedBox3r peBox(pe.extents); peBox.translate(pos);
// box is not entirely within the factory shape
if(!validateBox(peBox)){ LOG_TRACE("validateBox failed."); goto tryAgain; }
const shared_ptr<woo::Sphere>& peSphere=dynamic_pointer_cast<woo::Sphere>(pe.par->shape);
if(spheresOnly){
if(!peSphere) throw std::runtime_error("RandomInlet.spheresOnly: is true, but a nonspherical particle ("+pe.par->shape->pyStr()+") was returned by the generator.");
Real r=peSphere->radius;
Vector3r subPos=peSphere->nodes[0]->pos;
for(const auto& s: spheres.pack){
// check dist && don't collide with another sphere from this clump
// (abuses the *num* counter for clumpId)
if((s.c-(pos+subPos)).squaredNorm()<pow(s.r+r,2)){
LOG_TRACE("Collision with a particle in SpherePack (a particle generated in this step).");
goto tryAgain;
}
}
// don't add to spheres until all particles will have been checked for overlaps (below)
} else {
// see intersection with existing particles
bool overlap=false;
if(collideExisting){
vector<Particle::id_t> ids=collider->probeAabb(peBox.min(),peBox.max());
for(const auto& id: ids){
LOG_TRACE("Collider reports intersection with #"<<id);
if(id>(Particle::id_t)dem->particles->size() || !(*dem->particles)[id]) continue;
const shared_ptr<Shape>& sh2((*dem->particles)[id]->shape);
// no spheres, or they are too close
if(!peSphere || !sh2->isA<woo::Sphere>() || 1.1*(pos-sh2->nodes[0]->pos).squaredNorm()<pow(peSphere->radius+sh2->cast<Sphere>().radius,2)) goto tryAgain;
}
}
// intersection with particles generated by ourselves in this step
for(size_t i=0; i<genBoxes.size(); i++){
overlap=(genBoxes[i].squaredExteriorDistance(peBox)==0.);
if(overlap){
const auto& genSh(generated[i]->shape);
// for spheres, try to compute whether they really touch
if(!peSphere || !genSh->isA<Sphere>() || (pos-genSh->nodes[0]->pos).squaredNorm()<pow(peSphere->radius+genSh->cast<Sphere>().radius,2)){
LOG_TRACE("Collision with "<<i<<"-th particle generated in this step.");
goto tryAgain;
}
}
}
}
}
LOG_DEBUG("No collision (attempt "<<attempt<<"), particle will be created :-) ");
if(spheresOnly){
// num will be the same for all spheres within this clump (abuse the *num* counter)
for(const auto& pe: pee){
Vector3r subPos=pe.par->shape->nodes[0]->pos;
Real r=pe.par->shape->cast<Sphere>().radius;
spheres.pack.push_back(SpherePack::Sph((pos+subPos),r,/*clumpId*/(pee.size()==1?-1:num)));
}
}
break;
tryAgain: ; // try to position the same particle again
}
// particle was generated successfully and we have place for it
for(const auto& pe: pee){
genBoxes.push_back(AlignedBox3r(pe.extents).translate(pos));
generated.push_back(pe.par);
}
num+=1;
#ifdef WOO_OPENGL
Real color_=isnan(color)?Mathr::UnitRandom():color;
#endif
if(pee.size()>1){ // clump was generated
//LOG_WARN("Clumps not yet tested properly.");
vector<shared_ptr<Node>> nn;
for(auto& pe: pee){
auto& p=pe.par;
p->mask=mask;
#ifdef WOO_OPENGL
assert(p->shape);
if(color_>=0) p->shape->color=color_; // otherwise keep generator-assigned color
#endif
if(p->shape->nodes.size()!=1) LOG_WARN("Adding suspicious clump containing particle with more than one node (please check, this is perhaps not tested");
for(const auto& n: p->shape->nodes){
nn.push_back(n);
n->pos+=pos;
}
dem->particles->insert(p);
}
shared_ptr<Node> clump=ClumpData::makeClump(nn,/*no central node pre-given*/shared_ptr<Node>(),/*intersection*/false);
auto& dyn=clump->getData<DemData>();
if(shooter) (*shooter)(clump);
if(scene->trackEnergy) scene->energy->add(-DemData::getEk_any(clump,true,true,scene),"kinInlet",kinEnergyIx,EnergyTracker::ZeroDontCreate);
if(dyn.angVel!=Vector3r::Zero()){
throw std::runtime_error("pkg/dem/RandomInlet.cpp: generated particle has non-zero angular velocity; angular momentum should be computed so that rotation integration is correct, but it was not yet implemented.");
// TODO: compute initial angular momentum, since we will (very likely) use the aspherical integrator
}
ClumpData::applyToMembers(clump,/*reset*/false); // apply velocity
#ifdef WOO_OPENGL
boost::mutex::scoped_lock lock(dem->nodesMutex);
#endif
dyn.linIx=dem->nodes.size();
dem->nodes.push_back(clump);
mass+=clump->getData<DemData>().mass;
stepMass+=clump->getData<DemData>().mass;
} else {
auto& p=pee[0].par;
p->mask=mask;
assert(p->shape);
#ifdef WOO_OPENGL
if(color_>=0) p->shape->color=color_;
#endif
assert(p->shape->nodes.size()==1); // if this fails, enable the block below
const auto& node0=p->shape->nodes[0];
assert(node0->pos==Vector3r::Zero());
node0->pos+=pos;
auto& dyn=node0->getData<DemData>();
if(shooter) (*shooter)(node0);
if(scene->trackEnergy) scene->energy->add(-DemData::getEk_any(node0,true,true,scene),"kinInlet",kinEnergyIx,EnergyTracker::ZeroDontCreate);
mass+=dyn.mass;
stepMass+=dyn.mass;
assert(node0->hasData<DemData>());
dem->particles->insert(p);
#ifdef WOO_OPENGL
boost::mutex::scoped_lock lock(dem->nodesMutex);
#endif
dyn.linIx=dem->nodes.size();
dem->nodes.push_back(node0);
// handle multi-nodal particle (unused now)
#if 0
// TODO: track energy of the shooter
// in case the particle is multinodal, apply the same to all nodes
if(shooter) shooter(p.shape->nodes[0]);
const Vector3r& vel(p->shape->nodes[0]->getData<DemData>().vel); const Vector3r& angVel(p->shape->nodes[0]->getData<DemData>().angVel);
for(const auto& n: p.shape->nodes){
auto& dyn=n->getData<DemData>();
dyn.vel=vel; dyn.angVel=angVel;
mass+=dyn.mass;
n->linIx=dem->nodes.size();
dem->nodes.push_back(n);
}
#endif
}
};
stepDone:
setCurrRate(stepMass/(nSteps*scene->dt));
}
void MyTeleop::OperatorControl(void)
{
bool shootEnabled = false;
float magnitude = 0, direction = 0, rotation = 0;
MyDiscShooter shooter(myRobot);
MyClimber climber(myRobot);
JoystickButton leftTopButton(&myRobot->leftStick, Joystick::kDefaultTopButton);
JoystickButton leftTrigger(&myRobot->leftStick, Joystick::kDefaultTriggerButton);
//MyDiscShooterCmd *discShooterCmd;
LCD::ConsoleLog("MyTeleop.OperatorControl");
LCD::PrintLine(1, "Mode: OperatorControl");
LCD::PrintLine(4, "All: %d, Start=%d", myRobot->alliance, myRobot->startLocation);
// Sets command based disc shooter class to run when button pressed, as separate
// task from this one in the Command execution scheme. This depends on the command
// scheduler running and that is done below in the While loop.
// button class whenpressed requires a pointer to the command class instance we
// want to run so we use new to create the instance and return a pointer to
// that instance.
//discShooterCmd = new MyDiscShooterCmd(myRobot);
//leftTrigger.WhenPressed(discShooterCmd);
myRobot->robotDrive.SetSafetyEnabled(true);
// Driving loop runs until teleop is over or climbing phase started.
while (myRobot->IsEnabled() && myRobot->IsOperatorControl())
{
// Scheduler required to cause any 'commands' to run. If we don't use any commands,
// comment out following stmt.
//Scheduler::GetInstance()->Run();
// set driving parameters.
magnitude = myRobot->rightStick.GetMagnitude();
direction = myRobot->rightStick.GetDirectionDegrees();
rotation = myRobot->rotateStick.GetX();
LCD::PrintLine(3, "m=%f d=%f r=%f", magnitude, direction, rotation);
LCD::PrintLine(4, "r=%f", rotation);
myRobot->robotDrive.MecanumDrive_Polar(magnitude, direction, rotation);
// This logic shoots disk on trigger release.
if (myRobot->leftStick.GetTrigger(myRobot->leftStick.kLeftHand))
shootEnabled = true;
else if (shootEnabled)
{
shootEnabled = false;
shooter.Shoot();
}
// turn on/off climber Angular Adjustment (window) motor.
if (myRobot->leftStick.GetRawButton(JSB_TOP_LEFT))
climber.AngularAdjustmentMotorOn(true);
else
climber.AngularAdjustmentMotorOn(false);
// The design assumes operation of the climbing winches would be
// manually by JS buttons.
if (myRobot->leftStick.GetRawButton(JSB_LEFT_FRONT))
climber.Winch2Up();
else if (myRobot->leftStick.GetRawButton(JSB_LEFT_REAR))
climber.Winch2Down();
else
climber.Winch2Stop();
// control shooter ramp deployment.
if (myRobot->leftStick.GetRawButton(JSB_TOP_MIDDLE))
shooter.RampUp();
else if (myRobot->leftStick.GetRawButton(JSB_TOP_BACK))
shooter.RampDown();
else
shooter.RampStop();
// set shooter throttle value each loop.
shooter.SetThrottle(myRobot->leftStick.GetThrottle());
shooter.ShooterUpDown(myRobot->leftStick.GetY());
// Starts climbing process, drops out of driving loop when climb done.
// if (myRobot->rightStick.GetButton(myRobot->rightStick.kTopButton))
// {
// climber.Climb();
// break; // ends the While loop
// }
Wait(0.020); // wait for a motor update time
}
// wait for teleop period to end.
LCD::PrintLine(1, "Mode: End Wait");
LCD::ConsoleLog("MyTeleop.OperatorControl-endwait");
SmartDashboard::PutBoolean("End Wait", true);
myRobot->robotDrive.SetSafetyEnabled(false);
while (myRobot->IsEnabled() && myRobot->IsOperatorControl()) Wait(0.020);
// delete pointer to MyDiscShooterCmd object we created earlier.
// this releases the object instance.
//delete discShooterCmd;
LCD::ConsoleLog("MyTeleop.OperatorControl-end");
}
int main(int argc, char *argv[])
{
int i, seed;
int pids[NUM_PLAYERS];
int results[NUM_PLAYERS];
/* TODO: Use the following variables in the exec system call. Using the
* function sprintf and the arg1 variable you can pass the id parameter
* to the children
*/
/*
char arg0[] = "./shooter";
char arg1[10];
char *args[] = {arg0, arg1, NULL};
*/
/* TODO: initialize the communication with the players */
int pfd_parent_to_child[NUM_PLAYERS] [2];
int pfd_child_to_parent[NUM_PLAYERS] [2];
for (i = 0; i < NUM_PLAYERS; i++) {
pipe(pfd_child_to_parent[i]);
pipe(pfd_parent_to_child[i]);
}
for (i = 0; i < NUM_PLAYERS; i++) {
/* TODO: spawn the processes that simulate the players */
switch(pids[i] = fork()) {
case -1:
perror("could not create child process");
exit(EXIT_FAILURE);
break;
case 0:
printf("PID of child: <%ld>", (long) getpid());
shooter(i, pfd_parent_to_child[i][0], pfd_child_to_parent[i][1]);
close(pfd_parent_to_child[i][1]);
close(pfd_child_to_parent[i][0]);
for(int descIndex = 0; descIndex < NUM_PLAYERS; descIndex++) {
if (descIndex != i) {
close(pfd_parent_to_child[descIndex][0]);
close(pfd_parent_to_child[descIndex][1]);
close(pfd_child_to_parent[descIndex][0]);
close(pfd_child_to_parent[descIndex][1]);
}
}
break;
}
}
seed = time(NULL);
for (i = 0; i < NUM_PLAYERS; i++) {
seed++;
char seedString[16] = "";
sprintf(seedString, "%d", seed);
write(pfd_parent_to_child[i][1], &seed, sizeof(seed));
}
/* TODO: get the dice results from the players, find the winner */
int currentResult = 0;
int childIndex = 0;
int currentLeader = 0;
int highestScore = 0;
int pidResult;
for (i = 0; i < NUM_PLAYERS; i++) {
read(pfd_child_to_parent[i][0], ¤tResult, sizeof(int));
if(currentResult > highestScore) {
currentLeader = childIndex;
highestScore = currentResult;
}
childIndex++;
}
winner = currentLeader;
printf("master: player %d WINS\n", winner);
/* TODO: signal the winner */
kill(pids[winner],SIGUSR1);
/* TODO: signal all players the end of game */
for (i = 0; i < NUM_PLAYERS; i++) {
if(i != winner) {
kill(pids[i], SIGUSR2);
waitpid(pids[i], &pidResult, 0);
}
}
printf("master: the game ends\n");
/* TODO: cleanup resources and exit with success */
for (i = 0; i < NUM_PLAYERS; i++) {
close(pfd_parent_to_child[i][0]);
close(pfd_parent_to_child[i][1]);
close(pfd_child_to_parent[i][0]);
close(pfd_child_to_parent[i][1]);
}
return 0;
}
void MyTeleop::OperatorControl(void)
{
int encoderRaw = 0, encoderValue = 0;
bool checkBox1 = false, shootEnabled = false, arcadeDrive = false, arcadeEnabled = false;
MyDiscShooter shooter(myRobot);
MyClimber climber(myRobot);
JoystickButton topButton(&myRobot->stick, Joystick::kDefaultTopButton);
Encoder driveLeftEncoder(1, 2, false, Encoder::k4X);
int encoderDirection;
MyDiscShooterCmd *discShooterCmd;
LCD::ConsoleLog("MyTeleop.OperatorControl");
LCD::PrintLine(1, "Mode: OperatorControl");
LCD::PrintLine(4, "All: %d, Start=%d", myRobot->alliance, myRobot->startLocation);
// Sets command based disc shooter class to run when button pressed, as separate
// task from this one in the Command execution scheme. This depends on the command
// scheduler running and that is done below in the While loop.
// button class whenpressed requires a pointer to the command class instance we
// want to run so we use new to create the instance and return a pointer to
// that instance.
discShooterCmd = new MyDiscShooterCmd(myRobot);
topButton.WhenPressed(discShooterCmd);
//topButton.WhenPressed(new MyDiscShooterCmd(myRobot));
myRobot->robotDrive.SetSafetyEnabled(true);
driveLeftEncoder.Start();
// Driving loop runs until teleop is over or climbing phase started.
while (myRobot->IsEnabled() && myRobot->IsOperatorControl())
{
// Scheduler required to cause any 'commands' to run. If we don't use any commands,
// comment out following stmt.
Scheduler::GetInstance()->Run();
// the following code toggles the arcade drive flag on or off each time the right
// trigger is released.
if (myRobot->stick2.GetTrigger(myRobot->stick.kRightHand))
arcadeEnabled = true;
else if (arcadeEnabled)
//if (!myRobot->stick2.GetTrigger(myRobot->stick.kRightHand) && arcadeEnabled)
{
arcadeEnabled = false;
if (arcadeDrive)
arcadeDrive = false;
else
arcadeDrive = true;
SmartDashboard::PutBoolean("Arcade Drive", arcadeDrive);
}
if (arcadeDrive)
myRobot->robotDrive.ArcadeDrive(myRobot->stick2); // drive with arcade style (use right stick with trigger)
else
myRobot->robotDrive.TankDrive(myRobot->stick, myRobot->stick2); // drive tank style with both sticks
//LCD::ConsoleLog("left trigger=%d", myRobot->stick.GetTrigger(myRobot->stick.kLeftHand));
// This logic shoots disk on trigger release.
if (myRobot->stick.GetTrigger(myRobot->stick.kLeftHand))
shootEnabled = true;
else if (shootEnabled)
{
shootEnabled = false;
shooter.Shoot();
}
// Starts climbing process, drops out of driving loop when climb done.
if (myRobot->stick2.GetButton(myRobot->stick2.kTopButton))
{
climber.Climb();
break; // ends the While loop
}
Wait(0.020); // wait for a motor update time
// read encoder example.
encoderValue = driveLeftEncoder.Get();
encoderRaw = driveLeftEncoder.GetRaw();
if (driveLeftEncoder.GetDirection())
encoderDirection = 1; // forward
else
encoderDirection = 0; // backward
// example display info to dashboard lcd.
LCD::PrintLine(2, "Encoder Value: %d %d", encoderValue, encoderDirection);
LCD::PrintLine(3, "Encoder Raw: %d", encoderRaw);
// example on how to read a value from the dashboard.
checkBox1 = SmartDashboard::GetBoolean("Checkbox 1");
LCD::PrintLine(5, "Checkbox1 Value=%d", checkBox1);
}
// wait for teleop period to end.
LCD::PrintLine(1, "Mode: End Wait");
LCD::ConsoleLog("MyTeleop.OperatorControl-endwait");
SmartDashboard::PutBoolean("End Wait", true);
myRobot->robotDrive.SetSafetyEnabled(false);
while (myRobot->IsEnabled() && myRobot->IsOperatorControl()) Wait(0.020);
// delete pointer to MyDiscShooterCmd object we created earlier.
// this releases the object instance.
delete discShooterCmd;
LCD::ConsoleLog("MyTeleop.OperatorControl-end");
}
