Diff: bench.cpp

Revision:

0:3855d4588f76

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/bench.cpp	Fri Dec 01 11:14:38 2017 +0000
@@ -0,0 +1,97 @@
+#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();
+}
+
+
+
+