Diff: main.cpp

Revision:

5:96cd67bcac8c

Parent:

4:6e290eb553cd

Child:

6:1143996ac690

--- a/main.cpp	Wed Jan 10 04:44:10 2018 +0000
+++ b/main.cpp	Sun Jan 14 23:18:09 2018 +0000
@@ -56,7 +56,7 @@
 float U;
 
 
-int16_t log_vec[2500] = {0};
+//int8_t log_vec[1250] = {0};
 //int16_t log_vec_2[1250] = {0};
 
 
@@ -66,6 +66,8 @@
       }
       count++;
     
+    //d_out = !d_out;
+    
     if(count>1000 && count<2000){
         q_ref = 1.57f;
         //ref = 18000.0f;
@@ -83,48 +85,52 @@
     if(count>4500){
         controlmode = 1;
     }
+    /*
     if(count<5000){
             //log_vec_2[count/4] = (int)(q_est[1]*10.0f);
-            log_vec[count/2] = (int)(q*1000.0f);
+            log_vec[count/4] = q_raw>>4;
             }
     
-    if(count>20000 && count<22500){
-        //printf("%d\n\r", log_vec[count2]);
-        //wait_us(200);
+    if(count>20000 && count<21250){
+        printf("%d\n\r", log_vec[count2]);
+        wait_us(80);
         //printf("%d\n\r", log_vec_2[count2]);
         //wait_us(200);
         count2++;
         }
         
-        
+        */
       Control();
+      
       /*
       if(count > 5000){
           //io.printf("derp\n\r");
           //pc.printf("derp\n\r");
-          //pc.printf("%d   %f\n\r", q_raw, e);
-          printf("%f   %f\n\r", dq, dqdebug);
+          pc.printf("%d  \n\r", q_raw);
+          //printf("%f   %f\n\r", dq, dqdebug);
           //d_out = !d_out;
           count = 0;
           }
           */
+          
   TIM1->SR = 0x0; // reset the status register
 }
 
 
 /* Main Loop */
 int main() {
-    
+
     pc.baud(921600);
     io.baud(921600);
 
-
+    SystemCoreClockUpdate();
+    //printf("%d\n\r", SystemCoreClock);
     //pc.printf("\n\r Rubix Controller\n\r");
     id_1.mode(PullUp);
     id_2.mode(PullUp);
     id_3.mode(PullUp);
     id = GetID();
-    pc.printf(" Motor ID:  %d\n\r", id);
+    //pc.printf(" Motor ID:  %d\n\r", id);
     
     //d_in.mode(PullDown);
     led = 1;
@@ -159,7 +165,7 @@
     
     // Kalman Filter //
     // Update Model //
-    
+    /*
     q_est[0] += q_est[1]*F[0][1];
     q_est[1] += B[1]*U;
     
@@ -191,7 +197,7 @@
     P[0][1] = -K[0][1]*P[1][1] - P[0][1]*(K[0][0] - 1.0f);
     P[1][0] = -K[1][0]*P[0][0] - P[1][0]*(K[1][1] - 1.0f);
     P[1][1] = -K[1][0]*P[0][1] - P[1][1]*(K[1][1] - 1.0f);
-    
+    */
    
     
     // Control Law //