bench.cpp

00001 #include "bench.h"
00002 
00003 /**
00004  * Create an object representing the testbench; the 4 AS5048 sensors
00005  * with the offsets specified by the constants kOffsetsDegrees[4]
00006  * @param mosi: mosi pin for sensor chain
00007  * @param miso: miso pin for sensor chain
00008  * @param sck: clock pin for sensor chain
00009  * @param cs: chip select pin for sensor chain
00010  */
00011 Bench::Bench(PinName mosi, PinName miso, PinName sck, PinName cs,
00012       PinName p_lcm101) :
00013     as5048_(mosi, miso, sck, cs, sensors::kNumJoints),
00014     lcm101_(p_lcm101, sensors::kLcm101Offset, sensors::kLcm101Factor) 
00015 {
00016     for (int i=0; i<sensors::kNumJoints; ++i) {
00017         as5048_.setOffsetDegrees(i,sensors::kOffsetsDegrees[i]);
00018         as5048_.setDirection(i,sensors::kDirections[i]);
00019     }
00020 }
00021 
00022 /**
00023  * Update routine for the testbench.
00024  * - Updates the angle buffer of the sensor array
00025  * - ... that's it for now (add filtering?)
00026  * Note that angles lag one Update() behind, due to the way the SPI
00027  * protocol works.
00028  */
00029 void Bench::Update() 
00030 {
00031     as5048_.UpdateAngleBuffer();
00032 }
00033 
00034 float Bench::getDegrees(int i_joint) 
00035 {
00036     float ang = as5048_.getAngleDegrees(i_joint);
00037     if (ang>kCutOffDegrees) {
00038         return ang-As5048::kDegPerRev;
00039     }
00040     return ang;
00041 }
00042 
00043 /**
00044  * Obtain the joint angle in degrees.
00045  * These are the angles at the time of two Update() calls back
00046  * @param joint: the joint for which the angle is requested
00047  * @return: joint angle
00048  */
00049 float Bench::getDegrees(Joint joint) 
00050 {
00051     return getDegrees(joint);
00052 }
00053 
00054 float Bench::getRadians(int i_joint) 
00055 {
00056     float ang = as5048_.getAngleRadians(i_joint);
00057     if (ang>kCutOffRadians) {
00058         return ang-As5048::kRadPerRev;
00059     }
00060     return ang;
00061 }
00062 
00063 /**
00064  * Obtain the joint angle in radians.
00065  * These are the angles at the time of two Update() calls back
00066  * @param joint: the joint for which the angle is requested
00067  * @return: joint angle
00068  */
00069 float Bench::getRadians(Joint joint) 
00070 {
00071     return getRadians(joint);
00072 }
00073 
00074 
00075 const char* Bench::getJointName(int i_joint) 
00076 {
00077     return sensors::kJointNames[i_joint];
00078 }
00079 
00080 const char* Bench::getJointName(Joint joint) 
00081 {
00082     return getJointName(joint);
00083 }
00084 
00085 As5048* Bench::get_as5048() 
00086 {
00087     return &as5048_;
00088 }
00089 
00090 float Bench::getForce() 
00091 {
00092     return lcm101_.getForce();
00093 }
00094 
00095 
00096 
00097