void doBeaconCalibration() { // Update the robot Position (which must normally coincides with calibration Point) getOpponentRobotPosition(); Point* calibrationPoint = &(beaconSystem.calibrationPoint); OutputStream* outputStream = getOutputStreamLogger(INFO); recalibrateServoInitValue(outputStream, getLaser(LASER_INDEX_1), calibrationPoint->x, calibrationPoint->y); recalibrateServoInitValue(outputStream, getLaser(LASER_INDEX_2), getDistanceBetweenBeacon() - calibrationPoint->x, calibrationPoint->y); }
void SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) { laser_count_++; if ((laser_count_ % throttle_scans_) != 0) //1 激光频率问题 return; static ros::Time last_map_update(0,0); // Check whether we know about this laser yet // if New laser, need to create a Karto device for it. karto::LaserRangeFinder* laser = getLaser(scan); if(!laser) { ROS_WARN("Failed to create laser device for %s; discarding scan", scan->header.frame_id.c_str()); return; } if(debug_print_flag_) debugPrint_<<"1.0 laser_count_: "<<laser_count_<<" addScan "<<" "<<endl; karto::Pose2 odom_pose; if(addScan(laser, scan, odom_pose)) { ROS_DEBUG("added scan at pose: %.3f %.3f %.3f", odom_pose.GetX(), odom_pose.GetY(), odom_pose.GetHeading()); ROS_INFO("added scan at pose: %.3f %.3f %.3f", odom_pose.GetX(), odom_pose.GetY(), odom_pose.GetHeading()); geometry_msgs::Point debug_pose; debug_pose.x = odom_pose.GetX(); debug_pose.y = odom_pose.GetY(); debug_pose.z = odom_pose.GetHeading(); link_node_publisher_.publish(debug_pose); if(debug_print_flag_) debugPrint_<<"2.0 odom_pose: "<<odom_pose.GetX()<<" "<<odom_pose.GetY()<<" "<<odom_pose.GetHeading()<<endl; publishGraphVisualization(); if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_) { if(updateMap()) { last_map_update = scan->header.stamp; got_map_ = true; ROS_DEBUG("Updated the map"); if(debug_print_flag_) debugPrint_<<"3.0 updateMap: "<<endl; } } } }
Ray Monster::getLaserRay() { RectRenderable& rr = getLaser(); Vec2 pos = transform.getTransform()*rr.getLocalTransform().getTransform()*Vec2(0,0); Angle world_rot = transform.getRotation()+rr.getLocalTransform().getRotation(); //std::cout << "monster rotation: " << transform.getRotation() << std::endl; //std::cout << "laser rotation: " << rr.getLocalTransform().getRotation() << std::endl; Vec2 dir = Vec2(1.0f, 0.0f).rotate(world_rot); return Ray(pos, dir, max_laser_len); }
/** * Gets the position of the laser by triangulation of the information gived by the both laser. * If information is not correct, we give a position of (0, 0). */ Point* getOpponentRobotPosition() { Timer* beaconTimer = getTimerByCode(BEACON_TIMER_CODE); // we want to avoid that position change during compute lockAndWaitForTimer(beaconTimer); Laser* laser1 = getLaser(LASER_INDEX_1); Laser* laser2 = getLaser(LASER_INDEX_2); // Updates the detected Position of the laser int detectedPosition1 = updateDetectedPosition(laser1); int detectedPosition2 = updateDetectedPosition(laser2); Point* opponentRobotPosition = &(beaconSystem.opponentRobotPosition); // if there is a detected object (by both laser) if (detectedPosition1 != 0 && detectedPosition2 != 0) { // Too obsolete Information if (getLastDetectionTimeInMillis() > getObsoleteDetectionTimeThreshold()) { opponentRobotPosition->x = 0; opponentRobotPosition->y = 0; OutputStream* outputStream = getOutputStreamLogger(INFO); appendStringAndDec(outputStream, "LOST OBJECT SINCE=", getLastDetectionTimeInMillis()); println(outputStream); } else { float angleInRadian1 = getAngleInRadian(laser1); float angleInRadian2 = getAngleInRadian(laser2); float a1 = tan(angleInRadian1); float a2 = tan(angleInRadian2); // Compute the position fillPositionWithValues(opponentRobotPosition, beaconSystem.distanceBetweenBeacon, a1, a2); } } else { // Reset Robot Position opponentRobotPosition->x = 0; opponentRobotPosition->y = 0; } unlockTimer(beaconTimer); return opponentRobotPosition; }
Weapon::WeaponI* Weapon::Armory::getCoilgun(int level, int projLevel) { //Min level 0 int weaplv = std::max(0, std::min(level, 8)); //If -1, match weapon level (default behaviour) int projlv = (projLevel > -1 ? projLevel : level); Weapon::AutoBat* coilgun = new Weapon::AutoBat(fireCallback_, getLaser(projlv), "coilgun"); upgradeCoilgun(coilgun, weaplv, projlv); return static_cast<Weapon::WeaponI*>(coilgun); }
void printBeaconSystemConfiguration(OutputStream* outputStream) { Point* opponentPoint = getOpponentRobotPosition(); Laser* laser1 = getLaser(LASER_INDEX_1); Laser* laser2 = getLaser(LASER_INDEX_2); printLaserStruct(outputStream, laser1); println(outputStream); printLaserStruct(outputStream, laser2); appendCRLF(outputStream); // Last Detection Time appendStringAndDecf(outputStream, "lastDetectionTime=", getLastDetectionTimeInMillis()); println(outputStream); // Obsolete Detection Time Threshold appendStringAndDecf(outputStream, "obsoleteDetectionTimeThreshold=", getObsoleteDetectionTimeThreshold()); println(outputStream); // Notify Time Delay appendStringAndDecf(outputStream, "notifyTimeDelay=", getNotifyTimeDelay()); println(outputStream); // Opponent Position appendString(outputStream, "opponentPosition:"); printPoint(outputStream, opponentPoint, " mm"); // Distance between beacon appendStringAndDecf(outputStream, "distanceBetweenBeacon=", getDistanceBetweenBeacon()); appendString(outputStream, " mm"); println(outputStream); // Calibration Point appendString(outputStream, "calibrationPoint:"); printPoint(outputStream, &(beaconSystem.calibrationPoint), " mm"); // Opponent Position appendString(outputStream, "opponentPosition:"); printPoint(outputStream, opponentPoint, " mm"); }
void Weapon::Armory::upgradeCoilgun(Weapon::AutoBat* coilgun, int level, int projLevel) { //Match weapon level if (projLevel > -1) coilgun->setProjectile(getLaser(projLevel)); else coilgun->setProjectile(getLaser(level)); coilgun->setLevel(level); //Set base attributes coilgun->setBatterySize(1600); coilgun->setRefireTime(150); coilgun->setRechargeTime(1000); coilgun->setFireCharge(40); //L1H : RefireTime- if (level > 0) { coilgun->setRefireTime(100); } //L2B : BatterySize+ if (level > 1) { coilgun->setBatterySize(2400, true); } //L3F : RechargeTime- if (level > 2) { coilgun->setRechargeTime(500); } //L4S : FireCharge- if (level > 3) { coilgun->setFireCharge(20); } //L5H : RefireTime-- if (level > 4) { coilgun->setRefireTime(50); } //L6B : RechargeTime-- if (level > 5) { coilgun->setRechargeTime(250); } //L7F : BatterySize++ if (level > 6) { coilgun->setBatterySize(3200, true); } //L8S : FireCharge-- if (level > 7) { coilgun->setFireCharge(16); } }
void SlamKarto::laserCallback(const sensor_msgs::LaserScan::ConstPtr& scan) { laser_count_++; if ((laser_count_ % throttle_scans_) != 0) return; static ros::Time last_map_update(0,0); // Check whether we know about this laser yet karto::LaserRangeFinder* laser = getLaser(scan); if(!laser) { ROS_WARN("Failed to create laser device for %s; discarding scan", scan->header.frame_id.c_str()); return; } karto::Pose2 odom_pose; if(addScan(laser, scan, odom_pose)) { ROS_DEBUG("added scan at pose: %.3f %.3f %.3f", odom_pose.GetX(), odom_pose.GetY(), odom_pose.GetHeading()); publishGraphVisualization(); if(!got_map_ || (scan->header.stamp - last_map_update) > map_update_interval_) { if(updateMap()) { last_map_update = scan->header.stamp; got_map_ = true; ROS_DEBUG("Updated the map"); } } } }
/** * Internal timer callback. */ void beaconTimerCallback(Timer* timer) { if (!beaconSystem.enabled) { return; } // Count the number of laser which track something int trackCounter = 0; int laserIndex; for (laserIndex = 0; laserIndex < LASER_COUNT; laserIndex++) { Laser* laser = getLaser(laserIndex); detectsLaser(laser); if (hasTrackSomething(laser)) { trackCounter++; } } // all laser detects something ! if (trackCounter == LASER_COUNT) { beaconSystem.lastDetectionTime = 0.0f; } else { // The code is execute at 2000 Hz, so if we want milliSeconds, we increment by 0.5f beaconSystem.lastDetectionTime += 0.5f; } }
int receiverZigbeeTest(void) { /* char *macAdresse = "1"; char *panId = "1620"; char *channel = "100000"; char *function = "0"; char *adresse = "0"; char *data; char receiveChar; char valide; int toto; */ TRISB = 0; TRISB = 0; // Configure serialBuffer Buffer zigbeeBuffer; initBuffer(&zigbeeBuffer, SERIAL_PORT_ZIGBEE); setAndOpenSerialBuffer(&zigbeeBuffer); Buffer debugBuffer; initBuffer(&debugBuffer, SERIAL_PORT_DEBUG); setAndOpenSerialBuffer(&debugBuffer); setSerialBuffer(&debugBuffer); putString("Beacon Receiver start rs232\n"); initPwmForServo(1500); // Initialization of laserDetector // Laser 1 initLaserDetectorStruct( LASER_INDEX_1, LASER_SERVO_INDEX_1, &getLaserPin1, SERVO_LASER_1_MAX_LEFT_POSITION_DEFAULT_VALUE, SERVO_LASER_1_MAX_RIGHT_POSITION_DEFAULT_VALUE, SHORT_TRACKING_INTERVAL, BEACON_SERVO_1_FACTOR, BEACON_SERVO_1_INIT_COMPUTE_VALUE, BEACON_SERVO_1_ANGLE_DEGREE ); // Laser 2 initLaserDetectorStruct( LASER_INDEX_2, LASER_SERVO_INDEX_2, &getLaserPin2, SERVO_LASER_2_MAX_LEFT_POSITION_DEFAULT_VALUE, SERVO_LASER_2_MAX_RIGHT_POSITION_DEFAULT_VALUE, SHORT_TRACKING_INTERVAL, BEACON_SERVO_2_FACTOR, BEACON_SERVO_2_INIT_COMPUTE_VALUE, BEACON_SERVO_2_ANGLE_DEGREE ); while (1) { int laserIndex; for (laserIndex = 0; laserIndex < LASER_COUNT; laserIndex++) { detectsLaser(laserIndex); } delay100us(5); char c = handleChar(&debugBuffer, false); if (c != 0) { putString("---------\n"); Laser* laser1 = getLaser(LASER_INDEX_1); Laser* laser2 = getLaser(LASER_INDEX_2); printLaserStruct(&debugBuffer, laser1); println(); printLaserStruct(&debugBuffer, laser2); clearBuffer(&debugBuffer); Point p = getPosition(DISTANCE_BETWEEN_BEACON); if (p.x != 0 && p.y != 0) { sendDec(p.x); putString(", "); sendDec(p.y); println(); } } } /* valide = 0; while (1) { if (U2STAbits.URXDA == 1){ valide = getc(); putc1 (valide); } if (U1STAbits.URXDA == 1){ valide = getc1(); putc (valide); } } */ }
//Fun weapons Weapon::WeaponI* Weapon::Armory::getFun(int type) { Weapon::WeaponI * funGun = nullptr; switch (type) { //Boomerang case 22: { } // Explosive Buckshot case 44: { ProjectileDef gren = ProjectileDef(); gren.bounce = 0.f; gren.hpMAX = 1; gren.damage = 10; //gren.oneHit = true; gren.detonation.force = 0.25f; gren.detonation.radius = 2.f; gren.detonation.lifeTime = 50; gren.shrapnel.shards = 8; gren.shrapnel.level = 4; gren.width = 4.f; gren.lifeTime = 250; funGun = new Weapon::SpreadMag(fireCallback_, gren, "thumper", 16, 50, 500, 8, 0.3f); break; } //Broad Beam case 42: { ProjectileDef laser = getLaser(); laser.damage = 80.f; laser.hpMAX = 20; laser.lifeTime = 5000; laser.height = 25.f; laser.width = 200.f; laser.penetration = 100; funGun = new Weapon::SemiBat(fireCallback_, laser, "coilgun", 1280, 32, 64); break; } //Flak cannon case 47: { ProjectileDef shrap = getShrapnel(8); shrap.lifeTime = 175; shrap.oneHit = true; shrap.height = shrap.width * 8; shrap.width *= 4; shrap.shrapnel.shards = 4; shrap.shrapnel.level = 8; shrap.detonation.force = 0.5f; shrap.detonation.radius = 1.f; shrap.detonation.lifeTime = 150; funGun = new Weapon::SpreadMag(fireCallback_, shrap, "shotgun", 4, 0, 300, 10, 2.f); break; } //Chug cannon case 60: { ProjectileDef shug = ProjectileDef(); shug.velScale = 1.f; shug.width = 1.f; shug.hpMAX = 1; //shug.bounce = 1.0; shug.damage = 20; //shug.oneHit = true; shug.shrapnel.shards = 32; shug.shrapnel.level = 8; shug.lifeTime = 500; funGun = new Weapon::AutoMag(fireCallback_, shug, "launcher", 24, 50, 2000, 0.15f ); break; } //Black Hole Gun case 88: { ProjectileDef voib = ProjectileDef(); voib.detonation.force = -0.1f; voib.detonation.radius = 10.f; voib.detonation.lifeTime = 2500; voib.lifeTime = 500; voib.penetration = 15; voib.hpMAX = 10; voib.bounce = 0.75f; voib.width = 1; voib.height = 1; funGun = new Weapon::SemiMag(fireCallback_, voib, "railgun", 2, 400, 2000); break; } //Lance Beam case 111: { ProjectileDef lance = ProjectileDef(); lance.velScale = 1.f; lance.width = 0.5f; lance.height = 4.f; lance.penetration = 4; lance.hpMAX = 4; lance.damage = 30; lance.lifeTime = 300; funGun = new Weapon::SpreadBat(fireCallback_, lance, "coilgun", 2400, 16, 500, 16, 1, -10.0, 4.f, 2.f); break; } //Sonic Boom case 120: { ProjectileDef can = ProjectileDef(); can.velScale = 1.f; can.lifeTime = 1000; can.hpMAX = 1; can.oneHit = true; can.detonation.force = 0.3f; can.detonation.lifeTime = 1000; can.detonation.radius = 7.5f; can.width = 1; can.height = 1; funGun = new Weapon::SemiMag(fireCallback_, can, "shotgun", 10, 1000, 500); break; } //Tracking missile case 404: { ProjectileDef missile = ProjectileDef(); missile.velScale = 1.2f; missile.width = 2.5f; missile.height = 5.f; missile.tracking.radius = 20; missile.tracking.speed = 5.f; missile.lifeTime = 7500; //missile.shrapnel.level = 7; //missile.shrapnel.shards = 4; missile.detonation.force = 0.4f; missile.detonation.lifeTime = 50; missile.detonation.radius = 1.f; missile.oneHit = true; missile.hpMAX = 1; funGun = new Weapon::SemiMag(fireCallback_, missile, "launcher", 8, 500, 3000); break; } //Armageddon Cannon case 555: { ProjectileDef meteor = ProjectileDef(); meteor.lifeTime = 1000; meteor.width = 20; meteor.damage = 100; meteor.velScale = 16.f; meteor.hpMAX = 10; meteor.detonation.force = 0.5f; meteor.detonation.lifeTime = 25; meteor.detonation.radius = 10; funGun = new Weapon::AutoMag(fireCallback_, meteor, "cannon", 32, 500, 5000, .25f); break; } //Mikrowerfer case 666: { ProjectileDef wave = ProjectileDef(); wave.velScale = 0.75f; wave.width = 2.5f; wave.height = 1.5f; wave.bounce = 1.f; wave.hpMAX = 8; wave.damage = 40; wave.lifeTime = 500; funGun = new Weapon::SpreadBat(fireCallback_, wave, "launcher", 320, 16, 800, 16, 6, -1.0, 0.9f, -2.f); break; } //Chaingun case 888: { ProjectileDef bb = getShrapnel(8); bb.lifeTime = 300; bb.oneHit = true; bb.bounce = 0.f; bb.detonation.force = 0.005f; bb.detonation.radius = 0.5f; bb.detonation.lifeTime = 25; funGun = new Weapon::SpreadBat(fireCallback_, bb, "pistol", 2000, 64, 1000, 10, 8, 0, 0.5, 1.1f); break; } case 999: { ProjectileDef knife = ProjectileDef(); knife.oneHit = true; knife.hpMAX = 1; knife.damage = 999999; knife.height = 20.f; knife.width = 1.f; knife.velScale = 3.f; knife.lifeTime = 4000; funGun = new Weapon::SemiBat(fireCallback_, knife, "coilgun", 160, 16, 8); break; } }//End switch if (funGun != nullptr) funGun->setLevel(type); return funGun; }
void Monster::update(MyGame& game) { if (game.game_won) { return; } if (state == sDead) { // shoot body parts everywhere uint32_t parts_count = neck_parts_count+2; Angle delta_angle = 2*Angle::Pi/parts_count; Angle a = -Angle::Pi/6; Transform& et = getSprite(sprEye).getLocalTransform(); et.setPosition(et.getPosition() + Vec2(1, 0).rotate(a)*final_explosion_speed ); a += delta_angle; for (uint32_t i=0; i<neck_parts_count; ++i) { Transform& nt = getNeckPart(i).getLocalTransform(); nt.setPosition(nt.getPosition() + Vec2(1, 0).rotate(a)*final_explosion_speed ); a += delta_angle; } Transform& bt = getSprite(sprBase).getLocalTransform(); et.setPosition(bt.getPosition() + Vec2(1, 0).rotate(a)*final_explosion_speed ); return; } if (health < 0) { gameOver(game); return; } Window& w = game.getModule<Window>(); Events& e = w.getEvents(); if (current_animation_.update()) { if (current_animation_.isAtEnd()) { if (state == sMorphingToHand) { state = sHand; } else if (state == sMorphingToEye) { state = sEye; } } getSprite(sprEye).setTextureCoords(current_animation_.getRegion().getTextureRect()); } if (e.wasButtonClicked(MouseButton::mbRight)) { current_animation_.changeDir(); state = sMorphingToHand; } if (e.wasButtonReleased(MouseButton::mbRight)) { current_animation_.changeDir(); state = sMorphingToEye; } getLaser().setVisible(e.isButtonDown(MouseButton::mbLeft)); Vec2 mouse_pos = e.getMousePos(); Vec2 mouse_local = Mat3::invert(transform.getTransform())*w.getViewPort().viewToWorld(mouse_pos); //std::cout << "mouse_local: " << mouse_local << std::endl; Vec2 eye_pos = transform.getTransform()*getSprite(sprEye).getLocalTransform().getTransform()*Vec2(0,0); Vec2 eye_pos_screen = w.getViewPort().worldToView(eye_pos); Vec2 to_mouse_screen = mouse_pos - eye_pos_screen; if (!to_mouse_screen.isZero()) { Transform& eye_t = getSprite(sprEye).getLocalTransform(); Vec2 eye_pos_local = eye_t.getPosition(); Vec2 eye_to_mouse = mouse_local - eye_pos_local; float d_to_mouse = eye_to_mouse.getLen(); float coeff = (d_to_mouse-min_dist_to_mouse)/d_to_mouse; eye_pos_local += eye_to_mouse*coeff*eye_travel_stiffness; float d_to_base = eye_pos_local.getLen(); if (d_to_base > max_neck_length) { eye_pos_local *= max_neck_length/d_to_base; } eye_t.setPosition(eye_pos_local); // look to mouse Angle angle_delta = to_mouse_screen.getAngle()-eye_t.getRotation(); eye_t.setRotation(eye_t.getRotation()+angle_delta*eye_rot_stiffness); //RectRenderable& last_part = getNeckPart(neck_parts_count-1); Vec2 next_pos = eye_pos_local; // - Vec2(neck_part_len, 0).rotate(eye_t.getRotation()); // last_part.getLocalTransform().setPosition(next_pos); // last_part.getLocalTransform().setRotation(eye_t.getRotation()); for (int32_t i=neck_parts_count-1; i>=0; --i) { RectRenderable& neck_part = getNeckPart(i); Transform& t = neck_part.getLocalTransform(); Vec2 pos = t.getPosition(); float d_to_base = pos.getLen(); float desired_dist = i*(float)neck_part_len;; if (d_to_base > desired_dist) { pos *= desired_dist/d_to_base; } Vec2 to_next = next_pos - pos; float to_next_dist = to_next.getLen(); float coeff = (to_next_dist-neck_part_len)/to_next_dist; pos += to_next*coeff*neck_stiffness; t.setPosition(pos); t.setRotation(to_next.getAngle()); next_pos = pos; } getNeckPart(0).getLocalTransform().setPosition(Vec2(0,0)); } // put laser to eye getLaser().getLocalTransform() = getSprite(sprEye).getLocalTransform(); bool firing = false; if (getLaser().getVisible()) { if (state == sEye && laser > 0) { firing = true; // firing laser Ray laser_ray = getLaserRay(); EntityManager& ent_mgr = game.getModule<EntityManager>(); OverlapInfo oi; oi.setDepth(max_laser_len); bool target_is_missile = false; void* target = NULL; for (uint32_t i=0; i<game.helicopters.getHelicoptersCount(); ++i) { Helicopter& heli = game.helicopters.getHelicopter(i); if (heli.state == Helicopter::sExploding) continue; Rect heli_bound = heli.getBound(); OverlapInfo tmp_oi; if (laser_ray.overlaps(heli_bound, tmp_oi)) { if (tmp_oi.getDepth() < oi.getDepth()) { oi = tmp_oi; target = &heli; } } } for (uint32_t i=0; i<game.missiles.getMissilesCount(); ++i) { Missile& m = game.missiles.getMissile(i); if (m.state == Missile::sExploding) continue; Rect m_bound = m.getBound(); OverlapInfo tmp_oi; if (laser_ray.overlaps(m_bound, tmp_oi)) { if (tmp_oi.getDepth() < oi.getDepth()) { oi = tmp_oi; target = &m; target_is_missile = true; } } } getLaser().setSize({oi.getDepth(), 5}); if (target) { if (target_is_missile) { ((Missile*)target)->explode(); } else { ((Helicopter*)target)->health -=laser_dmg; } } laser -= laser_drain; } } if (!firing) { getLaser().setVisible(false); if (laser < 100){ laser += laser_recharge; } } // eye crash with helicopters Rect eye_bound = getEyeBound(); for (uint32_t i=0; i<game.helicopters.getHelicoptersCount(); ++i) { Helicopter& heli = game.helicopters.getHelicopter(i); if (eye_bound.overlaps(heli.getBound())) { heli.health -= eye_dmg; if (state == sHand) { health -= Helicopter::heli_damage*(1-dmg_resist_hand); } else { health -= Helicopter::heli_damage*(1-dmg_resist_eye); } } } // eye crash with missiles for (uint32_t i=0; i<game.missiles.getMissilesCount(); ++i) { Missile& m = game.missiles.getMissile(i); if (eye_bound.overlaps(m.getBound())) { float dmg; if (m.state == Missile::sOk) { dmg = Missile::missile_impact_damage; m.explode(); } else { dmg = Missile::missile_explosion_damage; } if (state == sHand) { health -= dmg*(1-dmg_resist_hand); } else { health -= dmg*(1-dmg_resist_eye); } } } }