Diff: isr.cpp

Revision:

2:b5c19d4eddcc

Parent:

1:70eed554399b

Child:

3:86ccde39f61b

--- a/isr.cpp	Sat Feb 21 21:39:33 2015 +0000
+++ b/isr.cpp	Mon Feb 23 19:42:50 2015 +0000
@@ -2,8 +2,10 @@
 #include "mbed.h"
 #include "shared.h"
 #include "constants.h"
+#include "util.h"
 
 void dtc_update() { 
+#ifndef __SVM
     float ia = motor->angle - motor->sensor_phase;
     if (ia < 0) ia += 360.0f;
     if (ia >= 360.0f) ia -= 360.0f;    
@@ -15,15 +17,60 @@
     motor->dtc_a = sinetab[(int) (ia)] * motor->throttle;
     motor->dtc_b = sinetab[(int) (ib)] * motor->throttle;
     motor->dtc_c = sinetab[(int) (ic)] * motor->throttle;
+#else
+    float ix = 120.0f - motor->angle;
+    if (ix < 0) ix += 360.0f;
+    if (ix >= 360.0f) ix -= 360.0f;
+    float dx = sinetab[(int) ix];
+    float iy = motor->angle;
+    if (iy < 0) iy += 360.0f;
+    if (iy >= 360.0f) iy -= 360.0f;
+    float dy = sinetab[(int) iy];
+    int sector = (int) (iy / 60.0f) + 1;
     
+    switch(sector) {
+    case 1:
+        motor->dtc_a = 1.0f - dx - dy;
+        motor->dtc_b = 1.0f + dx - dy;
+        motor->dtc_c = 1.0f + dx + dy;
+        break;
+    case 2:
+        motor->dtc_a = 1.0f - dx + dy;
+        motor->dtc_b = 1.0f - dx - dy;
+        motor->dtc_c = 1.0f + dx + dy;
+        break;
+    case 3:
+        motor->dtc_a = 1.0f + dx + dy;
+        motor->dtc_b = 1.0f - dx - dy;
+        motor->dtc_c = 1.0f + dx - dy;
+        break;
+    case 4:
+        motor->dtc_a = 1.0f + dx + dy;
+        motor->dtc_b = 1.0f - dx + dy;
+        motor->dtc_c = 1.0f - dx + dy; //suspect
+        break;
+    case 5:
+        motor->dtc_a = 1.0f + dx - dy;
+        motor->dtc_b = 1.0f + dx + dy;
+        motor->dtc_c = 1.0f - dx - dy;
+        break;
+    case 6:
+        motor->dtc_a = 1.0f - dx - dy;
+        motor->dtc_b = 1.0f + dx + dy;
+        motor->dtc_c = 1.0f - dx + dy;
+        break;
+    default:
+        break;
+    }   
+#endif    
     if (motor->halt) {
-        pha = 0.0f;
-        phb = 1.0f;
-        phc = 0.0f;
+        setDtcA(0);
+        setDtcB(1.0f);
+        setDtcC(0);
     } else {
-        pha = DB_COEFF * motor->dtc_a + DB_OFFSET;
-        phb = DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET;
-        phc = DB_COEFF * motor->dtc_c + DB_OFFSET;
+        setDtcA(DB_COEFF * motor->dtc_a + DB_OFFSET);
+        setDtcB(DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET);
+        setDtcC(DB_COEFF * motor->dtc_c + DB_OFFSET);
     }
 }
 
@@ -48,7 +95,7 @@
     }
     
     if(motor->angle > 360.0f) motor->angle = 360.0f;
-    if(motor->angle < 0) motor->angle=0;
+    if(motor->angle < 0) motor->angle = 0;
 }
 
 void throttle_update() {