public static void Reset(Simulation_Core.Robot robot, float t)
{
foreach (var mod in robot.modules)
{
mod.joint.Reset_Angle();
}
}
public static void Reset(Simulation_Core.Robot robot, double t)
{
foreach (var mod in robot.modules)
{
mod.joint.Stabilize_Angle();
}
}
public static void Forward(Simulation_Core.Robot robot, float t)//Standard forward movement
{
//Pitches:
for (int i = 0; i < robot.modules.Count; i++)
{
if (robot.modules[i].Axis == "Pitch")
{
angle = amplitudes[i] * Mathf.Sin(2 * Mathf.PI * t / period[i] + phaseDiff[i]) + offset[i]; //Angle = amplitude + sin(2pi * t / period + phase diff) + offset
robot.modules[i].joint.MOVE(angle);
}
else
{
robot.modules[i].joint.MOVE(0);
}
}
}
public static void Turn(Simulation_Core.Robot robot, float t)//Sideways rolling movement
{
for (int i = 0; i < robot.modules.Count; i++)
{
if (robot.modules[i].Axis == "Pitch")
{
angle = amplitudes[i] * Mathf.Sin(2 * Mathf.PI * t / period[i] + phaseDiff[i]) + offset[i]; //Angle = amplitude + sin(2pi * t / period + phase diff) + offset
robot.modules[i].joint.MOVE(angle);
}
//Reduntant, dont need the IFs
if (robot.modules[i].Axis == "Yaw")
{
angle = amplitudes[i] * Mathf.Sin(2 * Mathf.PI * t / period[i] + phaseDiff[i]) + offset[i]; //Angle = amplitude + sin(2pi * t / period + phase diff) + offset
robot.modules[i].joint.MOVE(angle);
}
}
}
public static bool Control(Simulation_Core.Robot robot, double t, double[] dyn_vars)
{
switch (nextAction)
{
case "Forward":
{
if (NewAction)
{
Initialize(nextAction, robot, f_movementVars[0], f_movementVars[1], move_direction * f_movementVars[2], f_movementVars[3], f_movementVars[4], f_movementVars[5], f_movementVars[6]);
}
//Initialize(action, robot, (double)dyn_vars[0], 0, move_direction * (double)dyn_vars[2], 0, (double)dyn_vars[4], 0, 0);
Forward(robot, t);
return(true);
}
case "Turn":
{
if (NewAction)
{
Initialize(nextAction, robot, t_movementVars[0], t_movementVars[1], move_direction * t_movementVars[2], t_movementVars[3], t_movementVars[4], t_movementVars[5], turn_direction * t_movementVars[6]);
}
//Initialize(action, robot, (double)dyn_vars[0], 0, move_direction * (double)dyn_vars[2], 0, (double)dyn_vars[4], 0, turn_direction * (double)dyn_vars[6]);
All_Movements(robot, t);
return(true);
}
case "Custom":
{
if (NewAction)
{
Initialize(nextAction, robot, f_movementVars[0], f_movementVars[1], f_movementVars[2], f_movementVars[3], f_movementVars[4], f_movementVars[5], f_movementVars[6]);
}
//Initialize(action, robot, (double)dyn_vars[0], 0, move_direction * (double)dyn_vars[2], 0, (double)dyn_vars[4], 0, turn_direction * (double)dyn_vars[6]);
All_Movements(robot, t);
return(true);
}
case "Idle": Idle(); return(true);
case "Reset": Reset(robot, t); return(true);
default: return(false);
}
}
public static bool Control(Simulation_Core.Robot robot, float t)
{
/*if (Time.fixedTime >= 30)
* newAction = "Forward";
* else if (Time.fixedTime <= 25)
* newAction = "Turn";
* else
* newAction = "Reset";//Must have a smooth reset. */
//newAction = "Forward";
switch (action)
{
case "Forward":
{
if (NewAction)
{
Initialize(action, robot, 2 * (Mathf.PI / 9.0f), 0, move_direction * 0.5f * Mathf.PI * 2.0f / 3.0f, 0, set_period, 0, 0);
}
Forward(robot, t);
return(true);
}
case "Turn":
{
if (NewAction)
{
Initialize(action, robot, 2 * (Mathf.PI / 9.0f), 0, move_direction * Mathf.PI * 2.0f / 3.0f, 0, set_period, 0, turn_direction * 20 * Mathf.PI / 180);
}
Turn(robot, t);
return(true);
}
case "Idle": Idle(); return(true);
case "Reset": Reset(robot, t); return(true);
default: return(false);
}
}
static float angle; //rads
public static void Initialize(string newAction, Simulation_Core.Robot robot, float _ampPitch, float _ampYaw, float _phaseOffsetPitch, float _phaseOffsetYaw, float _period, float _offsetPitch, float _offsetYaw)
{
Dynamics.action = newAction;
int mod_n = robot.modules.Count;
amplitudes = new float[mod_n];
period = new float[mod_n];
phaseDiff = new float[mod_n];
offset = new float[mod_n];
//can set "buttons" for each turning, rolling, etc.
for (int i = 0; i < robot.modules.Count; i++)
{
Dynamics.period[i] = _period;
if (robot.modules[i].Axis == "Pitch")
{
Dynamics.amplitudes[i] = _ampPitch;
Dynamics.phaseDiff[i] = 0;// _phaseDiffPitch;
Dynamics.offset[i] = _offsetPitch;
}
else
{
Dynamics.amplitudes[i] = _ampYaw;
Dynamics.phaseDiff[i] = 0;// _phaseDiffYaw;
Dynamics.offset[i] = _offsetYaw;
}
}
switch (newAction)
{
case "Turn":
{
//float phaseOffset = _phaseOffsetPitch;
for (int i = 0; i < mod_n; i++)
{
if (i > 1) //Not taking into account the first pitch and first yaw which should be 0
{
if (robot.modules[i].Axis == "Pitch")
{
Dynamics.phaseDiff[i] = Dynamics.phaseDiff[i - 2] + _phaseOffsetPitch;
}
else
{
Dynamics.phaseDiff[i] = Dynamics.phaseDiff[i - 2] + _phaseOffsetYaw;
}
}
}
} break;
case "Forward":
{
for (int i = 0; i < mod_n; i++)
{
if (i >= 1) //Not taking into account the first phase which should be 0
{
Dynamics.phaseDiff[i] = Dynamics.phaseDiff[i - 1] + _phaseOffsetPitch;
}
}
} break;
}
Dynamics.currentAction = newAction;
}
