Code for the COVR project DROPSAW project test rig (previously used for the Heros testing).
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL LCM101_DROPSAW LinearActuator
Diff: bench.cpp
- Revision:
- 0:3855d4588f76
- Child:
- 4:1cdce6c6c94e
diff -r 000000000000 -r 3855d4588f76 bench.cpp --- /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(); +} + + + +