File content as of revision 3:3fd9f1a00381:

#include "bench.h"

/**
 * Create an object representing the testbench; the 4 AS5048 sensors
 * with the offsets specified by the constants kOffsetsDegrees[4]
 * @param mosi: mosi pin for sensor chain
 * @param miso: miso pin for sensor chain
 * @param sck: clock pin for sensor chain
 * @param cs: chip select pin for sensor chain
 */
Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
      PinName p_lcm101) :
    as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
    lcm101_(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor) 
{
    for (int i=0; i<sensors::kNumJoints; ++i) {
        as5048_.setOffsetDegrees(i,sensors::kOffsetsDegrees[i]);
        as5048_.setDirection(i,sensors::kDirections[i]);
    }
}

/**
 * Update routine for the testbench.
 * - Updates the angle buffer of the sensor array
 * - ... that's it for now (add filtering?)
 * Note that angles lag one Update() behind, due to the way the SPI
 * protocol works.
 */
void Bench::Update() 
{
    as5048_.UpdateAngleBuffer();
}

float Bench::getDegrees(int i_joint) 
{
    float ang = as5048_.getAngleDegrees(i_joint);
    if (ang>kCutOffDegrees) {
        return ang-As5048::kDegPerRev;
    }
    return ang;
}

/**
 * Obtain the joint angle in degrees.
 * These are the angles at the time of two Update() calls back
 * @param joint: the joint for which the angle is requested
 * @return: joint angle
 */
float Bench::getDegrees(Joint joint) 
{
    return getDegrees(joint);
}

float Bench::getRadians(int i_joint) 
{
    float ang = as5048_.getAngleRadians(i_joint);
    if (ang>kCutOffRadians) {
        return ang-As5048::kRadPerRev;
    }
    return ang;
}

/**
 * Obtain the joint angle in radians.
 * These are the angles at the time of two Update() calls back
 * @param joint: the joint for which the angle is requested
 * @return: joint angle
 */
float Bench::getRadians(Joint joint) 
{
    return getRadians(joint);
}


const char* Bench::getJointName(int i_joint) 
{
    return sensors::kJointNames[i_joint];
}

const char* Bench::getJointName(Joint joint) 
{
    return getJointName(joint);
}

As5048* Bench::get_as5048() 
{
    return &as5048_;
}

float Bench::getForce() 
{
    return lcm101_.getForce();
}