K K / Mbed 2 deprecated heros_leg_readout_torque_addition

Torque calculation added to leg readout

Dependencies:   AS5048 LCM101 MODSERIAL PinDetect SDFileSystem mbed

Fork of heros_leg_readout by Martijn Grootens

bench.cpp@0:3855d4588f76, 2017-12-01 (annotated)

Committer:
megrootens
Date:
Fri Dec 01 11:14:38 2017 +0000
Revision:
0:3855d4588f76
serial printing and SD card logging of joint angles and force sensor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
megrootens 0:3855d4588f76 1 #include "bench.h"
megrootens 0:3855d4588f76 2
megrootens 0:3855d4588f76 3 /**
megrootens 0:3855d4588f76 4 * Create an object representing the testbench; the 4 AS5048 sensors
megrootens 0:3855d4588f76 5 * with the offsets specified by the constants kOffsetsDegrees[4]
megrootens 0:3855d4588f76 6 * @param mosi: mosi pin for sensor chain
megrootens 0:3855d4588f76 7 * @param miso: miso pin for sensor chain
megrootens 0:3855d4588f76 8 * @param sck: clock pin for sensor chain
megrootens 0:3855d4588f76 9 * @param cs: chip select pin for sensor chain
megrootens 0:3855d4588f76 10 */
megrootens 0:3855d4588f76 11 Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
megrootens 0:3855d4588f76 12 PinName p_lcm101) :
megrootens 0:3855d4588f76 13 as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
megrootens 0:3855d4588f76 14 lcm101_(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor)
megrootens 0:3855d4588f76 15 {
megrootens 0:3855d4588f76 16 for (int i=0; i<sensors::kNumJoints; ++i) {
megrootens 0:3855d4588f76 17 as5048_.setOffsetDegrees(i,sensors::kOffsetsDegrees[i]);
megrootens 0:3855d4588f76 18 as5048_.setDirection(i,sensors::kDirections[i]);
megrootens 0:3855d4588f76 19 }
megrootens 0:3855d4588f76 20 }
megrootens 0:3855d4588f76 21
megrootens 0:3855d4588f76 22 /**
megrootens 0:3855d4588f76 23 * Update routine for the testbench.
megrootens 0:3855d4588f76 24 * - Updates the angle buffer of the sensor array
megrootens 0:3855d4588f76 25 * - ... that's it for now (add filtering?)
megrootens 0:3855d4588f76 26 * Note that angles lag one Update() behind, due to the way the SPI
megrootens 0:3855d4588f76 27 * protocol works.
megrootens 0:3855d4588f76 28 */
megrootens 0:3855d4588f76 29 void Bench::Update()
megrootens 0:3855d4588f76 30 {
megrootens 0:3855d4588f76 31 as5048_.UpdateAngleBuffer();
megrootens 0:3855d4588f76 32 }
megrootens 0:3855d4588f76 33
megrootens 0:3855d4588f76 34 float Bench::getDegrees(int i_joint)
megrootens 0:3855d4588f76 35 {
megrootens 0:3855d4588f76 36 float ang = as5048_.getAngleDegrees(i_joint);
megrootens 0:3855d4588f76 37 if (ang>kCutOffDegrees) {
megrootens 0:3855d4588f76 38 return ang-As5048::kDegPerRev;
megrootens 0:3855d4588f76 39 }
megrootens 0:3855d4588f76 40 return ang;
megrootens 0:3855d4588f76 41 }
megrootens 0:3855d4588f76 42
megrootens 0:3855d4588f76 43 /**
megrootens 0:3855d4588f76 44 * Obtain the joint angle in degrees.
megrootens 0:3855d4588f76 45 * These are the angles at the time of two Update() calls back
megrootens 0:3855d4588f76 46 * @param joint: the joint for which the angle is requested
megrootens 0:3855d4588f76 47 * @return: joint angle
megrootens 0:3855d4588f76 48 */
megrootens 0:3855d4588f76 49 float Bench::getDegrees(Joint joint)
megrootens 0:3855d4588f76 50 {
megrootens 0:3855d4588f76 51 return getDegrees(joint);
megrootens 0:3855d4588f76 52 }
megrootens 0:3855d4588f76 53
megrootens 0:3855d4588f76 54 float Bench::getRadians(int i_joint)
megrootens 0:3855d4588f76 55 {
megrootens 0:3855d4588f76 56 float ang = as5048_.getAngleRadians(i_joint);
megrootens 0:3855d4588f76 57 if (ang>kCutOffRadians) {
megrootens 0:3855d4588f76 58 return ang-As5048::kRadPerRev;
megrootens 0:3855d4588f76 59 }
megrootens 0:3855d4588f76 60 return ang;
megrootens 0:3855d4588f76 61 }
megrootens 0:3855d4588f76 62
megrootens 0:3855d4588f76 63 /**
megrootens 0:3855d4588f76 64 * Obtain the joint angle in radians.
megrootens 0:3855d4588f76 65 * These are the angles at the time of two Update() calls back
megrootens 0:3855d4588f76 66 * @param joint: the joint for which the angle is requested
megrootens 0:3855d4588f76 67 * @return: joint angle
megrootens 0:3855d4588f76 68 */
megrootens 0:3855d4588f76 69 float Bench::getRadians(Joint joint)
megrootens 0:3855d4588f76 70 {
megrootens 0:3855d4588f76 71 return getRadians(joint);
megrootens 0:3855d4588f76 72 }
megrootens 0:3855d4588f76 73
megrootens 0:3855d4588f76 74
megrootens 0:3855d4588f76 75 const char* Bench::getJointName(int i_joint)
megrootens 0:3855d4588f76 76 {
megrootens 0:3855d4588f76 77 return sensors::kJointNames[i_joint];
megrootens 0:3855d4588f76 78 }
megrootens 0:3855d4588f76 79
megrootens 0:3855d4588f76 80 const char* Bench::getJointName(Joint joint)
megrootens 0:3855d4588f76 81 {
megrootens 0:3855d4588f76 82 return getJointName(joint);
megrootens 0:3855d4588f76 83 }
megrootens 0:3855d4588f76 84
megrootens 0:3855d4588f76 85 As5048* Bench::get_as5048()
megrootens 0:3855d4588f76 86 {
megrootens 0:3855d4588f76 87 return &as5048_;
megrootens 0:3855d4588f76 88 }
megrootens 0:3855d4588f76 89
megrootens 0:3855d4588f76 90 float Bench::getForce()
megrootens 0:3855d4588f76 91 {
megrootens 0:3855d4588f76 92 return lcm101_.getForce();
megrootens 0:3855d4588f76 93 }
megrootens 0:3855d4588f76 94
megrootens 0:3855d4588f76 95
megrootens 0:3855d4588f76 96
megrootens 0:3855d4588f76 97