Jonas Forsslund / Mbed 2 deprecated WoodenHapticsHID

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed FastPWM USBDevice

Fork of USBHID_TestCase by Samuel Mokrani

WoodenDevice.h@7:bb6454b72c57, 2016-10-28 (annotated)

Committer:
jofo
Date:
Fri Oct 28 12:34:49 2016 +0000
Revision:
7:bb6454b72c57
Parent:
4:3ab1e94b3bc4 
dont know if anything changed but this thing works 2016-10-28

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jofo 4:3ab1e94b3bc4 1 //! A collection of variables that can be set in ~/wooden_haptics.json
jofo 4:3ab1e94b3bc4 2 struct configuration {
jofo 4:3ab1e94b3bc4 3 double diameter_capstan_a; // m
jofo 4:3ab1e94b3bc4 4 double diameter_capstan_b; // m
jofo 4:3ab1e94b3bc4 5 double diameter_capstan_c; // m
jofo 4:3ab1e94b3bc4 6 double length_body_a; // m
jofo 4:3ab1e94b3bc4 7 double length_body_b; // m
jofo 4:3ab1e94b3bc4 8 double length_body_c; // m
jofo 4:3ab1e94b3bc4 9 double diameter_body_a; // m
jofo 4:3ab1e94b3bc4 10 double diameter_body_b; // m
jofo 4:3ab1e94b3bc4 11 double diameter_body_c; // m
jofo 4:3ab1e94b3bc4 12 double workspace_origin_x; // m
jofo 4:3ab1e94b3bc4 13 double workspace_origin_y; // m
jofo 4:3ab1e94b3bc4 14 double workspace_origin_z; // m
jofo 4:3ab1e94b3bc4 15 double workspace_radius; // m (for application information)
jofo 4:3ab1e94b3bc4 16 double torque_constant_motor_a; // Nm/A
jofo 4:3ab1e94b3bc4 17 double torque_constant_motor_b; // Nm/A
jofo 4:3ab1e94b3bc4 18 double torque_constant_motor_c; // Nm/A
jofo 4:3ab1e94b3bc4 19 double current_for_10_v_signal; // A
jofo 4:3ab1e94b3bc4 20 double cpr_encoder_a; // quadrupled counts per revolution
jofo 4:3ab1e94b3bc4 21 double cpr_encoder_b; // quadrupled counts per revolution
jofo 4:3ab1e94b3bc4 22 double cpr_encoder_c; // quadrupled counts per revolution
jofo 4:3ab1e94b3bc4 23 double max_linear_force; // N
jofo 4:3ab1e94b3bc4 24 double max_linear_stiffness; // N/m
jofo 4:3ab1e94b3bc4 25 double max_linear_damping; // N/(m/s)
jofo 4:3ab1e94b3bc4 26 double mass_body_b; // Kg
jofo 4:3ab1e94b3bc4 27 double mass_body_c; // Kg
jofo 4:3ab1e94b3bc4 28 double length_cm_body_b; // m distance to center of mass
jofo 4:3ab1e94b3bc4 29 double length_cm_body_c; // m from previous body
jofo 4:3ab1e94b3bc4 30 double g_constant; // m/s^2 usually 9.81 or 0 to
jofo 4:3ab1e94b3bc4 31 // disable gravity compensation
jofo 4:3ab1e94b3bc4 32
jofo 4:3ab1e94b3bc4 33 // Set values
jofo 4:3ab1e94b3bc4 34 configuration(const double* k):
jofo 4:3ab1e94b3bc4 35 diameter_capstan_a(k[0]), diameter_capstan_b(k[1]), diameter_capstan_c(k[2]),
jofo 4:3ab1e94b3bc4 36 length_body_a(k[3]), length_body_b(k[4]), length_body_c(k[5]),
jofo 4:3ab1e94b3bc4 37 diameter_body_a(k[6]), diameter_body_b(k[7]), diameter_body_c(k[8]),
jofo 4:3ab1e94b3bc4 38 workspace_origin_x(k[9]), workspace_origin_y(k[10]), workspace_origin_z(k[11]),
jofo 4:3ab1e94b3bc4 39 workspace_radius(k[12]), torque_constant_motor_a(k[13]),
jofo 4:3ab1e94b3bc4 40 torque_constant_motor_b(k[14]), torque_constant_motor_c(k[15]),
jofo 4:3ab1e94b3bc4 41 current_for_10_v_signal(k[16]), cpr_encoder_a(k[17]), cpr_encoder_b(k[18]),
jofo 4:3ab1e94b3bc4 42 cpr_encoder_c(k[19]), max_linear_force(k[20]), max_linear_stiffness(k[21]),
jofo 4:3ab1e94b3bc4 43 max_linear_damping(k[22]), mass_body_b(k[23]), mass_body_c(k[24]),
jofo 4:3ab1e94b3bc4 44 length_cm_body_b(k[25]), length_cm_body_c(k[26]), g_constant(k[27]){}
jofo 4:3ab1e94b3bc4 45 };
jofo 4:3ab1e94b3bc4 46
jofo 4:3ab1e94b3bc4 47
jofo 4:3ab1e94b3bc4 48 configuration default_woody(){
jofo 4:3ab1e94b3bc4 49 double data[] = { 0.010, 0.010, 0.010,
jofo 4:3ab1e94b3bc4 50 0.080, 0.205, 0.200,
jofo 4:3ab1e94b3bc4 51 0.160, 0.120, 0.120,
jofo 4:3ab1e94b3bc4 52 0.220, 0.000, 0.080, 0.100,
jofo 4:3ab1e94b3bc4 53 0.0603, 0.0603, 0.0603, 3.0, 4096, 2000, 2000,
jofo 4:3ab1e94b3bc4 54 12.0, 5000.0, 8.0,
jofo 4:3ab1e94b3bc4 55 0.170, 0.110, 0.051, 0.091, 9.81};
jofo 4:3ab1e94b3bc4 56 return configuration(data);
jofo 4:3ab1e94b3bc4 57 }
jofo 4:3ab1e94b3bc4 58
jofo 4:3ab1e94b3bc4 59 //==============================================================================
jofo 4:3ab1e94b3bc4 60 // Helper functions for getPosition & setForce
jofo 4:3ab1e94b3bc4 61 //==============================================================================
jofo 4:3ab1e94b3bc4 62 double getMotorAngle(int motor, const double cpr, const int encoderValue) {
jofo 4:3ab1e94b3bc4 63 const double pi = 3.14159265359;
jofo 4:3ab1e94b3bc4 64 return 2.0*pi*encoderValue/cpr;
jofo 4:3ab1e94b3bc4 65 }
jofo 4:3ab1e94b3bc4 66 /*
jofo 4:3ab1e94b3bc4 67 void setVolt(double v, int motor){
jofo 4:3ab1e94b3bc4 68 if(v > 10 || v< -10) { printf("Volt outside +/- 10 Volt\n"); return; }
jofo 4:3ab1e94b3bc4 69
jofo 4:3ab1e94b3bc4 70 // -10V to +10V is mapped from 0x0000 to 0xFFFF
jofo 4:3ab1e94b3bc4 71 unsigned int signal = (v+10.0)/20.0 * 0xFFFF;
jofo 4:3ab1e94b3bc4 72 S826_DacDataWrite(0,motor,signal,0);
jofo 4:3ab1e94b3bc4 73 }
jofo 4:3ab1e94b3bc4 74 */
jofo 4:3ab1e94b3bc4 75
jofo 4:3ab1e94b3bc4 76 struct pose {
jofo 4:3ab1e94b3bc4 77 double Ln;
jofo 4:3ab1e94b3bc4 78 double Lb;
jofo 4:3ab1e94b3bc4 79 double Lc;
jofo 4:3ab1e94b3bc4 80 double tA; // angle of body A (theta_A)
jofo 4:3ab1e94b3bc4 81 double tB; // angle of body B (theta_B)
jofo 4:3ab1e94b3bc4 82 double tC; // angle of body C (theta_C)
jofo 4:3ab1e94b3bc4 83 };
jofo 4:3ab1e94b3bc4 84
jofo 4:3ab1e94b3bc4 85 pose calculate_pose(const configuration& c, int* encoderValues) {
jofo 4:3ab1e94b3bc4 86 pose p;
jofo 4:3ab1e94b3bc4 87
jofo 4:3ab1e94b3bc4 88 double cpr[] = { c.cpr_encoder_a, c.cpr_encoder_b, c.cpr_encoder_c };
jofo 4:3ab1e94b3bc4 89 double gearRatio[] = { -c.diameter_body_a / c.diameter_capstan_a,
jofo 4:3ab1e94b3bc4 90 -c.diameter_body_b / c.diameter_capstan_b,
jofo 4:3ab1e94b3bc4 91 c.diameter_body_c / c.diameter_capstan_c };
jofo 4:3ab1e94b3bc4 92
jofo 4:3ab1e94b3bc4 93 double dofAngle[3];
jofo 4:3ab1e94b3bc4 94 for(int i=0;i<3;i++)
jofo 4:3ab1e94b3bc4 95 dofAngle[i] = getMotorAngle(i,cpr[i],encoderValues[i]) / gearRatio[i];
jofo 4:3ab1e94b3bc4 96
jofo 4:3ab1e94b3bc4 97 // Calculate dof angles (theta) for each body
jofo 4:3ab1e94b3bc4 98 p.Ln = c.length_body_a;
jofo 4:3ab1e94b3bc4 99 p.Lb = c.length_body_b;
jofo 4:3ab1e94b3bc4 100 p.Lc = c.length_body_c;
jofo 4:3ab1e94b3bc4 101 p.tA = dofAngle[0];
jofo 4:3ab1e94b3bc4 102 p.tB = dofAngle[1];
jofo 4:3ab1e94b3bc4 103 p.tC = dofAngle[2] - dofAngle[1];
jofo 4:3ab1e94b3bc4 104
jofo 4:3ab1e94b3bc4 105 return p;
jofo 4:3ab1e94b3bc4 106 }
jofo 4:3ab1e94b3bc4 107
jofo 4:3ab1e94b3bc4 108 struct vec {
jofo 4:3ab1e94b3bc4 109 double x;
jofo 4:3ab1e94b3bc4 110 double y;
jofo 4:3ab1e94b3bc4 111 double z;
jofo 4:3ab1e94b3bc4 112
jofo 4:3ab1e94b3bc4 113 vec(double x, double y, double z):x(x),y(y),z(z) {}
jofo 4:3ab1e94b3bc4 114 };
jofo 4:3ab1e94b3bc4 115
jofo 4:3ab1e94b3bc4 116 vec getPosition(const configuration& m_config, int* encoder_values){
jofo 4:3ab1e94b3bc4 117 double x,y,z;
jofo 4:3ab1e94b3bc4 118
jofo 4:3ab1e94b3bc4 119 const pose p = calculate_pose(m_config, encoder_values);
jofo 4:3ab1e94b3bc4 120 const double& Ln = p.Ln;
jofo 4:3ab1e94b3bc4 121 const double& Lb = p.Lb;
jofo 4:3ab1e94b3bc4 122 const double& Lc = p.Lc;
jofo 4:3ab1e94b3bc4 123 const double& tA = p.tA;
jofo 4:3ab1e94b3bc4 124 const double& tB = p.tB;
jofo 4:3ab1e94b3bc4 125 const double& tC = p.tC;
jofo 4:3ab1e94b3bc4 126
jofo 4:3ab1e94b3bc4 127 // Do forward kinematics (thetas -> xyz)
jofo 4:3ab1e94b3bc4 128 x = cos(tA)*(Lb*sin(tB)+Lc*cos(tB+tC)) - m_config.workspace_origin_x;
jofo 4:3ab1e94b3bc4 129 y = sin(tA)*(Lb*sin(tB)+Lc*cos(tB+tC)) - m_config.workspace_origin_y;
jofo 4:3ab1e94b3bc4 130 z = Ln + Lb*cos(tB) - Lc*sin(tB+tC) - m_config.workspace_origin_z;
jofo 4:3ab1e94b3bc4 131
jofo 4:3ab1e94b3bc4 132 return vec(x,y,z);
jofo 4:3ab1e94b3bc4 133 }