derp

Dependencies:   FastPWM3 mbed

Revision:
0:bac9c3a3a6ca
Child:
1:7b61790f6be9
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Mar 09 06:44:51 2016 +0000
@@ -0,0 +1,75 @@
+#include "mbed.h"
+#include "math.h"
+#include "PositionSensor.h"
+#include "FastPWM.h"
+#include "Transforms.h"
+#include "config.h"
+
+FastPWM a(PWMA);
+FastPWM b(PWMB);
+FastPWM c(PWMC);
+DigitalOut en(EN);
+
+AnalogOut test(TEST_DAC);
+AnalogIn Ia(IA);
+AnalogIn Ib(IB);
+
+PositionSensorEncoder pos(CPR, 0);
+
+Serial pc(USBTX, USBRX);
+
+using namespace Transforms;
+
+void config_globals() {
+    pc.baud(115200);
+    
+    a.period_us(200);
+    b.period_us(200);
+    c.period_us(200);
+    a = 1.0f;
+    b = 1.0f;
+    c = 1.0f;
+    
+    en = 1;
+}
+
+void startup_msg() {
+    pc.printf("%s\n\r\n\r", "FOC'ed in the Bot Rev A.");
+    pc.printf("%s\n\r", "====Config Data====");
+    pc.printf("Current Sensor Offset: %f mV\n\r", I_OFFSET);
+    pc.printf("Current Sensor Scale: %f mv/A\n\r", I_SCALE);
+    pc.printf("Bus Voltage: %f V\n\r", BUS_VOLTAGE);
+    pc.printf("Loop KP: %f\n\r", KP);
+    pc.printf("Loop KI: %f\n\r", KI);
+    pc.printf("\n\r");
+}    
+
+void main_loop() {
+    float p = pos.GetElecPosition() - POS_OFFSET + PI / 2;
+    if (p < 0) p += 2 * PI;
+    
+    float pos_dac = 0.85f * p / (2 * PI) + 0.05f;
+    
+    float v_ia = Ia.read() * AVDD;
+    float v_ib = Ib.read() * AVDD;
+    
+    float ia = (v_ia - I_OFFSET) / I_SCALE;
+    float ib = (v_ia - I_OFFSET) / I_SCALE;
+    
+    test = pos_dac;
+    
+    set_dtc(a, 0.5f + 0.5f * cosf(p));
+    set_dtc(b, 0.5f + 0.5f * cosf(p + 2 * PI / 3));
+    set_dtc(c, 0.5f + 0.5f * cosf(p - 2 * PI / 3));
+}
+    
+int main() {
+    config_globals();
+    startup_msg();
+    
+    Ticker loop;
+    loop.attach_us(main_loop, 200);
+    
+    for (;;) {
+    }
+}