Diff: main.cpp

Revision:

3:10863117020c

Parent:

2:bf29d24b69dd

Child:

4:9d37f163d64c

--- a/main.cpp	Mon Jan 11 12:03:23 2016 +0000
+++ b/main.cpp	Mon Jun 19 07:40:22 2017 +0000
@@ -9,11 +9,25 @@
 #include "position_sensor_error.h"
 #include "cogging_compensation.h"
 #include "main.h"
+Ticker tickObject;
+
+#include <iostream>
+
+using namespace std;
+const int n = 1000;
+int k = 0;
+int z = 0;
+int prbstest[n];
+int status;
+int statusnew;
+const int p = 2*n;
+int positievec[p];
+int q = 0;
 
 /** Main function
  * Bootstraps the system
  */
-typedef enum z_state{Z_ZERO,Z_B,Z_I,Z_K,Z_OFF}z_states;
+typedef enum z_state{/*Z_ZERO,Z_B,Z_K,Z_OFF*/Z_I,Z_NUL}z_states;
 
 void SetImpedance(float i, float b, float k, float pos)
 {
@@ -25,10 +39,10 @@
 
 void blink(void)
 {
-    static z_states localstate=Z_ZERO;
+    static z_states localstate=Z_I;
     switch(localstate)
     {
-        case Z_ZERO:
+        /*case Z_ZERO:
         {
             localstate = Z_B; //mass
             SetImpedance(0,0.03,0,position);
@@ -39,14 +53,20 @@
             localstate = Z_I; //fluid
             SetImpedance(0.0009,0.01,0.001,position);
             break;
-        }
+        }*/
         case Z_I:
         {
-            localstate = Z_K;//spring
-            SetImpedance(0,0,0.05,position);
+            localstate = Z_NUL;//spring
+            SetImpedance(0.00033,0.00033,0.0000034,0);
             break;
         }
-        case Z_K:
+        case Z_NUL:
+        {
+            localstate = Z_I;//spring
+            SetImpedance(0.00033,0.00033,0.0000034,500);
+            break;
+        }
+        /*case Z_K:
         {
             localstate = Z_OFF;
             SetImpedance(0,0,0,position);
@@ -56,15 +76,15 @@
         }
         case Z_OFF:
         {
-            localstate = Z_ZERO;
+            localstate = Z_I;
             SetImpedance(0,0.00,0,position);
             driver_enable_a = 1;
             driver_enable_b = 1;
             break;
-        }
+        }*/
         default:
         {
-            localstate = Z_ZERO;
+            localstate = Z_I;
             ZControl_I = 0;
             ZControl_B = 0;
             ZControl_K = 0;
@@ -74,24 +94,71 @@
     info_led_3 != info_led_3;
     wait_ms(300); //debounce
 }
-
+    
+void printer(){
+    int positie = GET_POSITION();
+    positievec[q] = positie;
+    cout << positievec[q] << ",";
+    q = q+1;
+    }
+     
 int main()
 {
     // Initialize system
+    //initialiseer_prbs();
     initialize_io();
     calibrate_current_sensor();
     calibrate_position();
 
-    torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
+    for (int i = 0; i <= n; i++) {
+        prbstest[i] = rand() % 2;
+        cout << prbstest[i] << ",";//"\r\n";
+        }
+        
+    while(z<=n){    
+        //torque_controller.attach_us(&torque_control, TORQUE_CONTROLLER_INTERVAL_US);
+        //tickObject.attach_us(&printer, 100000);
+        torque_control();
+        printer();
+        cout << z << ";\r\n";
+        z = z+1;
+        wait_us(TORQUE_CONTROLLER_INTERVAL_US);
+
+    }
 
     //send_report.length = 16;
     //recv_report.length = 16;
 
+        cout << "\r\n";
+        /*
+        for(int k=0; k <= n; k++) {
+         status = prbstest[k];
+         int kplus = k+1;
+         statusnew = prbstest[kplus];
+         if (status == statusnew){
+            //cout << "equal:\r\n";
+            //cout << status << " " << k << "\r\n";
+            //cout << statusnew << " " << kplus << "\r\n";
+            }
+         else if (status != statusnew){
+            //cout << "unequal:\r\n";
+            //cout << status << " " << k << "\r\n";
+            //cout << statusnew << " " << kplus << "\r\n";
+            blink();
+            }
+        }*/
+    cout << positievec;
+    
     while(1) {
         int32_t abspos = ABSPOS();
-
-        if(!user_btn)
+        
+        /*if(!user_btn)
             blink();
+        tickObject.attach(&blink, 5);*/
+        
+        //blink();
+        //wait(1);  
+        
         //send_report.data[3] = abspos & 0x000000ff;
         //send_report.data[2] = (abspos & 0x0000ff00) >> 8;
         //send_report.data[1] = (abspos & 0x00ff0000) >> 16;
@@ -116,10 +183,19 @@
         info_led_3 = !info_led_3;
         wait(0.01);
     }
-
     return 0;
 }
 
+/*//create a prbs signal
+void initialiseer_prbs() {
+    int prbstest[n] = {};
+
+    for (int i = 0; i <= n; i++) {
+        prbstest[i] = rand() % 2;
+        cout << prbstest[i];
+    }
+}*/
+
 /** Sample the current sensor to determine the offset
  */
 void calibrate_current_sensor()
@@ -165,7 +241,7 @@
 {
     user_btn.mode(PullUp);
     //user_btn.rise(blink);
-    pc.baud(115200);
+    pc.baud(9600);
     spi.format(14,3);
     driver_1a.period_us(33);
     driver_2a.period_us(33);
@@ -218,7 +294,14 @@
     position_cos    = arm_cos_f32(position_theta);
 
     // Impedance controller...
-    torque = -ZControl_K*0.00076699f*(ABSPOS()-ZControl_RefPos) - ZControl_B*speed - ZControl_I*acceleration;
+    if(prbstest[z] == 1){
+        torque = -0.1;//-ZControl_K*0.00076699f*(ABSPOS()-ZControl_RefPos) - ZControl_B*speed - ZControl_I*acceleration;
+        cout << torque << ",";//"\r\n";
+        }
+    else if(prbstest[z] == 0){
+        torque = 0.1;
+        cout << torque << ",";//"\r\n";
+        }
 
     // Preprocess torque command
     torque_setpoint = (torque > TORQUE_LIMIT) ? TORQUE_LIMIT : (torque < -TORQUE_LIMIT ? -TORQUE_LIMIT : torque);