Example #1
0
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;
}
Example #2
0
//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