float Motor::update() {
float pos = getPosition();
// Velocity calculation should happen independent of mode
float error = fmodf_mpi_pi(pos - setpoint);
float posCtrlVal = pd.update(error);
// In position mode, update the motor command
if (mode == POSITION_MODE)
val = posCtrlVal;
// In open-loop mode, val has been set and nothing else needs to be done
// Send command, but don't modify "val" (set by user)
correctedVal = mapVal(val + barrier.calculate(pos));
// send the command
sendOpenLoop(correctedVal);
// Return the command so that the slave can do the same thing
return correctedVal;
}
//MAIN RENDER CODE ================================================
void DataGrid::render()
{
//vector<T> xData(_xData); //Assign xData to non-const so we can pass by const reference.
// Add the border if flagged
if (flags & (int)o_t::BORDER) {
addBorder();
const int shrink = 4;
plotArea.setSize(plotArea.getWidth()-shrink, plotArea.getHeight()-shrink);
plotArea.translate(shrink/2, shrink/2);
}
// Add the title if flagged
if (flags & (int)o_t::TITLE) {
addTitle();
}
// Abort if series vec is empty
if (series.getNumSeries()==0){
cout << "No data series available, aborting. \n";
return;
}
// get ranges
const float xMin = series.getXmin();
const float xMax = series.getXmax();
const float yMin = series.getYmin();
const float yMax = series.getYmax();
// Another way of aliasing - pointless here bu I <3 lambdas
// To use this alternative, replace xMin with xMin() below
// auto xMin = [&](){return series.getXmin();};
// Add the y-axis if flagged
if (flags & (int)o_t::YAXIS) {
const int yaxWidth = 10;
Rectangle rAx = Rectangle(plotArea.getTL(), yaxWidth, plotArea.getHeight());
addYAxis(yMin, yMax, rAx);
// Shift the plot area
plotArea.setWidth(plotArea.getWidth()-yaxWidth);
plotArea.translate(yaxWidth, 0);
}
// Add simple x-axis indicators if requested
if (flags & (int)o_t::XAXIS) {
addXAxis(xMin, xMax, plotArea);
}
for (size_t ii=0; ii<series.getNumSeries(); ++ii){
// Warn if input is messed up
vector<float> xData(series.getXdata(ii));
vector<float> yData(series.getYdata(ii));
if (xData.size()!=yData.size()) {
cout << "WARNING: x and y data size mismatch, not plotting.\n";
return;
}
for(size_t nn=0; nn<xData.size(); ++nn){
int intX = mapVal( xData[nn], xMin, xMax, plotArea.getLeft(), plotArea.getRight() );
// Notice the subtle reversal of yMax and yMin here so data flipped the correct way
int intY = mapVal( yData[nn], yMax, yMin, plotArea.getTop(), plotArea.getBtm() );
addPoint(Point(intX, intY), series.getMarker(ii) );
}
}
// Add the legend if flagged
if (flags & (int)o_t::LEGEND) {
addLegend();
}
} // End of rendering function