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@3:3fd9f1a00381, 2018-06-05 (annotated)

Committer:
Technical_Muffin
Date:
Tue Jun 05 08:51:45 2018 +0000
Revision:
3:3fd9f1a00381
Parent:
0:3855d4588f76 
added torque calculation to the main file of the code. New calibration values were also added to correctly display the values.

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