Diff: main.cpp

Revision:

1:24b7ab90081a

Parent:

0:f3cf9865b7be

Child:

2:bf29d24b69dd

--- a/main.cpp	Fri Jan 16 10:47:17 2015 +0000
+++ b/main.cpp	Sat Jan 17 21:42:46 2015 +0000
@@ -1,6 +1,6 @@
 #include "mbed.h"
 #include "arm_math.h"
-#include "USBHID.h"
+//#include "USBHID.h"
 #include <math.h>
 #include <string>
 #include <stdlib.h>
@@ -13,6 +13,68 @@
 /** Main function
  * Bootstraps the system
  */
+typedef enum z_state{Z_ZERO,Z_B,Z_I,Z_K,Z_OFF}z_states;
+
+void SetImpedance(float i, float b, float k, float pos)
+{
+            ZControl_I = i;
+            ZControl_B = b;
+            ZControl_K = k;
+            ZControl_RefPos = pos;   
+}
+
+void blink(void)
+{
+    static z_states localstate=Z_ZERO;
+    switch(localstate)
+    {
+        case Z_ZERO:
+        {
+            localstate = Z_B;
+            SetImpedance(0,0.01,0,position);
+            break;
+        }
+        case Z_B:
+        {
+            localstate = Z_I;
+            SetImpedance(0.01,0,0,position);
+            break;
+        }
+        case Z_I:
+        {
+            localstate = Z_K;
+            SetImpedance(0,0,0.01,position);
+            break;
+        }
+        case Z_K:
+        {
+            localstate = Z_OFF;
+            SetImpedance(0,0,0,position);
+            driver_enable_a = 0;
+            driver_enable_b = 0;
+            break;
+        }
+        case Z_OFF:
+        {
+            localstate = Z_ZERO;
+            SetImpedance(0,0.00,0,position);
+            driver_enable_a = 1;
+            driver_enable_b = 1;
+            break;
+        }
+        default:
+        {
+            localstate = Z_ZERO;
+            ZControl_I = 0;
+            ZControl_B = 0;
+            ZControl_K = 0;
+            ZControl_RefPos = position;
+        }
+    }
+    info_led_3 != info_led_3;
+    wait_ms(300); //debounce
+}
+
 int main()
 {
     // Initialize system
@@ -21,35 +83,36 @@
     calibrate_position();
 
     torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
-    
-    send_report.length = 16;
-    recv_report.length = 16;
-    
-    while(1){
-                
+
+    //send_report.length = 16;
+    //recv_report.length = 16;
+
+    while(1) {
         int32_t abspos = ABSPOS();
-        
-        send_report.data[3] = abspos & 0x000000ff;
-        send_report.data[2] = (abspos & 0x0000ff00) >> 8;
-        send_report.data[1] = (abspos & 0x00ff0000) >> 16;
-        send_report.data[0] = (abspos & 0xff000000) >> 24;
-        
-        for(int i = 4; i < 16; i++){
-            send_report.data[i] = 0x0;
-        }
-        
+
+        if(!user_btn)
+            blink();
+        //send_report.data[3] = abspos & 0x000000ff;
+        //send_report.data[2] = (abspos & 0x0000ff00) >> 8;
+        //send_report.data[1] = (abspos & 0x00ff0000) >> 16;
+        //send_report.data[0] = (abspos & 0xff000000) >> 24;
+
+        //for(int i = 4; i < 16; i++){
+        //    send_report.data[i] = 0x0;
+        //}
+
         //Send the report
-        hid.send(&send_report);    
-        
+        //hid.send(&send_report);
+
         // Try to read
-        if(hid.readNB(&recv_report)) {
-                
-            ZControl_I          = (float)1e-6*((recv_report.data[3] << 24) | (recv_report.data[2] << 16) | (recv_report.data[1] << 8) | (recv_report.data[0]));
-            ZControl_B          = (float)1e-6*((recv_report.data[7] << 24) | (recv_report.data[6] << 16) | (recv_report.data[5] << 8) | (recv_report.data[4]));
-            ZControl_K          = (float)1e-6*((recv_report.data[11] << 24) | (recv_report.data[10] << 16) | (recv_report.data[9] << 8) | (recv_report.data[8]));
-            ZControl_RefPos     = (recv_report.data[15] << 24) | (recv_report.data[14] << 16) | (recv_report.data[13] << 8) | (recv_report.data[12]);
-        }
-        
+        //if(hid.readNB(&recv_report)) {
+
+        //    ZControl_I          = (float)1e-6*((recv_report.data[3] << 24) | (recv_report.data[2] << 16) | (recv_report.data[1] << 8) | (recv_report.data[0]));
+        //    ZControl_B          = (float)1e-6*((recv_report.data[7] << 24) | (recv_report.data[6] << 16) | (recv_report.data[5] << 8) | (recv_report.data[4]));
+        //    ZControl_K          = (float)1e-6*((recv_report.data[11] << 24) | (recv_report.data[10] << 16) | (recv_report.data[9] << 8) | (recv_report.data[8]));
+        //    ZControl_RefPos     = (recv_report.data[15] << 24) | (recv_report.data[14] << 16) | (recv_report.data[13] << 8) | (recv_report.data[12]);
+        //}
+
         info_led_3 = !info_led_3;
         wait(0.01);
     }
@@ -101,6 +164,7 @@
 void initialize_io()
 {
     user_btn.mode(PullUp);
+    //user_btn.rise(blink);
     pc.baud(115200);
     spi.format(14,3);
     driver_1a.period_us(33);