Diff: isr.cpp

Revision:

1:70eed554399b

Parent:

0:9753f3c2e5ca

Child:

2:b5c19d4eddcc

--- a/isr.cpp	Mon Feb 16 20:16:01 2015 +0000
+++ b/isr.cpp	Sat Feb 21 21:39:33 2015 +0000
@@ -3,34 +3,8 @@
 #include "shared.h"
 #include "constants.h"
 
-/*
-void commutate() {
-    if(BROCK_COMMUTATE){
-        motor->hall = (halla.read() << 2) + (hallb.read() << 1) + hallc.read();
-        if (motor->hall != HNext[motor->last_hall] && motor->last_hall != 0) {
-            motor->reverses++;
-            if (motor->reverses < 2) return;
-        }
-        motor->reverses = 0;
-        motor->last_hall = motor->hall;
-        motor->last_time = motor->ticks;
-        motor->major_pos = 60.0f * ATable[motor->hall];
-        motor->ticks = 0.0f;
-    }
-    else{
-        
-    }
-}
-*/
-
-
-
-void dtc_update() {
-    
-    float angle = motor->whangle;
-    
-      
-    float ia = angle - motor->sensor_phase;
+void dtc_update() { 
+    float ia = motor->angle - motor->sensor_phase;
     if (ia < 0) ia += 360.0f;
     if (ia >= 360.0f) ia -= 360.0f;    
     float ib = ia - 120.0f;
@@ -51,4 +25,40 @@
         phb = DB_COEFF * (1.0f - motor->dtc_b) + DB_OFFSET;
         phc = DB_COEFF * motor->dtc_c + DB_OFFSET;
     }
+}
+
+void pos_update() {
+    float ascaled = 2 *(((float) analoga - 0.143f)/(0.618f - 0.143f) - 0.5f);
+    float bscaled = 2 *(((float) analogb - 0.202f)/(0.542f - 0.202f) - 0.5f);
+   
+    float x = bscaled / ascaled;
+    
+    unsigned int index = (abs(x) / ATAN_UPPER_BOUND)* ATAN_TABLE_SIZE;
+    
+    if(index >= ATAN_TABLE_SIZE) index = ATAN_TABLE_SIZE - 1;
+    
+    if(bscaled < 0){
+        if(ascaled < 0) motor->angle = arctan[index];
+        if(ascaled > 0) motor->angle = 180.0f - arctan[index];
+    } 
+    
+    if(bscaled > 0){
+        if(ascaled > 0) motor->angle = 180.0f + arctan[index];
+        if(ascaled < 0) motor->angle = 360.0f - arctan[index];
+    }
+    
+    if(motor->angle > 360.0f) motor->angle = 360.0f;
+    if(motor->angle < 0) motor->angle=0;
+}
+
+void throttle_update() {
+    float throttle_raw = throttle_read;
+    throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
+    if (throttle_raw < 0.0f) throttle_raw = 0.0f;
+    motor->throttle = (1.0f - THROTTLE_LPF) * throttle_raw + THROTTLE_LPF * motor->throttle;
+    if (motor->throttle < 0.05f) {
+        motor->halt = 1;
+    } else {
+        motor->halt = 0;
+    }
 }
\ No newline at end of file