Bayley Wang
/
foc-ed_in_the_bot_compact
robot
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