rttaskfunctions.cpp

#include "rttaskglobals.h"
 
#include <cstdlib>
#include <ctime>
#include <cmath>
 
#include <iostream>
 
using namespace RSI::RapidCode;
using namespace RSI::RapidCode::RealTimeTasks;

// initialize the global data and random number generator
RSI_TASK(Initialize)
{
  data->counter = 0;
 
  data->sensorValue = 0;
  data->startingSample = 0;
 
  std::srand(std::time(nullptr));
}

// increment the counter in the global data
RSI_TASK(Increment)
{
  data->counter += 1;
}
 
// Simulate getting sensor value with sine wave
double getSensorValue(MotionController *controller, int32_t startingSample) {
  double FREQUENCY = 0.5;   // Oscillations/second
  double AMPLITUDE = 0.25;  // Oscillation strength (user units)
  double PI = 3.1415;       // Approximation of pi
 
  double secondsElapsed = ((double) controller->SampleCounterGet() - startingSample) / controller->SampleRateGet();
  return AMPLITUDE * std::sin(2 * PI * FREQUENCY * secondsElapsed);
}

//  move the axis to follow a simulated sensor value
RSI_TASK(FollowSensor)
{
  MotionController *controller = RTMotionControllerGet();
  if (data->startingSample == 0) {
    data->startingSample = controller->SampleCounterGet();
  }
  data->sensorValue = getSensorValue(controller, data->startingSample);
 
  RTAxisGet(0)->MoveTrapezoidal(data->sensorValue, 5, 10, 10);
}