Diff: main.cpp

Revision:

8:ed6b607462de

Parent:

7:bb6454b72c57

--- 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
+}