Diff: main.cpp

Revision:

4:3ab1e94b3bc4

Parent:

2:1db77338562f

Child:

5:2908292a8cf3

--- a/main.cpp	Fri Mar 01 13:23:28 2013 +0000
+++ b/main.cpp	Fri Mar 27 15:53:41 2015 +0000
@@ -1,36 +1,225 @@
 #include "mbed.h"
 #include "USBHID.h"
- 
+#include "WoodenDevice.h"
+
 //We declare a USBHID device. Input out output reports have a length of 8 bytes
-USBHID hid(8, 8);
+USBHID hid(48, 48);
  
 //This report will contain data to be sent
 HID_REPORT send_report;
 HID_REPORT recv_report;
+
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
+DigitalOut enableEscons(p14,1); 
  
-Serial pc(USBTX, USBRX);
+InterruptIn  encoder0_A(p5);
+InterruptIn  encoder0_B(p7);
+InterruptIn  encoder1_A(p8);
+InterruptIn  encoder1_B(p10);
+InterruptIn  encoder2_A(p11);
+InterruptIn  encoder2_B(p13);
+
+
+Serial pc(USBTX, USBRX); // tx, rx
+
+
+int no_enc = 3;
+
+int prev_state[3] = {-1,-1,-1};
+bool encoder_raw[3][2] = {{false,false},{false,false},{false,false}};
+
+PwmOut pwm[3]={p21,p22,p23};
+DigitalOut direction[3]={p24,p25,p26};
+
+int counter[3] = {0,0,0};
+
+// Pre  Cur  Dir  Dec   
+// 0 0  0 1  +     1
+// 0 0  1 0  -     2
+// 0 1  1 1  +     7
+// 0 1  0 0  -     4
+// 1 1  1 0  +    14
+// 1 1  0 1  -    13
+// 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};
+ 
+void encoder_callback(int _encoder,int AB,bool value){
+        int cur_state;
+        
+        encoder_raw[_encoder][AB]=value;
+        
+        cur_state = encoder_raw[_encoder][0] << 1 | encoder_raw[_encoder][1];
+
+        if(prev_state[_encoder] < 0) prev_state[_encoder] = cur_state;        
+        counter[_encoder] += stable[prev_state[_encoder] << 2 | cur_state];
+        prev_state[_encoder]=cur_state;
+}
+
+// "callback stubs"
+void callback_0_A_rise(void) { encoder_callback(0,0,true);}
+void callback_0_A_fall(void) { encoder_callback(0,0,false);}
+void callback_0_B_rise(void) { encoder_callback(0,1,true);}
+void callback_0_B_fall(void) { encoder_callback(0,1,false);}
+
+void callback_1_A_rise(void) { encoder_callback(1,0,true);}
+void callback_1_A_fall(void) { encoder_callback(1,0,false);}
+void callback_1_B_rise(void) { encoder_callback(1,1,true);}
+void callback_1_B_fall(void) { encoder_callback(1,1,false);}
+
+void callback_2_A_rise(void) { encoder_callback(2,0,true);}
+void callback_2_A_fall(void) { encoder_callback(2,0,false);}
+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){}
+};
+ 
  
 int main(void) {
-    send_report.length = 8;
- 
+    myled1 = 1;   // SETUP
+    myled2 = 0;
+    myled3 = 0;
+    myled4 = 0;    
+    
+    encoder0_A.rise(&callback_0_A_rise);
+    encoder0_A.fall(&callback_0_A_fall);
+    encoder0_B.rise(&callback_0_B_rise);
+    encoder0_B.fall(&callback_0_B_fall);
+    
+    encoder1_A.rise(&callback_1_A_rise);
+    encoder1_A.fall(&callback_1_A_fall);
+    encoder1_B.rise(&callback_1_B_rise);
+    encoder1_B.fall(&callback_1_B_fall);
+
+    encoder2_A.rise(&callback_2_A_rise);
+    encoder2_A.fall(&callback_2_A_fall);
+    encoder2_B.rise(&callback_2_B_rise);
+    encoder2_B.fall(&callback_2_B_fall);
+    
+    enableEscons = 1;
+    
+    configuration config = default_woody();  
+    
+    for(int i=0;i<3;i++){
+        pwm[i].period_us(1000);
+        pwm[i].write(0);  
+        direction[i]=0;
+        }      
+    
+    
+    pc.baud(115200);
+    
+    send_report.length = 48; 
+    woodenhaptics_message msg;
+    
+    Timer t;
+    t.start();
+    
+    Timer usb_timer;
+    usb_timer.start();
+
+    Timer debug_t;
+    debug_t.start();
+
+
+    pc.printf("Hello World Debug!\n\r");
+    
     while (1) {
-        //Fill the report
-        for (int i = 0; i < send_report.length; i++) {
-            send_report.data[i] = rand() & 0xff;
-        }
-            
-        //Send the report
-        hid.send(&send_report);
+        myled1 = 1;
+        myled2 = 1;
         
+
+
         //try to read a msg
         if(hid.readNB(&recv_report)) {
-            pc.printf("recv: ");
-            for(int i = 0; i < recv_report.length; i++) {
-                pc.printf("%d ", recv_report.data[i]);
+            // TODO: Make sure we read the latest message, not the oldest.
+            myled3 = !myled3; // We got data
+            
+
+            if(recv_report.length == 4*12){ // It should always be!                
+            
+                msg = *reinterpret_cast<woodenhaptics_message*>(recv_report.data);
+                
+                if(debug_t.read() > 0.5){
+                    pc.printf("Force: %f\n\r",msg.command_force_x);
+                    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};
+                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]);
+                    if(f[i] > 3.3)
+                        pwm[i].write(0.9);
+                    else
+                        pwm[i].write(0.8*f[i]/3.3+0.1);
+                }
+
+                
+                /*
+                pwm[0].write(0.5);
+                pwm[1].write(0.5);
+                pwm[2].write(0.5);
+                */
             }
-            pc.printf("\r\n");
+                
+            myled1 = 1;
         }
         
-        wait(0.1);
+        
+            
+        // "move the haptic device to the right"        
+        vec p = getPosition(config, counter);
+        msg.position_x = p.x;
+        msg.position_y = p.y;//0.05*sin(t.read());
+        msg.position_z = p.z;
+        
+        if(usb_timer.read() > 0.01) {
+            usb_timer.reset();
+
+            unsigned char* out_buf = reinterpret_cast<unsigned char*>(&msg);
+    
+            //Fill the report
+            for (int i = 0; i < send_report.length; i++) {
+                send_report.data[i] = out_buf[i];
+            }
+                
+            //Send the report
+            hid.send(&send_report);      
+            myled4 = !myled4;
+        }      
+               
+        //wait_us(100); // 0.1ms
     }
 }
\ No newline at end of file