Diff: main.cpp

Revision:

11:16d807d6b9c5

Parent:

10:4b7f2653fb45

--- a/main.cpp	Sun Feb 04 04:37:05 2018 +0000
+++ b/main.cpp	Wed Mar 07 19:25:49 2018 +0000
@@ -9,12 +9,12 @@
 #define J               0.000065f            // Inertia
 #define KT              0.0678f              // Torque Constant
 #define R               0.85f               // Resistance
-#define V_IN            30.0f               // DC input voltage
+#define V_IN            60.0f               // DC input voltage
 #define K_SAT           22000.0f               // Controller saturation gain
 #define DTC_MAX         0.97f               // Max duty cycle (limited by bootstrapping)
 #define V               V_IN*DTC_MAX        // Max useable voltage
 
-#define TICKSTORAD(x)           (float)x*2.0f*PI/CPR  
+#define TICKSTORAD(x)           (float)x*2.0f*PI/CPR   
 #define CONSTRAIN(x,min,max)    ((x)<(min)?(min):((x)>(max)?(max):(x)))
 
 Serial pc (PA_2, PA_3);                     // Serial to programming header
@@ -23,7 +23,7 @@
 DigitalIn id_2(PB_4);
 DigitalIn id_1(PB_5);
 DigitalOut led(PA_15);                      // Debug LED
-DigitalIn d_in(PA_4);                       // LED on input from AND Board
+DigitalIn d_in(PA_4);                       // Input from AND Board
 
 //AnalogOut a_out(PA_5);
 DigitalOut d_out(PA_5);                     // LED on output to AND Board
@@ -47,6 +47,9 @@
 int count, count2;
 int controlmode = 0;
 
+float i_est;
+float i_int;
+
 
 volatile int run_control = 0;
 volatile int position_setpoint = 0;
@@ -143,7 +146,7 @@
     {
         ;
     }
-    wait(0.001f);
+    wait(0.0001f);
     printf("done.\r\n");
 }
 
@@ -156,6 +159,7 @@
 int8_t get_and_board()
 {
     uint8_t value = d_in;
+    led = value;
     //printf("[BOARD %d] Check and board: %d\r\n",get_board_id(),value);
     return value;
 }
@@ -302,6 +306,8 @@
     u = CONSTRAIN(e, -V, V);
     WriteVoltage(u);
     U = KT*(u - KT*dq)/R;
+    i_est = U;
+    i_int += U
     //WriteVoltage(-10.0f);
     }