Diff: util.cpp

Revision:

0:9753f3c2e5ca

Child:

1:70eed554399b

diff -r 000000000000 -r 9753f3c2e5ca util.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/util.cpp	Mon Feb 16 20:16:01 2015 +0000
@@ -0,0 +1,52 @@
+#include "constants.h"
+#include "shared.h"
+#include "isr.h"
+#include <stdlib.h>
+
+void initTimers() {
+    dtc_upd_ticker.attach_us(&dtc_update, 20);    
+    pha.period_us(200);
+    phb.period_us(200);
+    phc.period_us(200);
+    pha = 0;
+    phb = 1.0f;
+    phc = 0;
+
+}
+
+void initPins() {    
+    halla.mode(PullUp);
+    hallb.mode(PullUp);
+    hallc.mode(PullUp);
+    
+    en = 0;
+}
+
+void initData() {
+    motor = (Motor*) malloc(sizeof(Motor));
+    motor->hall = (halla.read() << 2) + (hallb.read() << 1) + hallc.read();
+    motor->last_hall = 0;
+    motor->reverses = 0;
+    motor->last_time = 3000.0f;
+    motor->major_pos = 60.0f * ATable[motor->hall];
+    motor->ticks = 0.0f;
+    motor->sensor_phase = SENSOR_PHASE;
+    motor->halt = 1;
+    motor->whangle =0;
+    motor->analoga = analoga;
+    motor->analogb = analogb;
+    
+    float throttle_raw;
+    do {
+        throttle_raw = throttle;
+        throttle_raw = (throttle_raw - THROTTLE_DB) / (1.0f - THROTTLE_DB);
+        if (throttle_raw < 0.0f) throttle_raw = 0.0f;
+    } while (throttle_raw > 0.05f);
+    
+    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