Revision 1:24b7ab90081a, committed 2015-01-17

Comitter:

vsluiter

Date:

Sat Jan 17 21:42:46 2015 +0000

Parent:

0:f3cf9865b7be

Child:

2:bf29d24b69dd

Commit message:

Added state machine for impedances

Changed in this revision

--- 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);

--- a/main.h	Fri Jan 16 10:47:17 2015 +0000
+++ b/main.h	Sat Jan 17 21:42:46 2015 +0000
@@ -58,9 +58,9 @@
 int         ZControl_RefPos = 0;
 
 // IO Variables
-USBHID      hid(16,16);
-HID_REPORT  send_report;
-HID_REPORT  recv_report;
+//USBHID      hid(16,16);
+//HID_REPORT  send_report;
+//HID_REPORT  recv_report;
 DigitalIn   user_btn(p23);
 SPI         spi(NC, p6, p7);  
 Serial      pc(USBTX, USBRX);
@@ -86,6 +86,6 @@
 void calibrate_position();
 void initialize_io();
 void torque_control();
-void hid_control();
+//void hid_control();
 
 #endif
\ No newline at end of file

--- a/speedestimator.h	Fri Jan 16 10:47:17 2015 +0000
+++ b/speedestimator.h	Sat Jan 17 21:42:46 2015 +0000
@@ -4,29 +4,29 @@
 #include "main.h"
 
 struct speedEstimator {
-    
+
     int last_position;
     float position_int;
     int dead_ticks;
-    
+
     static const int fast_gain = SPEED_ESTIMATOR_FAST_GAIN;
     static const int deadband = SPEED_ESTIMATOR_DEADBAND;
-    
+
     /**
      * constructor
-     * 
+     *
      * Sets the default properties of the struct
      */
-    speedEstimator(int position) { 
-    
+    speedEstimator(int position) {
+
         // Set initial positions
         last_position = position;
         position_int = 1.0f*position;
         dead_ticks = 1;
     };
-    
+
     float get(int position) {
-        
+
         /**
          * Slow estimation
          */
@@ -37,42 +37,42 @@
         static float dp_dead = 0;
 
         dp = position - last_position;
-        
-        if(dp==0){
+
+        if(dp==0) {
             if(dead_ticks < 10000)
-                dead_ticks++; 
+                dead_ticks++;
             dp = dp_dead/dead_ticks;
         } else {
             if(dp > POSITION_ANTIALIAS)
                 dp-=POSITION_RESOLUTION;
             else if(dp < -POSITION_ANTIALIAS)
                 dp+=POSITION_RESOLUTION;
-            
+
             dp = dp/dead_ticks;
             dp_dead = dp;
             dead_ticks = 1;
         }
-            
+
         xz0 = SAMPLE_TIME_INVERSE_S*dp;
         yz1 = SPEED_ESTIMATOR_FILTER * xz1 + (1-SPEED_ESTIMATOR_FILTER) * yz1;
         xz1 = xz0;
-        
+
         last_position = position;
-        
+
         /**
          * Fast estimation
          */
         static float fast_sum;
         static float pre_int_sum;
-        
+
         fast_sum = position - (position_int + SAMPLE_TIME_US*0.5f*(float)1e-6*yz1);
         pre_int_sum = yz1+speedEstimator::fast_gain*(fast_sum > speedEstimator::deadband ? fast_sum - speedEstimator::deadband : (fast_sum < -speedEstimator::deadband ? fast_sum + speedEstimator::deadband : 0));
         position_int+=SAMPLE_TIME_US*1e-6*pre_int_sum;
-    
-        return pre_int_sum;    
-        
+
+        return pre_int_sum;
+
     };
-    
+
 };
 
 #endif
\ No newline at end of file