Allan Veale / Mbed 2 deprecated DROPSAWTestRigCode_V4

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

bench.cpp@4:1cdce6c6c94e, 2018-10-12 (annotated)

Committer:
cnckiwi31
Date:
Fri Oct 12 12:12:55 2018 +0000
Revision:
4:1cdce6c6c94e
Parent:
0:3855d4588f76
Child:
5:63063a9fa51c
HeRoS: read out and log joint angles and force sensor data from the leg test bench.

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
cnckiwi31 4:1cdce6c6c94e 10 * @param p_lcm101: analog input pin for load cell
cnckiwi31 4:1cdce6c6c94e 11 * @param p_spte0: analog input pin for pressure sensor 0
cnckiwi31 4:1cdce6c6c94e 12 * @param p_spte1: analog input pin for pressure sensor 1
megrootens 0:3855d4588f76 13 */
megrootens 0:3855d4588f76 14 Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
cnckiwi31 4:1cdce6c6c94e 15 PinName p_lcm101, PinName p_spte0, PinName p_spte1) :
megrootens 0:3855d4588f76 16 as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
cnckiwi31 4:1cdce6c6c94e 17 lcm101_(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor),
cnckiwi31 4:1cdce6c6c94e 18 spte0(p_spte0, sensors::kSPTE0Offset, sensors::kSPTE0Factor),
cnckiwi31 4:1cdce6c6c94e 19 spte1(p_spte1, sensors::kSPTE1Offset, sensors::kSPTE1Factor)
megrootens 0:3855d4588f76 20 {
megrootens 0:3855d4588f76 21 for (int i=0; i<sensors::kNumJoints; ++i) {
megrootens 0:3855d4588f76 22 as5048_.setOffsetDegrees(i,sensors::kOffsetsDegrees[i]);
megrootens 0:3855d4588f76 23 as5048_.setDirection(i,sensors::kDirections[i]);
megrootens 0:3855d4588f76 24 }
megrootens 0:3855d4588f76 25 }
megrootens 0:3855d4588f76 26
megrootens 0:3855d4588f76 27 /**
megrootens 0:3855d4588f76 28 * Update routine for the testbench.
megrootens 0:3855d4588f76 29 * - Updates the angle buffer of the sensor array
megrootens 0:3855d4588f76 30 * - ... that's it for now (add filtering?)
megrootens 0:3855d4588f76 31 * Note that angles lag one Update() behind, due to the way the SPI
megrootens 0:3855d4588f76 32 * protocol works.
megrootens 0:3855d4588f76 33 */
megrootens 0:3855d4588f76 34 void Bench::Update()
megrootens 0:3855d4588f76 35 {
megrootens 0:3855d4588f76 36 as5048_.UpdateAngleBuffer();
megrootens 0:3855d4588f76 37 }
megrootens 0:3855d4588f76 38
megrootens 0:3855d4588f76 39 float Bench::getDegrees(int i_joint)
megrootens 0:3855d4588f76 40 {
megrootens 0:3855d4588f76 41 float ang = as5048_.getAngleDegrees(i_joint);
megrootens 0:3855d4588f76 42 if (ang>kCutOffDegrees) {
megrootens 0:3855d4588f76 43 return ang-As5048::kDegPerRev;
megrootens 0:3855d4588f76 44 }
megrootens 0:3855d4588f76 45 return ang;
megrootens 0:3855d4588f76 46 }
megrootens 0:3855d4588f76 47
megrootens 0:3855d4588f76 48 /**
megrootens 0:3855d4588f76 49 * Obtain the joint angle in degrees.
megrootens 0:3855d4588f76 50 * These are the angles at the time of two Update() calls back
megrootens 0:3855d4588f76 51 * @param joint: the joint for which the angle is requested
megrootens 0:3855d4588f76 52 * @return: joint angle
megrootens 0:3855d4588f76 53 */
megrootens 0:3855d4588f76 54 float Bench::getDegrees(Joint joint)
megrootens 0:3855d4588f76 55 {
megrootens 0:3855d4588f76 56 return getDegrees(joint);
megrootens 0:3855d4588f76 57 }
megrootens 0:3855d4588f76 58
megrootens 0:3855d4588f76 59 float Bench::getRadians(int i_joint)
megrootens 0:3855d4588f76 60 {
megrootens 0:3855d4588f76 61 float ang = as5048_.getAngleRadians(i_joint);
megrootens 0:3855d4588f76 62 if (ang>kCutOffRadians) {
megrootens 0:3855d4588f76 63 return ang-As5048::kRadPerRev;
megrootens 0:3855d4588f76 64 }
megrootens 0:3855d4588f76 65 return ang;
megrootens 0:3855d4588f76 66 }
megrootens 0:3855d4588f76 67
megrootens 0:3855d4588f76 68 /**
megrootens 0:3855d4588f76 69 * Obtain the joint angle in radians.
megrootens 0:3855d4588f76 70 * These are the angles at the time of two Update() calls back
megrootens 0:3855d4588f76 71 * @param joint: the joint for which the angle is requested
megrootens 0:3855d4588f76 72 * @return: joint angle
megrootens 0:3855d4588f76 73 */
megrootens 0:3855d4588f76 74 float Bench::getRadians(Joint joint)
megrootens 0:3855d4588f76 75 {
megrootens 0:3855d4588f76 76 return getRadians(joint);
megrootens 0:3855d4588f76 77 }
megrootens 0:3855d4588f76 78
megrootens 0:3855d4588f76 79
megrootens 0:3855d4588f76 80 const char* Bench::getJointName(int i_joint)
megrootens 0:3855d4588f76 81 {
megrootens 0:3855d4588f76 82 return sensors::kJointNames[i_joint];
megrootens 0:3855d4588f76 83 }
megrootens 0:3855d4588f76 84
megrootens 0:3855d4588f76 85 const char* Bench::getJointName(Joint joint)
megrootens 0:3855d4588f76 86 {
megrootens 0:3855d4588f76 87 return getJointName(joint);
megrootens 0:3855d4588f76 88 }
megrootens 0:3855d4588f76 89
megrootens 0:3855d4588f76 90 As5048* Bench::get_as5048()
megrootens 0:3855d4588f76 91 {
megrootens 0:3855d4588f76 92 return &as5048_;
megrootens 0:3855d4588f76 93 }
megrootens 0:3855d4588f76 94
megrootens 0:3855d4588f76 95 float Bench::getForce()
megrootens 0:3855d4588f76 96 {
megrootens 0:3855d4588f76 97 return lcm101_.getForce();
megrootens 0:3855d4588f76 98 }
megrootens 0:3855d4588f76 99
cnckiwi31 4:1cdce6c6c94e 100 void Bench::nullForce()
cnckiwi31 4:1cdce6c6c94e 101 {
cnckiwi31 4:1cdce6c6c94e 102 return lcm101_.nullForce();
cnckiwi31 4:1cdce6c6c94e 103 }
cnckiwi31 4:1cdce6c6c94e 104
cnckiwi31 4:1cdce6c6c94e 105 float Bench::getPressure0()
cnckiwi31 4:1cdce6c6c94e 106 {
cnckiwi31 4:1cdce6c6c94e 107 return spte0.getPressure();
cnckiwi31 4:1cdce6c6c94e 108 }
cnckiwi31 4:1cdce6c6c94e 109
cnckiwi31 4:1cdce6c6c94e 110 void Bench::nullPressure0()
cnckiwi31 4:1cdce6c6c94e 111 {
cnckiwi31 4:1cdce6c6c94e 112 return spte0.nullPressure();
cnckiwi31 4:1cdce6c6c94e 113 }
cnckiwi31 4:1cdce6c6c94e 114
cnckiwi31 4:1cdce6c6c94e 115 float Bench::getPressure1()
cnckiwi31 4:1cdce6c6c94e 116 {
cnckiwi31 4:1cdce6c6c94e 117 return spte1.getPressure();
cnckiwi31 4:1cdce6c6c94e 118 }
cnckiwi31 4:1cdce6c6c94e 119
cnckiwi31 4:1cdce6c6c94e 120 void Bench::nullPressure1()
cnckiwi31 4:1cdce6c6c94e 121 {
cnckiwi31 4:1cdce6c6c94e 122 return spte1.nullPressure();
cnckiwi31 4:1cdce6c6c94e 123 }
megrootens 0:3855d4588f76 124
megrootens 0:3855d4588f76 125