Revision 8:ed6b607462de, committed 2019-05-18

Comitter:

jofo

Date:

Sat May 18 11:49:14 2019 +0000

Parent:

7:bb6454b72c57

Commit message:

Cleaned and tested for local build 2019-05-18

Changed in this revision

--- a/USBDevice.lib	Fri Oct 28 12:34:49 2016 +0000
+++ b/USBDevice.lib	Sat May 18 11:49:14 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/USBDevice/#c0605f23f916
+http://mbed.org/users/mbed_official/code/USBDevice/#53949e6131f6

--- a/WoodenDevice.h	Fri Oct 28 12:34:49 2016 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,133 +0,0 @@
-//! A collection of variables that can be set in ~/wooden_haptics.json 
-struct configuration {
-    double diameter_capstan_a;      // m
-    double diameter_capstan_b;      // m
-    double diameter_capstan_c;      // m
-    double length_body_a;           // m
-    double length_body_b;           // m
-    double length_body_c;           // m
-    double diameter_body_a;         // m
-    double diameter_body_b;         // m
-    double diameter_body_c;         // m
-    double workspace_origin_x;      // m
-    double workspace_origin_y;      // m
-    double workspace_origin_z;      // m
-    double workspace_radius;        // m (for application information)
-    double torque_constant_motor_a; // Nm/A
-    double torque_constant_motor_b; // Nm/A
-    double torque_constant_motor_c; // Nm/A
-    double current_for_10_v_signal; // A
-    double cpr_encoder_a;           // quadrupled counts per revolution
-    double cpr_encoder_b;           // quadrupled counts per revolution
-    double cpr_encoder_c;           // quadrupled counts per revolution
-    double max_linear_force;        // N
-    double max_linear_stiffness;    // N/m
-    double max_linear_damping;      // N/(m/s)
-    double mass_body_b;             // Kg
-    double mass_body_c;             // Kg
-    double length_cm_body_b;        // m     distance to center of mass  
-    double length_cm_body_c;        // m     from previous body
-    double g_constant;              // m/s^2 usually 9.81 or 0 to 
-                                          //       disable gravity compensation
-
-    // Set values
-    configuration(const double* k):
-      diameter_capstan_a(k[0]), diameter_capstan_b(k[1]), diameter_capstan_c(k[2]),
-      length_body_a(k[3]), length_body_b(k[4]), length_body_c(k[5]),
-      diameter_body_a(k[6]), diameter_body_b(k[7]), diameter_body_c(k[8]), 
-      workspace_origin_x(k[9]), workspace_origin_y(k[10]), workspace_origin_z(k[11]), 
-      workspace_radius(k[12]), torque_constant_motor_a(k[13]), 
-      torque_constant_motor_b(k[14]), torque_constant_motor_c(k[15]), 
-      current_for_10_v_signal(k[16]), cpr_encoder_a(k[17]), cpr_encoder_b(k[18]), 
-      cpr_encoder_c(k[19]), max_linear_force(k[20]), max_linear_stiffness(k[21]), 
-      max_linear_damping(k[22]), mass_body_b(k[23]), mass_body_c(k[24]), 
-      length_cm_body_b(k[25]), length_cm_body_c(k[26]), g_constant(k[27]){}
-};
-
-
-configuration default_woody(){
-    double data[] = { 0.010, 0.010, 0.010, 
-                      0.080, 0.205, 0.200, 
-                      0.160, 0.120, 0.120,
-                      0.220, 0.000, 0.080, 0.100, 
-                      0.0603, 0.0603, 0.0603, 3.0, 4096, 2000, 2000, 
-                      12.0, 5000.0, 8.0,
-                      0.170, 0.110, 0.051, 0.091, 9.81};
-    return configuration(data); 
-}
-
-//==============================================================================
-// Helper functions for getPosition & setForce
-//==============================================================================
-double getMotorAngle(int motor, const double cpr, const int encoderValue) {
-    const double pi = 3.14159265359;
-    return 2.0*pi*encoderValue/cpr;
-}
-/*
-void setVolt(double v, int motor){
-    if(v > 10 || v< -10) { printf("Volt outside +/- 10 Volt\n"); return; }
-
-    // -10V to +10V is mapped from 0x0000 to 0xFFFF
-    unsigned int signal = (v+10.0)/20.0 * 0xFFFF;
-    S826_DacDataWrite(0,motor,signal,0);
-}
-*/
-
-struct pose {
-    double Ln;
-    double Lb;
-    double Lc;
-    double tA;  // angle of body A (theta_A)
-    double tB;  // angle of body B (theta_B)
-    double tC;  // angle of body C (theta_C)
-};
-
-pose calculate_pose(const configuration& c, int* encoderValues) {
-    pose p;
-
-    double cpr[] = { c.cpr_encoder_a, c.cpr_encoder_b, c.cpr_encoder_c };
-    double gearRatio[] = { -c.diameter_body_a / c.diameter_capstan_a,
-                   -c.diameter_body_b / c.diameter_capstan_b,
-                    c.diameter_body_c / c.diameter_capstan_c };
-
-    double dofAngle[3];
-    for(int i=0;i<3;i++)
-        dofAngle[i] = getMotorAngle(i,cpr[i],encoderValues[i]) / gearRatio[i];
-
-    // Calculate dof angles (theta) for each body
-    p.Ln = c.length_body_a; 
-    p.Lb = c.length_body_b; 
-    p.Lc = c.length_body_c; 
-    p.tA = dofAngle[0];
-    p.tB = dofAngle[1];
-    p.tC = dofAngle[2] - dofAngle[1];
-
-    return p;
-}
-
-struct vec {
-    double x;
-    double y;
-    double z;
-    
-    vec(double x, double y, double z):x(x),y(y),z(z) {}
-};
-
-vec getPosition(const configuration& m_config, int* encoder_values){
-    double x,y,z;
-
-    const pose p = calculate_pose(m_config, encoder_values);
-    const double& Ln = p.Ln;
-    const double& Lb = p.Lb; 
-    const double& Lc = p.Lc; 
-    const double& tA = p.tA; 
-    const double& tB = p.tB; 
-    const double& tC = p.tC; 
-
-    // Do forward kinematics (thetas -> xyz)
-    x = cos(tA)*(Lb*sin(tB)+Lc*cos(tB+tC))     - m_config.workspace_origin_x;
-    y = sin(tA)*(Lb*sin(tB)+Lc*cos(tB+tC))     - m_config.workspace_origin_y;
-    z = Ln + Lb*cos(tB) - Lc*sin(tB+tC)        - m_config.workspace_origin_z;
-    
-    return vec(x,y,z);
-}
\ No newline at end of file

--- a/main.cpp	Fri Oct 28 12:34:49 2016 +0000
+++ b/main.cpp	Sat May 18 11:49:14 2019 +0000
@@ -1,6 +1,25 @@
+/*
+ *  WoodenHaptics 1.5 USB HID interface
+ *  Works with the PCB version 0.4, LPC1768 and
+ *  Escons 50/5 motor controllers using
+ *  PWM and a direction pin.
+ *
+ *  Developed by Jordi Solsona and Jonas Forsslund
+ *  (C) Forsslund Systems AB 2015-2019
+ *  www.woodenhaptics.org
+ *
+ *  Relased as open source under GNU GPL v.3 or later
+ *  Note: any distribution of binaries (or hardware)
+ *  Requires this source file, including any modifications,
+ *  as well as the attribution mentioned above.
+ *
+ *  Updated and tested 2019-05-18 by Jonas Forsslund.
+ *  jonas@forsslundsystems.com
+ *
+ */
+
 #include "mbed.h"
 #include "USBHID.h"
-#include "WoodenDevice.h"
 #include "FastPWM.h"
 
 
@@ -27,12 +46,6 @@
 
 Timeout msg_watchdog;
 
-
-
-
-Serial pc(USBTX, USBRX); // tx, rx
-
-
 int no_enc = 3;
 
 int prev_state[3] = {-1,-1,-1};
@@ -53,9 +66,8 @@
 // 1 0  0 0  +     8
 // 1 0  1 1  -    11
 //
-//                0 1  2 3  4 5 6 7 8 9 10 11 12 13 14 15
-//int stable[16] = {0,1,-1,0,-1,0,0,1,1,0, 0,-1, 0,-1, 1, 0}; //OLD
-int stable[16] =   {0,-1,1,0,1,0,0,-1,-1,0, 0,1, 0,1, -1, 0};
+//                0  1 2 3 4 5 6  7  8 9 10 11 12 13 14 15
+int stable[16] = {0,-1,1,0,1,0,0,-1,-1,0, 0, 1, 0, 1,-1, 0};
  
 void encoder_callback(int _encoder,int AB,bool value){
         int cur_state;
@@ -85,27 +97,6 @@
 void callback_2_B_rise(void) { encoder_callback(2,1,true);}
 void callback_2_B_fall(void) { encoder_callback(2,1,false);}
  
-// Our 12*4=48 byte message (used both up and down)
-struct woodenhaptics_message {
-    float position_x;
-    float position_y;
-    float position_z;
-    float command_force_x;
-    float command_force_y;
-    float command_force_z;
-    float actual_current_0;
-    float actual_current_1;
-    float actual_current_2;
-    float temperature_0;
-    float temperature_1;
-    float temperature_2;
-
-    woodenhaptics_message():position_x(0),position_y(0),position_z(0),
-                            command_force_x(0),command_force_y(0),command_force_z(0),
-                            actual_current_0(0),actual_current_1(0),actual_current_2(0),
-                            temperature_0(0),temperature_1(0),temperature_2(0){}
-};
-
 struct hid_to_pc_message { // 4*2 = 8 bytes
     short encoder_a;
     short encoder_b;
@@ -117,13 +108,12 @@
     short current_motor_a_mA;
     short current_motor_b_mA;
     short current_motor_c_mA;
-    unsigned int debug;
+    unsigned short debug;
 };
 
 void escon_timeout() {
     enableEscons = 0;
-    myled2 = 0;
-    
+    myled2 = 0;   
     myled3 = 0;
     myled4 = 0;
         
@@ -163,28 +153,19 @@
     
     enableEscons = 0;
     
-    configuration config = default_woody();  
-    
     for(int i=0;i<3;i++){
         pwm[i].period_us(500);
         pwm[i].write(0);  
         direction[i].period_us(500);
         direction[i].write(0);
-        }      
-    
-    
-    pc.baud(115200);
-    
+    }
+           
     send_report.length = 8; 
-    //woodenhaptics_message msg;
     
     hid_to_pc_message hid_to_pc;
     hid_to_pc.debug = 0;
     pc_to_hid_message pc_to_hid;
-    
-//    Timer t;
-//    t.start();
-    
+       
     Timer usb_timer;
     usb_timer.start();
 
@@ -193,58 +174,43 @@
     
     Timer message_timeout;
     message_timeout.start();
-
-
-    pc.printf("Hello World Debug!\n\r");
-    
     
     while (1) {
         myled1 = 1;
-        
-
 
         //try to read a msg
         if(hid.readNB(&recv_report)) {
+
             // TODO: Make sure we read the latest message, not the oldest.
-            myled3 = !myled3; // We got data
-            
+            myled3 = !myled3; // We got data            
 
             if(recv_report.length == 8){ // It should always be!                
-            
-            
-                //message_timeout.reset();
-                //enableEscons = 1;
-                //myled2 = 1;
+
                 msg_watchdog.detach();
                 msg_watchdog.attach(&escon_timeout,0.5);
                 enableEscons = 1;
                 myled2 = 1;
 
-            
-                pc_to_hid = *reinterpret_cast<pc_to_hid_message*>(recv_report.data);
+                // Reinterprete the recevied byte array as a pc_to_hid object.
+                // Since recv_report.data is defined as unsigned char we
+                // have to cast to char* to avoid strict aliasing warning.
+                char* dataptr = reinterpret_cast<char*>(recv_report.data);
+                pc_to_hid = *reinterpret_cast<pc_to_hid_message*>(dataptr);
                 
-                //if(debug_t.read() > 0.5){
-                //    pc.printf("Force: %f\n\r",pc_to_hid.current_motor_a_mA);
-                //    debug_t.reset();
-                //}
-                
-                //float f[3] = {msg.command_force_x, msg.command_force_y, msg.command_force_z};
-                
-                //float f[3] = {msg.command_force_x, msg.command_force_y, msg.command_force_z};
-                float f[3] = {pc_to_hid.current_motor_a_mA*0.001, pc_to_hid.current_motor_b_mA*0.001,pc_to_hid.current_motor_c_mA*0.001};
+                float f[3] = {pc_to_hid.current_motor_a_mA*0.001f,
+                              pc_to_hid.current_motor_b_mA*0.001f,
+                              pc_to_hid.current_motor_c_mA*0.001f};
+
                 for(int i=0;i<3;i++){
                     int dir = f[i] > 0 ? 1 : 0;
                     direction[i].write(dir);
-//                    if(i==1)
-//                        pc.printf("Direction: %d (direction %d) \n\r", dir, direction[1].read());
-                    float abs_val = std::abs(f[i]);
+                    float abs_val = f[i]<0? -f[i] : f[i];
                     if(abs_val > 3.0)
                         pwm[i].write(0.9);
                     else
                         pwm[i].write(0.8*abs_val/3.0+0.1);
                 }
-            }
-                
+            }                
             myled1 = 1;
         }
 
@@ -253,14 +219,13 @@
         hid_to_pc.encoder_c = counter[2];
         
         if(usb_timer.read() > 0.00001) {
-            pc.printf("%f\n",usb_timer.read());
             usb_timer.reset();
 
             hid_to_pc.debug++;
             unsigned char* out_buf = reinterpret_cast<unsigned char*>(&hid_to_pc);
     
             //Fill the report
-            for (int i = 0; i < send_report.length; i++) {
+            for (unsigned int i = 0; i < send_report.length; i++) {
                 send_report.data[i] = out_buf[i];
             }
                 
@@ -269,4 +234,4 @@
             myled4 = !myled4;
         }
     }
-}
\ No newline at end of file
+}

--- a/mbed.bld	Fri Oct 28 12:34:49 2016 +0000
+++ b/mbed.bld	Sat May 18 11:49:14 2019 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed/builds/487b796308b0
\ No newline at end of file
+https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400
\ No newline at end of file