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;
}