Diff: main.cpp

Revision:

124:e70ca81676fc

Parent:

123:d81d91c9abe8

Child:

126:498f56ba051e

--- a/main.cpp	Fri Apr 28 01:11:04 2017 +0000
+++ b/main.cpp	Sun Apr 30 03:46:34 2017 +0000
@@ -27,12 +27,15 @@
 FOCStruct foc;
 ControlStruct control;
 
+DigitalOut test(PC_5);
+int time_counter = 0;
+float test_dtc = 0.2f;
+
 DQMapper *dq;
 ThrottleMapper *th;
 
 int loop_counter = 0;
 bool control_enabled = false;
-DigitalOut test(PC_5);
 
 void update_velocity() {
     read.last_p_mech = read.p_mech;
@@ -64,7 +67,7 @@
     loop_counter++;
     
     /*update position, sin, cos*/
-    foc.p = io.pos->GetElecPosition() - POS_OFFSET + read.w / 1048.0f;
+    foc.p = io.pos->GetElecPosition() - POS_OFFSET;// + read.w / 1048.0f;
     float sin_p = sinf(foc.p);
     float cos_p = cosf(foc.p);
     
@@ -98,8 +101,12 @@
         foc.vq = 0.0f;
     }
     
+    float pv = foc.p + read.w * 3.0f * VOLTAGE_CMD_DELAY * 1e-6;
+    float sin_pv = sinf(pv);
+    float cos_pv = cosf(pv);
+    
     /*inverse transforms*/
-    invpark(foc.vd, foc.vq, sin_p, cos_p, &foc.valpha, &foc.vbeta);
+    invpark(foc.vd, foc.vq, sin_pv, cos_pv, &foc.valpha, &foc.vbeta);
     
     float va, vb, vc, voff;
     
@@ -155,7 +162,6 @@
     io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), 
                         (int) (255 * foc.vd), (int) (255 * foc.vq));
     //io.pc->printf("%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_filtered, (int) control.q_filtered, (int) (255 * foc.vd), (int) (255 * foc.vq));
-    //io.pc->printf("%d\n", io.pos->IsValid());
     wait(1.0f / LOG_FREQUENCY);
 }
         
@@ -163,14 +169,12 @@
     int start_state = io.throttle_in->state();
     
     if (TIM1->SR & TIM_SR_UIF ) {
-        test = 1;
         ADC1->CR2  |= 0x40000000; 
         volatile int delay;
         for (delay = 0; delay < 35; delay++);
         
         read.adval1 = ADC1->DR;
         read.adval2 = ADC2->DR;
-        test = 0;
 
         commutate();
     }
@@ -180,7 +184,7 @@
 }
 
 int main() {
-    dq = new LutMapper();//AngleMapper(1.7805f, 48.05f);
+    dq = new LutMapper();//DirectMapper(-130.0f, 43.0f);
     th = new NullThrottleMapper();
     BREMSInit(&io, &read, &foc, &control, false);