Version 3 is with update to the test rig with a linear actuator
Dependencies: SPTE_10Bar_5V mbed AS5048 SDFileSystem MODSERIAL PinDetect LCM101 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?
User | Revision | Line number | New 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 |