Diff: main.cpp

Revision:

3:82c00c8e2cb4

Parent:

2:cfc39b0843ae

--- a/main.cpp	Thu Mar 02 07:36:56 2017 +0000
+++ b/main.cpp	Tue Mar 07 08:06:39 2017 +0000
@@ -1,4 +1,5 @@
 #include "mbed.h"
+#include "math.h"
 
 #include "PreferenceWriter.h"
 #include "FlashWriter.h"
@@ -7,10 +8,13 @@
 
 Serial pc(USBTX, USBRX);
 PreferenceWriter pref(6);
-DigitalOut led(LED1);
+PwmOut led(LED1);
 
-int index = 0, mode = MODE_RUN;
+int index = 0, loop_counter = 0;
 char linebuf[128];
+float p = 0.0f;
+float torque_pct = 0.0f, user_cmd = 0.0f;
+float vout = 0.0f;
 
 void rxCallback() {
     while (pc.readable()) {
@@ -34,18 +38,74 @@
     }
 }
 
-void commutate() {
-    if (mode != MODE_RUN) {
-        led = 0;
-    } else {
-        led = 1;
+void slow_loop() {
+    switch (BREMS_src) {
+    case CMD_SRC_RC:
+        //rc throttle code here;
+        break;        
+    case CMD_SRC_ANALOG:
+        //analog throttle code here;
+        break;
+    case CMD_SRC_SERIAL:
+    case CMD_SRC_TERMINAL:
+    case CMD_SRC_CAN:
+        //we presume these are set by free-running interrupts;
+        break;
+    default:
+        break;
     }
 }
 
-int main() {
+void commutate() {
+    p += 0.001f;
+    if (p >= 6.28318f) p = 0.0f;
+    
+    loop_counter++;
+    if (loop_counter % 50 == 0) {
+        loop_counter = 0;
+        slow_loop();
+    }
+    
+    switch (BREMS_op) {
+    case OP_TORQUE:
+        torque_pct = user_cmd;
+        break;
+    case OP_DRIVING:
+        //driving mode here;
+        break;
+    case OP_SPEED:
+        //speed loop here;
+        break;
+    case OP_POSITION:
+        //position loop here;
+        break;
+    }
+    
+    switch (BREMS_mode) {
+    case MODE_RUN:
+        vout = torque_pct * sinf(p);
+        break;
+    case MODE_CFG:
+        vout = 0.0f;
+        break;
+    case MODE_ZERO:
+        //zero'ing code here;
+        break;
+    case MODE_CHR:
+        //wizarding code here;
+        break;
+    default:
+        break;
+    }
+    
+    led = 0.5f + 0.5f * vout;
+}
+
+int main() {    
     pc.baud(115200);
     pc.attach(rxCallback);
-    pc.printf("%s\n", "Serial Configuration Test");
+    pc.printf("%s\n", "FOC'ed in the bot version A");
+    cmd_reload(&pc, &pref);
     pc.printf("%s", ">");
     
     Ticker tick;