EFeru/hoverboard-firmware-hack-SIN
1
0
Fork 0
forked from lucysrausch/hoverboard-firmware-hack
Code Pull Requests Activity

Added new BLDC motor control

Browse Source
This commit is contained in:
EmanuelFeru
2019-05-26 15:33:57 +02:00
parent f06ce1fcd3
commit febbfc823c
11 changed files with 2428 additions and 1412 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1313
Src/BLDC_controller.c Normal file
View File

File diff suppressed because it is too large Load Diff

212
Src/BLDC_controller_data.c Normal file
View File

@@ -0,0 +1,212 @@
/*
* Academic License - for use in teaching, academic research, and meeting
* course requirements at degree granting institutions only. Not for
* government, commercial, or other organizational use.
*
* File: BLDC_controller_data.c
*
* Code generated for Simulink model 'BLDC_controller'.
*
* Model version : 1.800
* Simulink Coder version : 8.13 (R2017b) 24-Jul-2017
* C/C++ source code generated on : Sat May 25 21:42:39 2019
*
* Target selection: ert.tlc
* Embedded hardware selection: ARM Compatible->ARM Cortex
* Emulation hardware selection:
* Differs from embedded hardware (MATLAB Host)
* Code generation objectives:
* 1. Execution efficiency
* 2. RAM efficiency
* Validation result: Not run
*/
#include "BLDC_controller.h"
/* Block parameters (auto storage) */
P rtP = {
/* Variable: cf_spdCoef
* Referenced by:
* '<S29>/cf_spdCoef'
* '<S69>/cf_spdCoef'
*/
66667,
/* Variable: n_commAcvLo
* Referenced by:
* '<S15>/Relay'
* '<S55>/Relay'
*/
100,
/* Variable: n_commDeacvHi
* Referenced by:
* '<S15>/Relay'
* '<S55>/Relay'
*/
180,
/* Variable: n_thresSpdDeacv
* Referenced by:
* '<S23>/n_thresSpdDeacv'
* '<S63>/n_thresSpdDeacv'
*/
5,
/* Variable: r_commDCDeacv
* Referenced by:
* '<S15>/r_commDCDeacv'
* '<S55>/r_commDCDeacv'
*/
70,
/* Variable: r_phaAdvDC_XA
* Referenced by:
* '<S13>/r_phaAdvDC_XA'
* '<S53>/r_phaAdvDC_XA'
*/
{ 0, 100, 200, 300, 400, 500, 600, 700, 800, 900, 1000 },
/* Variable: a_phaAdv_M1
* Referenced by:
* '<S13>/a_phaAdv_M2'
* '<S53>/a_phaAdv_M2'
*/
{ 0, 0, 7, 2, 2, 2, 4, 5, 9, 16, 25 },
/* Variable: z_maxCntRst
* Referenced by:
* '<S23>/z_maxCntRst'
* '<S63>/z_maxCntRst'
* '<S29>/z_maxCntRst'
* '<S69>/z_maxCntRst'
*/
2000,
/* Variable: z_ctrlTypSel
* Referenced by:
* '<S12>/z_ctrlTypSel1'
* '<S52>/z_ctrlTypSel1'
*/
3U,
/* Variable: z_nrEdgeSpdAcv
* Referenced by:
* '<S23>/z_nrEdgeSpdAcv'
* '<S63>/z_nrEdgeSpdAcv'
*/
5U,
/* Variable: b_phaAdvEna
* Referenced by:
* '<S13>/a_elecPeriod1'
* '<S53>/a_elecPeriod1'
*/
1
};
/* Constant parameters (auto storage) */
const ConstP rtConstP = {
/* Pooled Parameter (Expression: r_trapPhaA_M1)
* Referenced by:
* '<S43>/r_trapPhaA_M1'
* '<S83>/r_trapPhaA_M1'
*/
{ 1000, 1000, 1000, -1000, -1000, -1000, 1000 },
/* Pooled Parameter (Expression: r_trapPhaB_M1)
* Referenced by:
* '<S43>/r_trapPhaB_M1'
* '<S83>/r_trapPhaB_M1'
*/
{ -1000, -1000, 1000, 1000, 1000, -1000, -1000 },
/* Pooled Parameter (Expression: r_trapPhaC_M1)
* Referenced by:
* '<S43>/r_trapPhaC_M1'
* '<S83>/r_trapPhaC_M1'
*/
{ 1000, -1000, -1000, -1000, 1000, 1000, 1000 },
/* Pooled Parameter (Expression: r_sinPhaA_M1)
* Referenced by:
* '<S45>/r_sinPhaA_M1'
* '<S85>/r_sinPhaA_M1'
*/
{ 500, 643, 766, 866, 940, 985, 1000, 985, 940, 866, 766, 643, 500, 342, 174,
0, -174, -342, -500, -643, -766, -866, -940, -985, -1000, -985, -940, -866,
-766, -643, -500, -342, -174, 0, 174, 342, 500 },
/* Pooled Parameter (Expression: r_sinPhaB_M1)
* Referenced by:
* '<S45>/r_sinPhaB_M1'
* '<S85>/r_sinPhaB_M1'
*/
{ -1000, -985, -940, -866, -766, -643, -500, -342, -174, 0, 174, 342, 500, 643,
766, 866, 940, 985, 1000, 985, 940, 866, 766, 643, 500, 342, 174, 0, -174,
-342, -500, -643, -766, -866, -940, -985, -1000 },
/* Pooled Parameter (Expression: r_sinPhaC_M1)
* Referenced by:
* '<S45>/r_sinPhaC_M1'
* '<S85>/r_sinPhaC_M1'
*/
{ 500, 342, 174, 0, -174, -342, -500, -643, -766, -866, -940, -985, -1000,
-985, -940, -866, -766, -643, -500, -342, -174, 0, 174, 342, 500, 643, 766,
866, 940, 985, 1000, 985, 940, 866, 766, 643, 500 },
/* Pooled Parameter (Expression: r_sin3PhaA_M1)
* Referenced by:
* '<S44>/r_sin3PhaA_M1'
* '<S84>/r_sin3PhaA_M1'
*/
{ 795, 930, 991, 996, 971, 942, 930, 942, 971, 996, 991, 930, 795, 584, 310, 0,
-310, -584, -795, -930, -991, -996, -971, -942, -930, -942, -971, -996, -991,
-930, -795, -584, -310, 0, 310, 584, 795 },
/* Pooled Parameter (Expression: r_sin3PhaB_M1)
* Referenced by:
* '<S44>/r_sin3PhaB_M1'
* '<S84>/r_sin3PhaB_M1'
*/
{ -930, -942, -971, -996, -991, -930, -795, -584, -310, 0, 310, 584, 795, 930,
991, 996, 971, 942, 930, 942, 971, 996, 991, 930, 795, 584, 310, 0, -310,
-584, -795, -930, -991, -996, -971, -942, -930 },
/* Pooled Parameter (Expression: r_sin3PhaC_M1)
* Referenced by:
* '<S44>/r_sin3PhaC_M1'
* '<S84>/r_sin3PhaC_M1'
*/
{ 795, 584, 310, 0, -310, -584, -795, -930, -991, -996, -971, -942, -930, -942,
-971, -996, -991, -930, -795, -584, -310, 0, 310, 584, 795, 930, 991, 996,
971, 942, 930, 942, 971, 996, 991, 930, 795 },
/* Pooled Parameter (Expression: z_commutMap_M1)
* Referenced by:
* '<S15>/z_commutMap_M1'
* '<S55>/z_commutMap_M1'
*/
{ 1000, -1000, 0, 1000, 0, -1000, 0, 1000, -1000, -1000, 1000, 0, -1000, 0,
1000, 0, -1000, 1000 },
/* Pooled Parameter (Expression: vec_hallToPos)
* Referenced by:
* '<S21>/vec_hallToPos'
* '<S61>/vec_hallToPos'
*/
{ 0U, 5U, 3U, 4U, 1U, 0U, 2U, 0U },
/* Pooled Parameter (Expression: [0 1;1 0;0 1;0 1;1 0;1 0;0 0;0 0])
* Referenced by:
* '<S30>/Logic'
* '<S70>/Logic'
*/
{ 0, 1, 0, 0, 1, 1, 0, 0, 1, 0, 1, 1, 0, 0, 0, 0 }
};
/*
* File trailer for generated code.
*
* [EOF]
*/

284
Src/bldc.c
View File

@@ -1,16 +1,38 @@
/*
* This file has been re-implemented with 4 new selectable motor control methods.
* Recommended control method: 3 = Sinusoidal 3rd order. This control method offers superior performanace
* compared to previous method. The new method features:
* ► reduced noise and vibrations
* ► smooth torque output
* ► improved motor efficiency -> lower energy consumption
*
* Copyright (C) 2019 Emanuel FERU <aerdronix@gmail.com>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "stm32f1xx_hal.h"
#include "defines.h"
#include "setup.h"
#include "config.h"
// Matlab includes
#include "BLDC_controller.h" /* Model's header file */
#include "rtwtypes.h"
volatile int posl = 0;
volatile int posr = 0;
volatile int pwml = 0;
volatile int pwmr = 0;
volatile int weakl = 0;
volatile int weakr = 0;
extern volatile int speed;
@@ -18,110 +40,13 @@ extern volatile adc_buf_t adc_buffer;
extern volatile uint32_t timeout;
uint32_t buzzerFreq = 0;
uint32_t buzzerPattern = 0;
uint32_t buzzerFreq = 0;
uint32_t buzzerPattern = 0;
uint32_t buzzerTimer = 0;
uint8_t enable = 0;
uint8_t enable = 0;
const int pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000
const uint8_t hall_to_pos[8] = {
0,
0,
2,
1,
4,
5,
3,
0,
};
inline void blockPWM(int pwm, int pos, int *u, int *v, int *w) {
switch(pos) {
case 0:
*u = 0;
*v = pwm;
*w = -pwm;
break;
case 1:
*u = -pwm;
*v = pwm;
*w = 0;
break;
case 2:
*u = -pwm;
*v = 0;
*w = pwm;
break;
case 3:
*u = 0;
*v = -pwm;
*w = pwm;
break;
case 4:
*u = pwm;
*v = -pwm;
*w = 0;
break;
case 5:
*u = pwm;
*v = 0;
*w = -pwm;
break;
default:
*u = 0;
*v = 0;
*w = 0;
}
}
inline void blockPhaseCurrent(int pos, int u, int v, int *q) {
switch(pos) {
case 0:
*q = u - v;
// *u = 0;
// *v = pwm;
// *w = -pwm;
break;
case 1:
*q = u;
// *u = -pwm;
// *v = pwm;
// *w = 0;
break;
case 2:
*q = u;
// *u = -pwm;
// *v = 0;
// *w = pwm;
break;
case 3:
*q = v;
// *u = 0;
// *v = -pwm;
// *w = pwm;
break;
case 4:
*q = v;
// *u = pwm;
// *v = -pwm;
// *w = 0;
break;
case 5:
*q = -(u - v);
// *u = pwm;
// *v = 0;
// *w = -pwm;
break;
default:
*q = 0;
// *u = 0;
// *v = 0;
// *w = 0;
}
}
uint32_t buzzerTimer = 0;
const int pwm_res = 64000000 / 2 / PWM_FREQ; // = 2000
int offsetcount = 0;
int offsetrl1 = 2000;
@@ -133,20 +58,11 @@ int offsetdcr = 2000;
float batteryVoltage = BAT_NUMBER_OF_CELLS * 4.0;
int curl = 0;
// int errorl = 0;
// int kp = 5;
// volatile int cmdl = 0;
int last_pos = 0;
int timer = 0;
const int max_time = PWM_FREQ / 10;
volatile int vel = 0;
//scan 8 channels with 2ADCs @ 20 clk cycles per sample
//meaning ~80 ADC clock cycles @ 8MHz until new DMA interrupt =~ 100KHz
//=640 cpu cycles
void DMA1_Channel1_IRQHandler() {
DMA1->IFCR = DMA_IFCR_CTCIF1;
// HAL_GPIO_WritePin(LED_PORT, LED_PIN, 1);
@@ -180,61 +96,6 @@ void DMA1_Channel1_IRQHandler() {
RIGHT_TIM->BDTR |= TIM_BDTR_MOE;
}
int ul, vl, wl;
int ur, vr, wr;
//determine next position based on hall sensors
uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN);
uint8_t hall_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN);
uint8_t hall_wl = !(LEFT_HALL_W_PORT->IDR & LEFT_HALL_W_PIN);
uint8_t hall_ur = !(RIGHT_HALL_U_PORT->IDR & RIGHT_HALL_U_PIN);
uint8_t hall_vr = !(RIGHT_HALL_V_PORT->IDR & RIGHT_HALL_V_PIN);
uint8_t hall_wr = !(RIGHT_HALL_W_PORT->IDR & RIGHT_HALL_W_PIN);
uint8_t halll = hall_ul * 1 + hall_vl * 2 + hall_wl * 4;
posl = hall_to_pos[halll];
posl += 2;
posl %= 6;
uint8_t hallr = hall_ur * 1 + hall_vr * 2 + hall_wr * 4;
posr = hall_to_pos[hallr];
posr += 2;
posr %= 6;
blockPhaseCurrent(posl, adc_buffer.rl1 - offsetrl1, adc_buffer.rl2 - offsetrl2, &curl);
//setScopeChannel(2, (adc_buffer.rl1 - offsetrl1) / 8);
//setScopeChannel(3, (adc_buffer.rl2 - offsetrl2) / 8);
// uint8_t buzz(uint16_t *notes, uint32_t len){
// static uint32_t counter = 0;
// static uint32_t timer = 0;
// if(len == 0){
// return(0);
// }
// struct {
// uint16_t freq : 4;
// uint16_t volume : 4;
// uint16_t time : 8;
// } note = notes[counter];
// if(timer / 500 == note.time){
// timer = 0;
// counter++;
// }
// if(counter == len){
// counter = 0;
// }
// timer++;
// return(note.freq);
// }
//create square wave for buzzer
buzzerTimer++;
if (buzzerFreq != 0 && (buzzerTimer / 5000) % (buzzerPattern + 1) == 0) {
@@ -245,35 +106,64 @@ void DMA1_Channel1_IRQHandler() {
HAL_GPIO_WritePin(BUZZER_PORT, BUZZER_PIN, 0);
}
//update PWM channels based on position
blockPWM(pwml, posl, &ul, &vl, &wl);
blockPWM(pwmr, posr, &ur, &vr, &wr);
// ############################### MOTOR CONTROL ###############################
int ul, vl, wl;
int ur, vr, wr;
int weakul, weakvl, weakwl;
if (pwml > 0) {
blockPWM(weakl, (posl+5) % 6, &weakul, &weakvl, &weakwl);
} else {
blockPWM(-weakl, (posl+1) % 6, &weakul, &weakvl, &weakwl);
// Get hall sensors values
uint8_t hall_ul = !(LEFT_HALL_U_PORT->IDR & LEFT_HALL_U_PIN);
uint8_t hall_vl = !(LEFT_HALL_V_PORT->IDR & LEFT_HALL_V_PIN);
uint8_t hall_wl = !(LEFT_HALL_W_PORT->IDR & LEFT_HALL_W_PIN);
uint8_t hall_ur = !(RIGHT_HALL_U_PORT->IDR & RIGHT_HALL_U_PIN);
uint8_t hall_vr = !(RIGHT_HALL_V_PORT->IDR & RIGHT_HALL_V_PIN);
uint8_t hall_wr = !(RIGHT_HALL_W_PORT->IDR & RIGHT_HALL_W_PIN);
static boolean_T OverrunFlag = false;
/* Check for overrun */
if (OverrunFlag) {
return;
}
ul += weakul;
vl += weakvl;
wl += weakwl;
OverrunFlag = true;
/* Set motor inputs here */
rtU.b_hallALeft = hall_ul;
rtU.b_hallBLeft = hall_vl;
rtU.b_hallCLeft = hall_wl;
rtU.r_DCLeft = pwml;
int weakur, weakvr, weakwr;
if (pwmr > 0) {
blockPWM(weakr, (posr+5) % 6, &weakur, &weakvr, &weakwr);
} else {
blockPWM(-weakr, (posr+1) % 6, &weakur, &weakvr, &weakwr);
}
ur += weakur;
vr += weakvr;
wr += weakwr;
rtU.b_hallARight = hall_ur;
rtU.b_hallBRight = hall_vr;
rtU.b_hallCRight = hall_wr;
rtU.r_DCRight = pwmr;
/* Step the controller */
BLDC_controller_step();
/* Get motor outputs here */
ul = rtY.DC_phaALeft;
vl = rtY.DC_phaBLeft;
wl = rtY.DC_phaCLeft;
// motSpeedLeft = rtY.n_motLeft;
// motAngleLeft = rtY.a_elecAngleLeft;
ur = rtY.DC_phaARight;
vr = rtY.DC_phaBRight;
wr = rtY.DC_phaCRight;
// motSpeedRight = rtY.n_motRight;
// motAngleRight = rtY.a_elecAngleRight;
/* Indicate task complete */
OverrunFlag = false;
// ###############################################################################
LEFT_TIM->LEFT_TIM_U = CLAMP(ul + pwm_res / 2, 10, pwm_res-10);
LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 10, pwm_res-10);
LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 10, pwm_res-10);
RIGHT_TIM->RIGHT_TIM_U = CLAMP(ur + pwm_res / 2, 10, pwm_res-10);
RIGHT_TIM->RIGHT_TIM_V = CLAMP(vr + pwm_res / 2, 10, pwm_res-10);
RIGHT_TIM->RIGHT_TIM_W = CLAMP(wr + pwm_res / 2, 10, pwm_res-10);
LEFT_TIM->LEFT_TIM_U = CLAMP(ul + pwm_res / 2, 10, pwm_res-10);
LEFT_TIM->LEFT_TIM_V = CLAMP(vl + pwm_res / 2, 10, pwm_res-10);
LEFT_TIM->LEFT_TIM_W = CLAMP(wl + pwm_res / 2, 10, pwm_res-10);
RIGHT_TIM->RIGHT_TIM_U = CLAMP(ur + pwm_res / 2, 10, pwm_res-10);
RIGHT_TIM->RIGHT_TIM_V = CLAMP(vr + pwm_res / 2, 10, pwm_res-10);
RIGHT_TIM->RIGHT_TIM_W = CLAMP(wr + pwm_res / 2, 10, pwm_res-10);
}

58
Src/main.c
View File

@@ -23,8 +23,13 @@
#include "defines.h"
#include "setup.h"
#include "config.h"
#include <stdlib.h>
//#include "hd44780.h"
// Matlab includes - from auto-code generation
#include "BLDC_controller.h" /* Model's header file */
#include "rtwtypes.h"
void SystemClock_Config(void);
extern TIM_HandleTypeDef htim_left;
@@ -38,7 +43,6 @@ extern UART_HandleTypeDef huart2;
int cmd1; // normalized input values. -1000 to 1000
int cmd2;
int cmd3;
typedef struct{
int16_t steer;
@@ -55,8 +59,6 @@ int speed; // global variable for speed. -1000 to 1000
extern volatile int pwml; // global variable for pwm left. -1000 to 1000
extern volatile int pwmr; // global variable for pwm right. -1000 to 1000
extern volatile int weakl; // global variable for field weakening left. -1000 to 1000
extern volatile int weakr; // global variable for field weakening right. -1000 to 1000
extern uint8_t buzzerFreq; // global variable for the buzzer pitch. can be 1, 2, 3, 4, 5, 6, 7...
extern uint8_t buzzerPattern; // global variable for the buzzer pattern. can be 1, 2, 3, 4, 5, 6, 7...
@@ -77,7 +79,7 @@ int milli_vel_error_sum = 0;
void poweroff() {
if (abs(speed) < 20) {
// if (abs(speed) < 20) { // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons
buzzerPattern = 0;
enable = 0;
for (int i = 0; i < 8; i++) {
@@ -86,11 +88,12 @@ void poweroff() {
}
HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, 0);
while(1) {}
}
// }
}
int main(void) {
HAL_Init();
__HAL_RCC_AFIO_CLK_ENABLE();
HAL_NVIC_SetPriorityGrouping(NVIC_PRIORITYGROUP_4);
@@ -127,6 +130,19 @@ int main(void) {
HAL_ADC_Start(&hadc1);
HAL_ADC_Start(&hadc2);
//////////////////////////////////////
/* Set BLDC controller parameters */
rtP.z_ctrlTypSel = CTRL_TYP_SEL;
rtP.b_phaAdvEna = PHASE_ADV_ENA;
rtP.n_commDeacvHi = COMM_DEACV_HI;
rtP.n_commAcvLo = COMM_ACV_LO;
/* Initialize BLDC controller */
BLDC_controller_initialize();
/////////////////////////////////////
for (int i = 8; i >= 0; i--) {
buzzerFreq = i;
HAL_Delay(100);
@@ -137,7 +153,6 @@ int main(void) {
int lastSpeedL = 0, lastSpeedR = 0;
int speedL = 0, speedR = 0;
float direction = 1;
#ifdef CONTROL_PPM
PPM_Init();
@@ -180,6 +195,7 @@ int main(void) {
enable = 1; // enable motors
while(1) {
HAL_Delay(DELAY_IN_MAIN_LOOP); //delay in ms
@@ -201,8 +217,8 @@ int main(void) {
#ifdef CONTROL_ADC
// ADC values range: 0-4095, see ADC-calibration in config.h
cmd1 = CLAMP(adc_buffer.l_tx2 - ADC1_MIN, 0, ADC1_MAX) / (ADC1_MAX / 1000.0f); // ADC1
cmd2 = CLAMP(adc_buffer.l_rx2 - ADC2_MIN, 0, ADC2_MAX) / (ADC2_MAX / 1000.0f); // ADC2
cmd1 = CLAMP(adc_buffer.l_tx2 - ADC1_MIN, 0, ADC1_MAX) / (ADC1_MAX / 1000.0f); // ADC1
cmd2 = CLAMP(adc_buffer.l_rx2 - ADC2_MIN, 0, ADC2_MAX) / (ADC2_MAX / 1000.0f); // ADC2
// use ADCs as button inputs:
button1 = (uint8_t)(adc_buffer.l_tx2 > 2000); // ADC1
@@ -218,7 +234,6 @@ int main(void) {
timeout = 0;
#endif
// ####### LOW-PASS FILTER #######
steer = steer * (1.0 - FILTER) + cmd1 * FILTER;
speed = speed * (1.0 - FILTER) + cmd2 * FILTER;
@@ -234,7 +249,7 @@ int main(void) {
#endif
// ####### SET OUTPUTS #######
// ####### SET OUTPUTS (if the target change less than +/- 50) #######
if ((speedL < lastSpeedL + 50 && speedL > lastSpeedL - 50) && (speedR < lastSpeedR + 50 && speedR > lastSpeedR - 50) && timeout < TIMEOUT) {
#ifdef INVERT_R_DIRECTION
pwmr = speedR;
@@ -256,24 +271,27 @@ int main(void) {
// ####### CALC BOARD TEMPERATURE #######
board_temp_adc_filtered = board_temp_adc_filtered * 0.99 + (float)adc_buffer.temp * 0.01;
board_temp_deg_c = ((float)TEMP_CAL_HIGH_DEG_C - (float)TEMP_CAL_LOW_DEG_C) / ((float)TEMP_CAL_HIGH_ADC - (float)TEMP_CAL_LOW_ADC) * (board_temp_adc_filtered - (float)TEMP_CAL_LOW_ADC) + (float)TEMP_CAL_LOW_DEG_C;
// ####### DEBUG SERIAL OUT #######
#ifdef CONTROL_ADC
setScopeChannel(0, (int)adc_buffer.l_tx2); // 1: ADC1
setScopeChannel(1, (int)adc_buffer.l_rx2); // 2: ADC2
// setScopeChannel(0, (int)adc_buffer.l_tx2); // 1: ADC1
// setScopeChannel(1, (int)adc_buffer.l_rx2); // 2: ADC2
#endif
setScopeChannel(2, (int)speedR); // 3: output speed: 0-1000
setScopeChannel(3, (int)speedL); // 4: output speed: 0-1000
setScopeChannel(4, (int)adc_buffer.batt1); // 5: for battery voltage calibration
setScopeChannel(5, (int)(batteryVoltage * 100.0f)); // 6: for verifying battery voltage calibration
setScopeChannel(6, (int)board_temp_adc_filtered); // 7: for board temperature calibration
setScopeChannel(7, (int)board_temp_deg_c); // 8: for verifying board temperature calibration
setScopeChannel(0, (int)speedR); // 1: output command: [-1000, 1000]
setScopeChannel(1, (int)speedL); // 2: output command: [-1000, 1000]
setScopeChannel(2, (int)rtY.n_motRight); // 3: Real motor speed [rpm]
setScopeChannel(3, (int)rtY.n_motLeft); // 4: Real motor speed [rpm]
setScopeChannel(4, (int)adc_buffer.batt1); // 5: for battery voltage calibration
setScopeChannel(5, (int)(batteryVoltage * 100.0f)); // 6: for verifying battery voltage calibration
setScopeChannel(6, (int)board_temp_adc_filtered); // 7: for board temperature calibration
setScopeChannel(7, (int)board_temp_deg_c); // 8: for verifying board temperature calibration
consoleScope();
}
// ####### POWEROFF BY POWER-BUTTON #######
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN) && weakr == 0 && weakl == 0) {
if (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {
enable = 0;
while (HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN)) {}
poweroff();