arma::vec BaderGrid::regional_charges(const arma::mat & P) {
arma::vec q(maxima.size());
q.zeros();
for(size_t i=0;i<maxima.size();i++) {
arma::mat Sat(regional_overlap(i));
q(i)=arma::trace(P*Sat);
}
return -q;
}
static void BucketOctave(int16_t const * in, int16_t * out) {
for (unsigned i = 0; i < BUCKET_COUNT; ++i) {
int32_t sum = 0;
unsigned j = i;
for (unsigned octave = 0; octave < 4; ++octave) {
sum += in[j];
j += BUCKET_COUNT;
}
out[i] = Sat(sum);
}
}
/**
* Correct state variables for time step
* @param t_inc time step increment
* @param flag initial step if true
*/
void gridpack::dynamic_simulation::Exdc1Model::corrector(double t_inc, bool flag)
{
double Feedback;
// State 2
//printf("TR = %f, Vterminal = %f, x2_1 = %f\n", TR, Vterminal, x2_1);
if (TR > TS_THRESHOLD * t_inc) dx2_1 = (Vterminal - x2_1) / TR; // SJin: x2 is Vsens
else x2_1 = Vterminal; // propogate state immediately
// State 5
if (TF > TS_THRESHOLD * t_inc) {
dx5_1 = (x1_1 * KF / TF - x5_1) / TF;
Feedback = x1_1 * KF / TF - x5_1;
} else {
x5_1 = 0;
Feedback = 0;
}
// State 3
double Vstab = 0.0; // SJin: Output from PSS, set to 0.0 for now.
double LeadLagIN = Vref - x2_1 + Vstab - Feedback;
double LeadLagOUT;
//printf("LeadLagIN = %f, TC = %f, TB = %f, x3 = %f\n", LeadLagIN, TC, TB, x3);
if (TB > (TS_THRESHOLD * t_inc)) {
dx3_1 = (LeadLagIN * (1 - TC / TB) - x3_1) / TB;
LeadLagOUT = LeadLagIN * TC / TB + x3_1;
} else
LeadLagOUT = LeadLagIN;
// State 4
if (x4_1 > Vrmax) x4_1 = Vrmax;
if (x4_1 < Vrmin) x4_1 = Vrmin;
if (TA > (TS_THRESHOLD * t_inc))
dx4_1 = (LeadLagOUT * KA - x4_1) / TA;
else {
dx4_1 = 0;
x4_1 = LeadLagOUT * KA;
}
if (dx4_1 > 0 && x4_1 >= Vrmax) dx4_1 = 0;
if (dx4_1 < 0 && x4_1 <= Vrmin) dx4_1 = 0;
// State 1
dx1_1 = x4_1 - x1_1 * (KE + Sat(x1));
x1_1 = x1 + (dx1 + dx1_1) / 2.0 * t_inc;
x2_1 = x2 + (dx2 + dx2_1) / 2.0 * t_inc;
x3_1 = x3 + (dx3 + dx3_1) / 2.0 * t_inc;
x4_1 = x4 + (dx4 + dx4_1) / 2.0 * t_inc;
x5_1 = x5 + (dx5 + dx5_1) / 2.0 * t_inc;
///printf("exdc1 dx: %f\t%f\t%f\t%f\t%f\t\n", dx1_1, dx2_1, dx3_1, dx4_1, dx5_1);
///printf("exdc1 x: %f\t%f\t%f\t%f\t%f\n", x1_1, x2_1, x3_1, x4_1, x5_1);
Efd = x1_1 * (1 + w);
///printf("exdc1 Efd: %f\n", Efd);
}
/**
* Initialize exciter model before calculation
* @param mag voltage magnitude
* @param ang voltage angle
* @param ts time step
*/
void gridpack::dynamic_simulation::Exdc1Model::init(double mag, double ang, double ts)
{
///printf("exdc1: Efd = %f\n", Efd);
x1 = Efd;
x4 = Efd * (KE + Sat(Efd));
if (TB > (TS_THRESHOLD * ts))
x3 = (x4 / KA) * (1 - TC / TB); // SJin: x4 is Vr
else
x3 = x4 / KA;
x2 = mag; // SJin: mag is Vterminal
//printf("KF = %f, TF = %f, x1 = %f, ts = %f\n", KF, TF, x1, ts);
if (TF > (TS_THRESHOLD * ts))
x5 = x1 * (KF / TF); // SJin: x1 is Ve
else
x5 = 0.0;
//x5 = 0.0; // Diao: force x5 to 0.0 for now
Vref = mag + x4 / KA;
//printf("Vref = %f\n", Vref);
///printf("exdc1 init: %f\t%f\t%f\t%f\t%f\n", x1, x2, x3, x4, x5);
}
static Sk4f saturation_4f(const Sk4f& s, const Sk4f& d) {
float sa = s[SkPM4f::A];
float sr = s[SkPM4f::R];
float sg = s[SkPM4f::G];
float sb = s[SkPM4f::B];
float da = d[SkPM4f::A];
float dr = d[SkPM4f::R];
float dg = d[SkPM4f::G];
float db = d[SkPM4f::B];
float Dr = dr;
float Dg = dg;
float Db = db;
SetSat(&Dr, &Dg, &Db, Sat(sr, sg, sb) * da);
SetLum(&Dr, &Dg, &Db, sa * da, Lum(dr, dg, db) * sa);
return color_alpha(s * inv_alpha(d) + d * inv_alpha(s) + set_argb(0, Dr, Dg, Db),
sa + da - sa * da);
}
static Sk4f hue_4f(const Sk4f& s, const Sk4f& d) {
float sa = s[SkPM4f::A];
float sr = s[SkPM4f::R];
float sg = s[SkPM4f::G];
float sb = s[SkPM4f::B];
float da = d[SkPM4f::A];
float dr = d[SkPM4f::R];
float dg = d[SkPM4f::G];
float db = d[SkPM4f::B];
float Sr = sr;
float Sg = sg;
float Sb = sb;
SetSat(&Sr, &Sg, &Sb, Sat(dr, dg, db) * sa);
SetLum(&Sr, &Sg, &Sb, sa * da, Lum(dr, dg, db) * sa);
return color_alpha(s * inv_alpha(d) + d * inv_alpha(s) + set_argb(0, Sr, Sg, Sb),
sa + da - sa * da);
}
static int16_t SumBucket(int16_t const * in, unsigned index) {
uint16_t lo = pitch_bounds[index];
uint16_t hi = pitch_bounds[index + 1];
unsigned lo_int = lo >> 8;
unsigned hi_int = hi >> 8;
uint8_t lo_frc = lo & 0xFF;
uint8_t hi_frc = hi & 0xFF;
unsigned count = hi_int - lo_int;
int16_t const * p = in + lo_int;
int32_t result = Sum(p, count);
result -= in[lo_int] / 2;
result -= Trapezoid(lo_frc, in[lo_int], in[lo_int + 1]);
result += in[hi_int] / 2;
result += Trapezoid(hi_frc, in[hi_int], in[hi_int + 1]);
return Sat(result);
}
/**
* Predict new state variables for time step
* @param t_inc time step increment
* @param flag initial step if true
*/
void gridpack::dynamic_simulation::Exdc1Model::predictor(double t_inc, bool flag)
{
if (!flag) {
x1 = x1_1;
x2 = x2_1;
x3 = x3_1;
x4 = x4_1;
x5 = x5_1;
}
//printf("...........%f\t%f\t%f\t%f\t%f\n", x1, x2, x3, x4, x5);
//printf(".........Vterminal = %f\n", Vterminal);
double Feedback;
// State 2
//printf("TR = %f\n", TR);
if (TR > TS_THRESHOLD * t_inc) dx2 = (Vterminal - x2) / TR; // SJin: x2 is Vsens
else x2 = Vterminal; // propogate state immediately
// State 5
if (TF > TS_THRESHOLD * t_inc) {
dx5 = (x1 * KF / TF - x5) / TF;
Feedback = x1 * KF / TF - x5;
} else {
x5 = 0;
Feedback = 0;
}
//printf("TF = %f, t_inc = %f, x5 = %f, dx5 = %f\n", TF, t_inc, x5, dx5);
// State 3
double Vstab = 0.0; // SJin: Output from PSS, set to 0.0 for now.
double LeadLagIN = Vref - x2 + Vstab - Feedback;
//printf("Vref = %f, TB = %f, TC = %f\n", Vref, TB, TC);
double LeadLagOUT;
//printf("LeadLagIN = %f, TC = %f, TB = %f, x3 = %f\n", LeadLagIN, TC, TB, x3);
if (TB > (TS_THRESHOLD * t_inc)) {
dx3 = (LeadLagIN * (1 - TC / TB) - x3) / TB;
LeadLagOUT = LeadLagIN * TC / TB + x3;
} else
LeadLagOUT = LeadLagIN;
// State 4
//printf("x4 = %f, Vrmax = %f, Vrmin = %f, TA = %f, LeadLagOUT = %f, KA = %f\n", x4, Vrmax, Vrmin, TA, LeadLagOUT, KA);
if (x4 > Vrmax) x4 = Vrmax;
if (x4 < Vrmin) x4 = Vrmin;
if (TA > (TS_THRESHOLD * t_inc))
dx4 = (LeadLagOUT * KA - x4) / TA;
else {
dx4 = 0;
x4 = LeadLagOUT * KA;
}
if (dx4 > 0 && x4 >= Vrmax) dx4 = 0;
if (dx4 < 0 && x4 <= Vrmin) dx4 = 0;
// State 1
dx1 = x4 - x1 * (KE + Sat(x1));
x1_1 = x1 + dx1 * t_inc;
x2_1 = x2 + dx2 * t_inc;
x3_1 = x3 + dx3 * t_inc;
x4_1 = x4 + dx4 * t_inc;
x5_1 = x5 + dx5 * t_inc;
//printf("x2 = %f, dx2 = %f, x2_1 = %f\n", x2, dx2, x2_1);
//printf("x5 = %f, dx5 = %f, x5_1 = %f\n", x5, dx5, x5_1);
///printf("exdc1 dx: %f\t%f\t%f\t%f\t%f\t\n", dx1, dx2, dx3, dx4, dx5);
///printf("exdc1 x: %f\t%f\t%f\t%f\t%f\n", x1_1, x2_1, x3_1, x4_1, x5_1);
Efd = x1_1 * (1 + w);
///printf("exdc1 Efd: %f\n", Efd);
}
void PositionCommand::control()
{
geometry_msgs::Twist vel_cmd_msg;
geometry_msgs::Pose current_error_msg ;
Eigen::Matrix<double,6,1> current_error;
if (control_)
{
current_error= goal_pose_ - current_pose_;
// if the error is less than epsilon goal is reached
if(current_error.norm() < .1)
{
ROS_INFO("Reached goal!");
}
accum_error_ = accum_error_+ period_*(current_error + previous_error_)/2.0;
Eigen::Matrix<double,6,6> Cp = Kp * current_error.transpose();
Eigen::Matrix<double,6,6> Ci = Ki * accum_error_.transpose();
Eigen::Matrix<double,6,6> Cd = Kd * (current_error - previous_error_).transpose()/period_;
//std::cout << "Cp =" << Cp << std::endl;
//std::cout << "Ci =" << Ci << std::endl;
//std::cout << "Cd =" << Cd << std::endl;
Eigen::Matrix<double,6,6> vel_cmd_current = Cp + Ci + Cd ;
Eigen::Matrix<double,3,1> linear_vel_cmd_l_frame;
linear_vel_cmd_l_frame << vel_cmd_current(0,0),
vel_cmd_current(1,1),
vel_cmd_current(2,2);
Eigen::Matrix<double,3,1> linear_vel_cmd_g_frame = rot_matrix_.inverse() * linear_vel_cmd_l_frame;
if (chirp_enable_)
{
linear_vel_cmd_g_frame(0,0) = linear_vel_cmd_g_frame(0,0) + chirp_command();
}
vel_cmd_msg.linear.x = Sat(linear_vel_cmd_g_frame(0,0),1,-1);
vel_cmd_msg.linear.y = Sat(linear_vel_cmd_g_frame(1,0),1,-1);
vel_cmd_msg.linear.z = Sat(linear_vel_cmd_g_frame(2,0),1,-1);
vel_cmd_msg.angular.z = Sat(vel_cmd_current(5,5),1,-1);
previous_error_ = current_error;
control_ = false;
current_error_pub.publish(current_error_msg);
std::cout << "CE: x =" << current_error(0,0)
<< " y = " << current_error(1,0)
<< " z = " << current_error(2,0)
<< " w = " << current_error(5,0)
<< std::endl;
}
current_error_msg.position.x = current_error(0,0);
current_error_msg.position.y = current_error(1,0);
current_error_msg.position.z = current_error(2,0);
current_error_msg.orientation.x = current_error(0,0);
current_error_msg.orientation.y = current_error(1,0);
current_error_msg.orientation.z = current_error(2,0);
vel_cmd.publish(vel_cmd_msg);
// std::cout << "AE: x =" << accum_error_(0,0)
// << " y = " << accum_error_(1,0)
// << " z = " << accum_error_(2,0)
// << " w = " << accum_error_(5,0)
// << std::endl;
std::cout << "Kp = " << Kp.transpose()<< std::endl;
std::cout << "Ki = " << Ki.transpose()<< std::endl;
std::cout << "Kd = " << Kd.transpose()<< std::endl;
std::cout << "vel: x =" << vel_cmd_msg.linear.x
<< " y = " << vel_cmd_msg.linear.y
<< " z = " << vel_cmd_msg.linear.z
<< " w = " << vel_cmd_msg.angular.z
<< std::endl;
}
void HexagonView::Draw(sf::RenderTarget* rt) const
{
if(!m_model) {
return;
}
const double time = m_model->GetTime();
const double hexagonRadius = 1.0 + 0.15 * sin(time * 10);
const double playerRadius = 1.4;
const int numSides = m_model->GetNumSides();
const double zoom = 1.0;
const double w = zoom * 16;
const double h = zoom * 9;
sf::View view(sf::FloatRect(-w/2,-h/2,w,h));
view.setRotation(m_model->GetRotation());
rt->setView(view);
//Draw bg
sf::ConvexShape bgSide(4);
for(int i = 0; i < numSides; ++i) {
bgSide.setFillColor(HSVtoRGB(Hue(), Sat(), 0.8));
const double in = hexagonRadius - 0.1 + 0.05 * sin(time * 5);
ConstructSideShape(bgSide, i, numSides, in, hexagonRadius);
rt->draw(bgSide);
bgSide.setFillColor(HSVtoRGB(Hue(), Sat(), 0.2));
ConstructSideShape(bgSide, i, numSides, 0, in);
rt->draw(bgSide);
if(i % 2) {
bgSide.setFillColor(HSVtoRGB(Hue(), Sat(), 0.2));
} else {
bgSide.setFillColor(HSVtoRGB(Hue(), Sat(), 0.3));
}
ConstructSideShape(bgSide, i, numSides, hexagonRadius, 32);
rt->draw(bgSide);
}
//Draw obstacles
sf::ConvexShape obsShape(4);
obsShape.setFillColor(HSVtoRGB(Hue(), Sat(), 0.8));
for(int i = 0; i < numSides; i++) {
Obstacle* obs = m_model->GetObstacle(i);
while(obs) {
const double pd = m_model->GetPlayerDistance();
double start = obs->start - pd + playerRadius;
const double end = start + obs->end - obs->start;
if(start < hexagonRadius) {
start = hexagonRadius;
}
if(end > hexagonRadius) {
ConstructSideShape(obsShape, i, numSides, start, end);
rt->draw(obsShape);
}
obs = obs->next;
}
}
//Draw player
if(m_drawPlayer) {
sf::ConvexShape playerShape(3);
const double pos = m_model->GetPlayerPosition();
//Get the two corners the player is between
const int posMin = floor(pos);
const int posMax = ceil(pos);
const double posMinX = playerRadius * cos(posMin * 2 * M_PI / numSides);
const double posMinY = playerRadius * sin(posMin * 2 * M_PI / numSides);
const double posMaxX = playerRadius * cos(posMax * 2 * M_PI / numSides);
const double posMaxY = playerRadius * sin(posMax * 2 * M_PI / numSides);
const double lerp = pos - posMin;
const double posX = LInterp(lerp, posMinX, posMaxX);
const double posY = LInterp(lerp, posMinY, posMaxY);
const double pulseScale = 0.1 + 0.025 * sin(time * 10);
const double turnMod = m_model->IsGameOver() ? 0 : m_model->GetPlayerDirection() * 15;
playerShape.setPoint(0, sf::Vector2f( -0.20, -pulseScale ) );
playerShape.setPoint(1, sf::Vector2f( 0, 0 ) );
playerShape.setPoint(2, sf::Vector2f( -0.20, pulseScale ) );
playerShape.setFillColor(HSVtoRGB(Hue(), Sat(), 1.0));
playerShape.setPosition(posX,posY);
playerShape.setRotation(pos * 360 / numSides + turnMod);
rt->draw(playerShape);
}
}
