OrthotropicElasticityProperty::OrthotropicElasticityProperty(PropertyModel* /* propmodel */, ParameterModel* paramodel, int id) : Property(id) { setName("Orthotropic Elasticity"); setDisplayName(QObject::tr("Orthotropic Elasticity")); setCategory(LinearElasticProperty); setType(Elasticity); setBehavior(Orthotropic); Parameter *par; par = paramodel->getParameter("Young's Modulus X direction"); addParameter(par->clone()); par = paramodel->getParameter("Young's Modulus Y direction"); addParameter(par->clone()); par = paramodel->getParameter("Young's Modulus Z direction"); addParameter(par->clone()); par = paramodel->getParameter("Poisson's Ratio XY"); addParameter(par->clone()); par = paramodel->getParameter("Poisson's Ratio YZ"); addParameter(par->clone()); par = paramodel->getParameter("Poisson's Ratio XZ"); addParameter(par->clone()); par = paramodel->getParameter("Shear Modulus XY"); addParameter(par->clone()); par = paramodel->getParameter("Shear Modulus YZ"); addParameter(par->clone()); par = paramodel->getParameter("Shear Modulus XZ"); addParameter(par->clone()); }
void PigMover::runRetreat() { // TODO: Is there a better way? static uint8_t retreatCounter = 0; // Still running current behavior if(counter % LEG_RETREAT_PERIOD) return; switch(retreatCounter % 3) { case 0: setMovement(WheelController::STOP); break; case 1: setMovement(WheelController::BACKWARD); break; case 2: setMovement(WheelController::BACKWARD); break; } // Retreat only for a set time if(retreatCounter % 9 == 0) { setBehavior(SEARCH); } retreatCounter++; }
OrthotropicSecantCoefficientOfThermalExpansion::OrthotropicSecantCoefficientOfThermalExpansion(PropertyModel *propmodel, ParameterModel* paramodel, int id) : Property(id) { setName("Orthotropic Secant Coefficient of Thermal Expansion"); setDisplayName(QObject::tr("Orthotropic Secant Coefficient of Thermal Expansion")); setCategory(PhysicalProperty); setType(CoefficientOfThermalExpansion); setBehavior(Orthotropic); setDefinition(Secant); Parameter *par; par = paramodel->getParameter("Coefficient of Thermal Expansion X direction"); addParameter(par->clone()); par = paramodel->getParameter("Coefficient of Thermal Expansion Y direction"); addParameter(par->clone()); par = paramodel->getParameter("Coefficient of Thermal Expansion Z direction"); addParameter(par->clone()); referenceTemperatureProperty_ = dynamic_cast<ReferenceTemperatureProperty*>(propmodel->getProperty("Reference Temperature")); referenceTemperatureProperty_->setBehavior(Orthotropic); referenceTemperatureProperty_->setDefinition(Secant); referenceTemperatureProperty_->setMaterialProperty("Coefficient of Thermal Expansion"); par = referenceTemperatureProperty_->getParameter("Reference Temperature"); addParameter(par); }
OrthotropicElasticityProperty::OrthotropicElasticityProperty(const OrthotropicElasticityProperty& property) : Property(property.getId()) { setName("Orthotropic Elasticity"); setDisplayName(QObject::tr("Orthotropic Elasticity")); setCategory(LinearElasticProperty); setType(Elasticity); setBehavior(Orthotropic); const Parameter *par1x = property.getParameter("Young's Modulus X direction"); addParameter(par1x->clone()); const Parameter *par1y = property.getParameter("Young's Modulus Y direction"); addParameter(par1y->clone()); const Parameter *par1z = property.getParameter("Young's Modulus Z direction"); addParameter(par1z->clone()); const Parameter *par2xy = property.getParameter("Poisson's Ratio XY"); addParameter(par2xy->clone()); const Parameter *par2yz = property.getParameter("Poisson's Ratio YZ"); addParameter(par2yz->clone()); const Parameter *par2xz = property.getParameter("Poisson's Ratio XZ"); addParameter(par2xz->clone()); const Parameter *par3xy = property.getParameter("Shear Modulus XY"); addParameter(par3xy->clone()); const Parameter *par3yz = property.getParameter("Shear Modulus YZ"); addParameter(par3yz->clone()); const Parameter *par3xz = property.getParameter("Shear Modulus XZ"); addParameter(par3xz->clone()); }
// In the future, we could include more than 2 flight plans... // flightplanNum is 0 for the main lgo instructions, and 1 for RTL instructions //void init_flightplan(int16_t flightplanNum) void flightplan_logo_begin(int16_t flightplanNum) { struct relative2D curHeading; struct relative3D IMUloc; int8_t earth_yaw; int16_t angle; if (flightplanNum == 1) // RTL instructions set { currentInstructionSet = (struct logoInstructionDef*)rtlInstructions; numInstructionsInCurrentSet = NUM_RTL_INSTRUCTIONS; } else if (flightplanNum == 0) // Main instructions set { currentInstructionSet = (struct logoInstructionDef*)instructions; numInstructionsInCurrentSet = NUM_INSTRUCTIONS; } instructionIndex = 0; logoStackIndex = 0; logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_SUBROUTINE; logoStack[logoStackIndex].arg = 0; logoStack[logoStackIndex].returnInstructionIndex = -1; // When starting over, begin on instruction 0 currentTurtle = PLANE; penState = 0; // 0 means down. more than 0 means up turtleLocations[PLANE].x._.W1 = IMUlocationx._.W1; turtleLocations[PLANE].y._.W1 = IMUlocationy._.W1; turtleLocations[PLANE].z = IMUlocationz._.W1; turtleLocations[CAMERA].x._.W1 = IMUlocationx._.W1; turtleLocations[CAMERA].y._.W1 = IMUlocationy._.W1; turtleLocations[CAMERA].z = IMUlocationz._.W1; // Calculate heading from Direction Cosine Matrix (rather than GPS), // So that this code works when the plane is static. e.g. at takeoff curHeading.x = -rmat[1]; curHeading.y = rmat[4]; earth_yaw = rect_to_polar(&curHeading); // (0=East, ccw) angle = (earth_yaw * 180 + 64) >> 7; // (ccw, 0=East) angle = -angle + 90; // (clockwise, 0=North) turtleAngles[PLANE] = turtleAngles[CAMERA] = angle; setBehavior(0); IMUloc.x = IMUlocationx._.W1; IMUloc.y = IMUlocationy._.W1; IMUloc.z = IMUlocationz._.W1; update_goal_from(IMUloc); interruptIndex = 0; interruptStackBase = 0; process_instructions(); }
IsotropicThermalConductivityProperty::IsotropicThermalConductivityProperty(const IsotropicThermalConductivityProperty& property) : Property(property.getId()) { setName("Isotropic Thermal Conductivity"); setDisplayName(QObject::tr("Isotropic Thermal Conductivity")); setCategory(ThermalProperty); setType(ThermalConductivity); setBehavior(Isotropic); const Parameter *par = property.getParameter("Thermal Conductivity"); addParameter(par->cloneWithData()); }
PigMover::PigMover(WheelController & legs, NoseController & nose, uint16_t transitionPeriod) : currentMovement(WheelController::STOP), transitionPeriod(transitionPeriod), transitionCounter(0), counter(0), legs(legs), nose(nose) { setBehavior(SEARCH); }
IsotropicThermalConductivityProperty::IsotropicThermalConductivityProperty(PropertyModel* /* propmodel */, ParameterModel* paramodel, int id) : Property(id) { setName("Isotropic Thermal Conductivity"); setDisplayName(QObject::tr("Isotropic Thermal Conductivity")); setCategory(ThermalProperty); setType(ThermalConductivity); setBehavior(Isotropic); Parameter *par = paramodel->getParameter("Thermal Conductivity"); addParameter(par->clone()); }
void init_behavior(void) { current_orientation = F_NORMAL; desired_behavior.W = current_orientation; setBehavior(current_orientation); if (TRIGGER_TYPE != TRIGGER_TYPE_NONE) { triggerActionSetValue(TRIGGER_ACTION != TRIGGER_PULSE_HIGH); } }
IsotropicInstantaneousCoefficientOfThermalExpansion::IsotropicInstantaneousCoefficientOfThermalExpansion(const IsotropicInstantaneousCoefficientOfThermalExpansion& property) : Property(property.getId()) { setName("Isotropic Instantaneous Coefficient of Thermal Expansion"); setDisplayName(QObject::tr("Isotropic Instantaneous Coefficient of Thermal Expansion")); setCategory(PhysicalProperty); setType(CoefficientOfThermalExpansion); setBehavior(Isotropic); setDefinition(Instantaneous); const Parameter *par = property.getParameter("Coefficient of Thermal Expansion"); addParameter(par->cloneWithData()); }
void PigMover::notify(Reading reading) { if(!reading.isValid()) return; currentReading = reading; // Ran into pen limit while searching, but not aligned. Find line to get // ready for retreat. if(reading.left && !reading.right) { if (currentBehavior == SEARCH) { setBehavior(FIND_LINE_CCW); } else if (currentBehavior == RETREAT) { setBehavior(SEARCH); } return; } // Similar to the previous case if(!reading.left && reading.right) { if (currentBehavior == SEARCH) { setBehavior(FIND_LINE_CW); } else if (currentBehavior == RETREAT) { setBehavior(SEARCH); } return; } // Found pen limit and is aligned. Retreat. if(reading.left && reading.right) { if(currentBehavior == SEARCH || currentBehavior == FIND_LINE_CW || currentBehavior == FIND_LINE_CCW) { setBehavior(AVOID_LINE); } return; } if(!reading.left && !reading.right) { if(currentBehavior == FIND_LINE_CW || currentBehavior == FIND_LINE_CCW) { // Was looking for pen limit, but somehow lost it. Get back to // searching again. setBehavior(SEARCH); } else if (currentBehavior == AVOID_LINE) { // Got clear of the line when trying to get out of it. Start // the actual retreat. setBehavior(RETREAT); } return; } }
void Ship::setType(Ship::Type type){ switch (type){ case Ship::NONE: player = Player::NONE; setBehavior(new NormalBehavior()); break; case Ship::NORMAL: image = QImage(":NORMAL"); setBehavior(new NormalBehavior()); break; case Ship::ROCKET: image = QImage(":ROCKET"); setBehavior(new RocketBehavior()); break; case Ship::BOMB: image = QImage(":BOMB"); setBehavior(new BombBehavior()); break; case Ship::LASER: image = QImage(":LASER"); setBehavior(new LaserBehavior()); break; case Ship::PHASER: image = QImage(":PHASER"); setBehavior(new PhaserBehavior()); break; } this->type = type; }
void set_waypoint(int16_t index) { DPRINT("set_waypoint(%u)\r\n", index); if (index < numPointsInCurrentSet) { waypointIndex = index; #if (USE_MAVLINK == 1) mavlink_waypoint_changed(waypointIndex); #endif if (waypointIndex == 0) { if (numPointsInCurrentSet > 1) { struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[numPointsInCurrentSet-1]); current_waypoint = wp_to_relative(currentWaypointSet[0]); navigate_set_goal(previous_waypoint.loc, current_waypoint.loc); set_camera_view(current_waypoint.viewpoint); } else { current_waypoint = wp_to_relative(currentWaypointSet[0]); navigate_set_goal(GPSlocation, current_waypoint.loc); set_camera_view(current_waypoint.viewpoint); } setBehavior(currentWaypointSet[0].flags); } else { struct relWaypointDef previous_waypoint = wp_to_relative(currentWaypointSet[waypointIndex-1]); current_waypoint = wp_to_relative(currentWaypointSet[waypointIndex]); navigate_set_goal(previous_waypoint.loc, current_waypoint.loc); set_camera_view(current_waypoint.viewpoint); setBehavior(current_waypoint.flags); } #if (DEADRECKONING == 0) navigate_compute_bearing_to_goal(); #endif } }
// ---------------------------------------------------------------------------- // set new behavior and perform default actions // ---------------------------------------------------------------------[ set ] void WRTbotDrive::set( wrt_ir_rc_t &rc, wrt_motor_t &motor ) { switch( rc.behavior & wrt_rc_cmd_behavior_set ) { default: // -------------------------------[ default behavior: ir hand control ] case wrt_rc_cmd_behavior_irc: { // ---------------------------------------------------------------- // --- do: respond to hand held infrared remote control motion keys // --- do: forward (key: ^ ), reverse (key: v ) // --- do: left (key: |<<), right (key: >>| ) // ---------------------------------------------------------------- setBehavior( rc, motor ); } break; // ------------------------------------[ behavior: hunt ambient light ] case wrt_rc_cmd_behavior_lightHunter: { // ---------------------------------------------------------------- // --- do: hunt for ambient light sensor // --- do: stop if no light is sensensed // --- do: avoid obstacles? // ---------------------------------------------------------------- setBehavior( rc, motor ); } break; // --------------------------------------------------[ behavior: pray ] case wrt_rc_cmd_behavior_pray: { // ---------------------------------------------------------------- // --- do: avoid obstacles, turn away from obstacles // --- do: stop if no way forward // --- do: backtrack and turn if no way forward after N msec // ---------------------------------------------------------------- } break; } // switch } // done set()
IsotropicInstantaneousCoefficientOfThermalExpansion::IsotropicInstantaneousCoefficientOfThermalExpansion(PropertyModel* /* propmodel */, ParameterModel* paramodel, int id) : Property(id) { setName("Isotropic Instantaneous Coefficient of Thermal Expansion"); setDisplayName(QObject::tr("Isotropic Instantaneous Coefficient of Thermal Expansion")); setCategory(PhysicalProperty); setType(CoefficientOfThermalExpansion); setBehavior(Isotropic); setDefinition(Instantaneous); Parameter *par = paramodel->getParameter("Coefficient of Thermal Expansion"); addParameter(par->clone()); }
OrthotropicThermalConductivityProperty::OrthotropicThermalConductivityProperty(const OrthotropicThermalConductivityProperty& property) : Property(property.getId()) { setName("Orthotropic Thermal Conductivity"); setDisplayName(QObject::tr("Orthotropic Thermal Conductivity")); setCategory(ThermalProperty); setType(ThermalConductivity); setBehavior(Orthotropic); const Parameter *par1 = property.getParameter("Thermal Conductivity X direction"); addParameter(par1->cloneWithData()); const Parameter *par2 = property.getParameter("Thermal Conductivity Y direction"); addParameter(par2->cloneWithData()); const Parameter *par3 = property.getParameter("Thermal Conductivity Z direction"); addParameter(par3->cloneWithData()); }
OrthotropicInstantaneousCoefficientOfThermalExpansion::OrthotropicInstantaneousCoefficientOfThermalExpansion(const OrthotropicInstantaneousCoefficientOfThermalExpansion& property) : Property(property.getId()) { setName("Orthotropic Instantaneous Coefficient of Thermal Expansion"); setDisplayName(QObject::tr("Orthotropic Instantaneous Coefficient of Thermal Expansion")); setCategory(PhysicalProperty); setType(CoefficientOfThermalExpansion); setBehavior(Orthotropic); setDefinition(Instantaneous); const Parameter *par1 = property.getParameter("Coefficient of Thermal Expansion X direction"); addParameter(par1->cloneWithData()); const Parameter *par2 = property.getParameter("Coefficient of Thermal Expansion Y direction"); addParameter(par2->cloneWithData()); const Parameter *par3 = property.getParameter("Coefficient of Thermal Expansion Z direction"); addParameter(par3->cloneWithData()); }
IsotropicSecantCoefficientOfThermalExpansion::IsotropicSecantCoefficientOfThermalExpansion(const IsotropicSecantCoefficientOfThermalExpansion& property) : Property(property.getId()) { setName("Isotropic Secant Coefficient of Thermal Expansion"); setDisplayName(QObject::tr("Isotropic Secant Coefficient of Thermal Expansion")); setCategory(PhysicalProperty); setType(CoefficientOfThermalExpansion); setBehavior(Isotropic); setDefinition(Secant); const Parameter *par1 = property.getParameter("Coefficient of Thermal Expansion"); addParameter(par1->clone()); referenceTemperatureProperty_ = dynamic_cast<ReferenceTemperatureProperty*>(property.referenceTemperatureProperty_->clone()); referenceTemperatureProperty_->setBehavior(Isotropic); referenceTemperatureProperty_->setDefinition(Secant); referenceTemperatureProperty_->setMaterialProperty("Coefficient of Thermal Expansion"); addParameter(referenceTemperatureProperty_->getParameter("Reference Temperature")); }
//void run_flightplan(void) void flightplan_waypoints_update(void) { // first run any injected wp from the serial port if (wp_inject_pos == WP_INJECT_READY) { current_waypoint = wp_to_relative(wp_inject); navigate_set_goal(GPSlocation, current_waypoint.loc); set_camera_view(current_waypoint.viewpoint); setBehavior(current_waypoint.flags); navigate_compute_bearing_to_goal(); wp_inject_pos = 0; return; } // steering is based on cross track error. // waypoint arrival is detected computing distance to the "finish line". // note: locations are measured in meters // velocities are in centimeters per second // locations have a range of +-32000 meters (20 miles) from origin if (desired_behavior._.altitude) { if (abs(IMUheight - navigate_get_goal(NULL)) < ((int16_t)altit.HeightMargin)) { next_waypoint(); } } else { if (tofinish_line < WAYPOINT_PROXIMITY_RADIUS) // crossed the finish line { if (desired_behavior._.loiter) { navigate_set_goal(GPSlocation, wp_to_relative(currentWaypointSet[waypointIndex]).loc); } else { next_waypoint(); } } } }
IsotropicElasticityProperty::IsotropicElasticityProperty(const IsotropicElasticityProperty& property) : Property(property.getId()) { setName("Isotropic Elasticity"); setDisplayName(QObject::tr("Isotropic Elasticity")); setCategory(LinearElasticProperty); setType(Elasticity); setBehavior(Isotropic); const Parameter *par1 = property.getParameter("Young's Modulus"); addParameter(par1->cloneWithData()); const Parameter *par2 = property.getParameter("Poisson's Ratio"); addParameter(par2->cloneWithData()); const Parameter *par3 = property.getParameter("Shear Modulus"); addParameter(par3->cloneWithData()); const Parameter *par4 = property.getParameter("Bulk Modulus"); addParameter(par4->cloneWithData()); widget_ = 0; setCalculationMode(CalcFromYoungsModulusAndPoissonsRatio); }
// In the future, we could include more than 2 waypoint sets... // flightplanNum is 0 for main waypoints, and 1 for RTL waypoints //void init_flightplan(int16_t flightplanNum) void flightplan_waypoints_begin(int16_t flightplanNum) { if (flightplanNum == 1) // RTL waypoint set { load_flightplan(rtlWaypoints, NUMBER_RTL_POINTS); // currentWaypointSet = (struct waypointDef*)rtlWaypoints; // numPointsInCurrentSet = NUMBER_RTL_POINTS; } else if (flightplanNum == 0) // Main waypoint set { load_flightplan(waypoints, NUMBER_POINTS); // currentWaypointSet = (struct waypointDef*)waypoints; // numPointsInCurrentSet = NUMBER_POINTS; } waypointIndex = 0; current_waypoint = wp_to_relative(currentWaypointSet[0]); navigate_set_goal(GPSlocation, current_waypoint.loc); set_camera_view(current_waypoint.viewpoint); setBehavior(current_waypoint.flags); // udb_background_trigger(); // trigger navigation immediately }
IsotropicElasticityProperty::IsotropicElasticityProperty(PropertyModel * /* propmodel */, ParameterModel* paramodel, int id) : Property(id) { setName("Isotropic Elasticity"); setDisplayName(QObject::tr("Isotropic Elasticity")); setCategory(LinearElasticProperty); setType(Elasticity); setBehavior(Isotropic); Parameter *par; par = paramodel->getParameter("Young's Modulus"); addParameter(par->clone()); par = paramodel->getParameter("Poisson's Ratio"); addParameter(par->clone()); par = paramodel->getParameter("Shear Modulus"); addParameter(par->clone()); par = paramodel->getParameter("Bulk Modulus"); addParameter(par->clone()); widget_ = 0; setCalculationMode(CalcFromYoungsModulusAndPoissonsRatio); }
/*! Adjusts behavior to \a behavior. */ void QUmlOpaqueExpression::setBehavior(QUmlBehavior *behavior) { // This is a read-write association end if (_behavior != behavior) { _behavior = behavior; if (behavior && behavior->asQModelingObject() && this->asQModelingObject()) QObject::connect(behavior->asQModelingObject(), SIGNAL(destroyed()), this->asQModelingObject(), SLOT(setBehavior())); } }
Ship::~Ship() { setBehavior(NULL); }
Ship::Ship() : player(Player::NONE), type(Ship::NONE), behavior(NULL) { image = QImage(":NORMAL"); setBehavior(new NormalBehavior); }
static boolean process_one_instruction(struct logoInstructionDef instr) { if (instr.use_param) { // Use the subroutine's parameter instead of the instruction's arg value int16_t ind = get_current_stack_parameter_frame_index(); instr.arg = logoStack[ind].arg; } switch (instr.cmd) { case 1: // Repeat switch (instr.subcmd) { case 0: // Repeat N times (or forever if N == -1) if (logoStackIndex < LOGO_STACK_DEPTH-1) { logoStackIndex++; logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_REPEAT; logoStack[logoStackIndex].arg = instr.arg; logoStack[logoStackIndex].returnInstructionIndex = instructionIndex; } break; case 1: // End if (logoStackIndex > 0) { if (logoStack[logoStackIndex].frameType == LOGO_FRAME_TYPE_REPEAT) { // END REPEAT if (logoStack[logoStackIndex].arg > 1 || logoStack[logoStackIndex].arg == -1) { if (logoStack[logoStackIndex].arg != -1) { logoStack[logoStackIndex].arg--; } instructionIndex = logoStack[logoStackIndex].returnInstructionIndex; } else { logoStackIndex--; } } else if (logoStack[logoStackIndex].frameType == LOGO_FRAME_TYPE_SUBROUTINE) { // END SUBROUTINE instructionIndex = logoStack[logoStackIndex].returnInstructionIndex; logoStackIndex--; if (logoStackIndex < interruptStackBase) { interruptStackBase = 0; instructionsProcessed = MAX_INSTRUCTIONS_PER_CYCLE; // stop processing instructions after finishing interrupt } } else if (logoStack[logoStackIndex].frameType == LOGO_FRAME_TYPE_IF) { // Do nothing at the end of an IF block logoStackIndex--; } } else { // Extra, unmatched END goes back to the start of the program instructionIndex = logoStack[0].returnInstructionIndex; logoStackIndex = 0; interruptStackBase = 0; } break; case 3: // Else if (logoStack[logoStackIndex].frameType == LOGO_FRAME_TYPE_IF) { instructionIndex = find_end_of_current_if_block(); logoStackIndex--; } break; case 2: // To (define a function) { // Shouldn't ever run these lines. // If we do get here, restart from the top of the logo program. instructionIndex = logoStack[0].returnInstructionIndex; logoStackIndex = 0; interruptStackBase = 0; } break; } break; case 10: // Exec (reset the stack and then call a subroutine) instructionIndex = find_start_of_subroutine(instr.subcmd); logoStack[0].returnInstructionIndex = instructionIndex; logoStackIndex = 0; interruptStackBase = 0; break; case 2: // Do (call a subroutine) if (logoStackIndex < LOGO_STACK_DEPTH-1) { logoStackIndex++; logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_SUBROUTINE; logoStack[logoStackIndex].arg = instr.arg; logoStack[logoStackIndex].returnInstructionIndex = instructionIndex; } instructionIndex = find_start_of_subroutine(instr.subcmd); break; case 3: // Forward/Back switch (instr.subcmd) { case 0: // Forward { int16_t cangle = turtleAngles[currentTurtle]; // 0-359 (clockwise, 0=North) int8_t b_angle = (cangle * 182 + 128) >> 8; // 0-255 (clockwise, 0=North) b_angle = -b_angle - 64; // 0-255 (ccw, 0=East) turtleLocations[currentTurtle].x.WW += (__builtin_mulss(-cosine(b_angle), instr.arg) << 2); turtleLocations[currentTurtle].y.WW += (__builtin_mulss(-sine(b_angle), instr.arg) << 2); } break; } break; case 4: // Rotate switch (instr.subcmd) { case 0: // Right { int16_t angle = turtleAngles[currentTurtle] + instr.arg; while (angle < 0) angle += 360; angle = angle % 360; turtleAngles[currentTurtle] = angle; break; } case 1: // Set Angle turtleAngles[currentTurtle] = instr.arg; break; case 2: // Use current angle { turtleAngles[currentTurtle] = get_current_angle(); break; } case 3: // Use angle to goal { turtleAngles[currentTurtle] = get_angle_to_point(IMUlocationx._.W1, IMUlocationy._.W1); break; } } break; case 5: // MV/SET location - X, Y, and Z switch (instr.subcmd) { case 0: // Move X turtleLocations[currentTurtle].x._.W1 += instr.arg; break; case 1: // Set X location turtleLocations[currentTurtle].x._.W0 = 0; turtleLocations[currentTurtle].x._.W1 = instr.arg; break; case 2: // Move Y turtleLocations[currentTurtle].y._.W1 += instr.arg; break; case 3: // Set Y location turtleLocations[currentTurtle].y._.W0 = 0; turtleLocations[currentTurtle].y._.W1 = instr.arg; break; case 4: // Move Z turtleLocations[currentTurtle].z += instr.arg; break; case 5: // Set Z location turtleLocations[currentTurtle].z = instr.arg; break; case 6: // Use current position (for x and y) turtleLocations[currentTurtle].x._.W0 = 0; turtleLocations[currentTurtle].x._.W1 = IMUlocationx._.W1; turtleLocations[currentTurtle].y._.W0 = 0; turtleLocations[currentTurtle].y._.W1 = IMUlocationy._.W1; break; case 7: // HOME turtleAngles[currentTurtle] = 0; turtleLocations[currentTurtle].x.WW = 0; turtleLocations[currentTurtle].y.WW = 0; break; case 8: // Absolute set high value absoluteHighWord = instr.arg; break; case 9: // Absolute set low X value { absoluteXLong._.W1 = absoluteHighWord; absoluteXLong._.W0 = instr.arg; break; } case 10: // Absolute set low Y value { struct waypoint3D wp; struct relative3D rel; union longww absoluteYLong; absoluteYLong._.W1 = absoluteHighWord; absoluteYLong._.W0 = instr.arg; wp.x = absoluteXLong.WW; wp.y = absoluteYLong.WW; wp.z = 0; rel = dcm_absolute_to_relative(wp); turtleLocations[currentTurtle].x._.W0 = 0; turtleLocations[currentTurtle].x._.W1 = rel.x; turtleLocations[currentTurtle].y._.W0 = 0; turtleLocations[currentTurtle].y._.W1 = rel.y; break; } } break; case 6: // Flags switch (instr.subcmd) { case 0: // Flag On setBehavior(desired_behavior.W | instr.arg); break; case 1: // Flag Off setBehavior(desired_behavior.W & ~instr.arg); break; case 2: // Flag Toggle setBehavior(desired_behavior.W ^ instr.arg); break; } break; case 7: // Pen Up/Down switch (instr.subcmd) { case 0: // Pen Up penState++; break; case 1: // Pen Down if (penState > 0) penState--; break; case 2: // Pen Toggle penState = (penState == 0); if (penState == 0) instr.do_fly = 1; // Set the Fly Flag break; } break; case 8: // Set Turtle (choose plane or camera target) currentTurtle = (instr.arg == CAMERA) ? CAMERA : PLANE; break; case 9: // Modify PARAM switch (instr.subcmd) { case 0: // Set param { int16_t ind = get_current_stack_parameter_frame_index(); logoStack[ind].arg = instr.arg; break; } case 1: // Add to param { int16_t ind = get_current_stack_parameter_frame_index(); logoStack[ind].arg += instr.arg; break; } case 2: // Multiply param { int16_t ind = get_current_stack_parameter_frame_index(); logoStack[ind].arg *= instr.arg; break; } case 3: // Divide param { int16_t ind = get_current_stack_parameter_frame_index(); if (instr.arg != 0) // Avoid divide by 0! { logoStack[ind].arg /= instr.arg; } break; } } break; case 11: // Speed #if (SPEED_CONTROL == 1) switch (instr.subcmd) { case 0: // Increase Speed desiredSpeed += instr.arg * 10; break; case 1: // Set Speed desiredSpeed = instr.arg * 10; break; } if (desiredSpeed < 0) desiredSpeed = 0; #endif break; case 12: // Interrupts switch (instr.subcmd) { case 1: // Set interruptIndex = find_start_of_subroutine(instr.arg); break; case 0: // Clear interruptIndex = 0; break; } break; case 13: // Load to PARAM { int16_t ind = get_current_stack_parameter_frame_index(); logoStack[ind].arg = logo_value_for_identifier(instr.subcmd); break; } case 14: // IF commands case 15: case 16: case 17: case 18: case 19: { int16_t val = logo_value_for_identifier(instr.subcmd); boolean condTrue = false; if (instr.cmd == 14 && val == instr.arg) condTrue = true; // IF_EQ else if (instr.cmd == 15 && val != instr.arg) condTrue = true; // IF_NE else if (instr.cmd == 16 && val > instr.arg) condTrue = true; // IF_GT else if (instr.cmd == 17 && val < instr.arg) condTrue = true; // IF_LT else if (instr.cmd == 18 && val >= instr.arg) condTrue = true; // IF_GE else if (instr.cmd == 19 && val <= instr.arg) condTrue = true; // IF_LE if (condTrue) { if (logoStackIndex < LOGO_STACK_DEPTH-1) { logoStackIndex++; logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_IF; } } else { // jump to the matching END or ELSE instructionIndex = find_end_of_current_if_block(); if (currentInstructionSet[instructionIndex].subcmd == 3) // is entering an ELSE block { if (logoStackIndex < LOGO_STACK_DEPTH-1) { logoStackIndex++; logoStack[logoStackIndex].frameType = LOGO_FRAME_TYPE_IF; } } } break; } } return instr.do_fly; }