Diff: isr.cpp

Revision:

5:eeb8af99cb6c

Parent:

4:f18f6bc5e1fd

Child:

6:4960629abb90

--- a/isr.cpp	Thu Feb 26 04:49:21 2015 +0000
+++ b/isr.cpp	Thu Feb 26 09:50:36 2015 +0000
@@ -3,6 +3,7 @@
 #include "shared.h"
 #include "constants.h"
 #include "util.h"
+#include "math.h"
 
 void dtc_update() { 
 #ifndef __SVM
@@ -97,6 +98,20 @@
     if(motor->angle > 360.0f) motor->angle = 360.0f;
     if(motor->angle < 0) motor->angle = 0;
     
+    //calculate rotational rate
+    
+    float speed_raw = motor->angle - motor->lastangle;
+    //these IFs check for theta = 0 crossings
+    if(speed_raw < -180.0f){
+        speed_raw = motor->angle - motor->lastangle + 360.0f;
+    }
+    if(speed_raw > 180.0f){
+        speed_raw = motor->angle - motor->lastangle - 360.0f;
+    }
+    motor->lastangle = motor->angle;
+    
+    motor->speed = (1.0f - SPEED_LPF) * speed_raw + SPEED_LPF * motor->speed;
+    
 #ifdef __DEBUG
     skipidx++;
     if (skipidx == SKIP) {
@@ -117,9 +132,25 @@
     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) {
+    if (motor->throttle < 0.05f & abs(motor->speed) < 1.0f) {
         motor->halt = 1;
     } else {
         motor->halt = 0;
     }
+}
+
+void isense_update() {
+    float ia = ia_read;
+    float ib = ib_read;
+    float irms = sqrt(ia*ia + ib*ib + (ia+ib)*(ia+ib));
+    
+    
+    if(motor->halt == 1){ //zero the current sensor OMG THIS IS SO SCARY OMG
+        motor->izero = irms;
+    }
+    
+    motor->current = irms-motor->izero;
+    
+    //float iset = motor->imax * motor->throttle
+    //float error = irms - iset  
 }
\ No newline at end of file