Diff: main.cpp

Revision:

82:5e741c5ffd9f

Parent:

76:c66014e50384

Child:

83:eb3704d4943f

--- a/main.cpp	Sat Feb 25 01:12:27 2017 +0000
+++ b/main.cpp	Fri Mar 10 08:29:13 2017 +0000
@@ -8,10 +8,13 @@
 #include "Transforms.h"
 #include "DQMapper.h"
 #include "ThrottleMapper.h"
+#include "PreferenceWriter.h"
+#include "CommandProcessor.h"
 
 #include "BREMSStructs.h"
 #include "BREMSConfig.h"
 
+#include "config.h"
 #include "config_motor.h"
 #include "config_loop.h"
 #include "config_pins.h"
@@ -30,7 +33,7 @@
 ThrottleMapper *th;
 
 int loop_counter = 0;
-bool control_enabled = false;
+float user_cmd = 0.0f;
 
 void update_velocity() {
     read.last_p_mech = read.p_mech;
@@ -49,7 +52,7 @@
 
 void commutate() {    
     /*safety checks, do we do anything this cycle?*/
-    if (!control_enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
+    if (!control.enabled && io.throttle_in->get_enabled() && io.pos->IsValid() && is_driving()) {
         go_enabled();
     }
     
@@ -74,6 +77,10 @@
     clarke(foc.ia, foc.ib, &foc.alpha, &foc.beta);
     park(foc.alpha, foc.beta, sin_p, cos_p, &foc.d, &foc.q);
     
+    /*get references*/
+    /*happens here, as we may want other controllers running each cycle*/
+    dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
+    
     /*PI controller*/
     control.d_filtered = update_filter(control.d_filtered, foc.d, DQ_FILTER_STRENGTH);
     control.q_filtered = update_filter(control.q_filtered, foc.q, DQ_FILTER_STRENGTH);
@@ -91,7 +98,7 @@
     
     constrain_norm(&foc.vd, &foc.vq, 1.0f, 1.0f, 1.0f);
     
-    if (!control_enabled) {
+    if (!control.enabled) {
         foc.vd = 0.0f;
         foc.vq = 0.0f;
     }
@@ -123,40 +130,36 @@
 }
 
 void slow_loop() {
-    float x = io.throttle_in->get_throttle();
-    control.torque_percent = update_filter(control.torque_percent, th->map(io.throttle_in->get_throttle(), read.w), THROTTLE_FILTER_STRENGTH);
-    
-    dq->map(control.torque_percent, read.w, &control.d_ref, &control.q_ref);
-}
-
-void go_enabled() {
-    control_enabled = true;
-    io.en->write(1);
-}
-
-void go_disabled() {
-    control.d_integral = 0.0f;
-    control.q_integral = 0.0f;
-    control_enabled = false;
-    io.en->write(0);
-}
-
-bool is_driving() {
-    return io.throttle_in->get_throttle() > 0.01f || fabsf(read.w) > W_SAFE;
-}
-
-float update_filter(float old, float x, float str) {
-    return str * old + (1.0f - str) * x;
-}
-
-void log() {
-    io.pc->printf("%d,%d,%d,%d,%d,%d,%d,%d\n", (int) read.w, (int) control.d_ref, (int) control.d_filtered, (int) control.q_ref, (int) control.q_filtered, (int) (255 * control.torque_percent), 
-                        (int) (255 * foc.vd), (int) (255 * foc.vq));
-    //io.pc->printf("%d\n", io.throttle_in->get_usecs());
-    wait(1.0f / LOG_FREQUENCY);
+    switch (BREMS_src) {
+    case CMD_SRC_RC:
+        user_cmd = update_filter(user_cmd, io.throttle_in->get_throttle(), THROTTLE_FILTER_STRENGTH);
+        break;
+    case CMD_SRC_ANALOG:
+        //analog code here;
+        break;
+    case CMD_SRC_TERMINAL:
+    case CMD_SRC_SERIAL:
+    case CMD_SRC_CAN:
+    case CMD_SRC_INTERNAL:
+    default:
+        break;
+    }
+    switch (BREMS_op) {
+    case OP_TORQUE:
+        control.torque_percent = user_cmd;
+        break;
+    case OP_DRIVING:
+        control.torque_percent = th->map(user_cmd, read.w);
+        break;
+    case OP_SPEED:
+        //set velocity setpoint here;
+        break;
+    case OP_POSITION:
+        //set pos setpoint here;
+        break;
+    }
 }
         
-
 extern "C" void TIM1_UP_TIM10_IRQHandler(void) {
     int start_state = io.throttle_in->state();
     if (TIM1->SR & TIM_SR_UIF ) {
@@ -175,11 +178,10 @@
 int main() {
     dq = new LutMapper();
     th = new NullThrottleMapper();
-    BREMSInit(&io, &read, &foc, &control, false);
+
+    BREMSInit(&io, &read, &foc, &control);
+    io.pc->attach(rxCallback);
     
     for (;;) {
-        if (ENABLE_LOGGING) {
-            log();
-        }
     }
 }
\ No newline at end of file