From 3d59c76f65a080a9fc34ca3c25744b6d944a83d1 Mon Sep 17 00:00:00 2001 From: eddyem Date: Sat, 25 May 2019 17:16:04 +0300 Subject: [PATCH] 3 servos management in development --- .gitignore | 6 + F0-nolib/Servo/Readme.md | 13 +- F0-nolib/Servo/Servo.creator.user | 20 +-- F0-nolib/Servo/Servo.creator.user.20 | 204 +++++++++++++++++++++++++++ F0-nolib/Servo/Servo.files | 4 +- F0-nolib/Servo/adc.c | 2 +- F0-nolib/Servo/effects.c | 111 +++++++++++++++ F0-nolib/Servo/effects.h | 36 +++++ F0-nolib/Servo/hardware.c | 175 +++++++++++++++-------- F0-nolib/Servo/hardware.h | 62 ++------ F0-nolib/Servo/main.c | 10 +- F0-nolib/Servo/protocol.c | 127 ++++++++++++++--- F0-nolib/Servo/servo.bin | Bin 4172 -> 6300 bytes F0-nolib/Servo/usart.c | 20 ++- F0-nolib/Servo/usart.h | 4 +- 15 files changed, 626 insertions(+), 168 deletions(-) create mode 100644 F0-nolib/Servo/Servo.creator.user.20 create mode 100644 F0-nolib/Servo/effects.c create mode 100644 F0-nolib/Servo/effects.h diff --git a/.gitignore b/.gitignore index 17287e7..3843d7f 100644 --- a/.gitignore +++ b/.gitignore @@ -16,3 +16,9 @@ *.sublime-workspace F1/client-term/client *.bk +*.config +*.creator +*.creator.user* +*.files +*.includes + diff --git a/F0-nolib/Servo/Readme.md b/F0-nolib/Servo/Readme.md index 571e922..9a04c37 100644 --- a/F0-nolib/Servo/Readme.md +++ b/F0-nolib/Servo/Readme.md @@ -2,22 +2,13 @@ Servo motors SG-90 management ============================= ## GPIO - -- PA0 - (ADC_IN0) - Servo1 control, -- PA1 - (ADC_IN1) - Servo2 control, -- PA2 - (ADC_IN2) - Servo3 control, -- PA3 - (ADC_IN3) - external analogue signal, -- PA4 - (PullUp in) - ext. input 0, -- PA5 - (PullUp in) - ext. input 1, +- PA0..PA3 - 4 ADC inputs - PA6 - (TIM3_CH1) - Servo1, - PA7 - (TIM3_CH2) - Servo2, - PA9 - (USART_Tx) - TX, - PA10 - (USART_Rx) - RX, -- PA13 - (PullUp in) - Jumper 0, -- PA14 - (PullUp in) - Jumper 1, - PB1 - (TIM3_CH4) - Servo3, -- PF0 - (OpenDrain) - Buzzer, -- PF1 - (OpenDrain) - External LED (or weak laser module). +- PF1 - (OpenDrain) - external LED or laser (0 - active) ## UART 115200N1, not more than 100ms between data bytes in command. diff --git a/F0-nolib/Servo/Servo.creator.user b/F0-nolib/Servo/Servo.creator.user index f804542..7865f38 100644 --- a/F0-nolib/Servo/Servo.creator.user +++ b/F0-nolib/Servo/Servo.creator.user @@ -1,6 +1,6 @@ - + EnvironmentId @@ -54,10 +54,7 @@ ProjectExplorer.Project.PluginSettings - - - true - + ProjectExplorer.Project.Target.0 @@ -78,7 +75,6 @@ false - false true Сборка @@ -97,7 +93,6 @@ false - false true Сборка @@ -172,19 +167,18 @@ 2 + - Особая программа + + ProjectExplorer.CustomExecutableRunConfiguration - 3768 false true false false true - - 1 @@ -195,10 +189,10 @@ ProjectExplorer.Project.Updater.FileVersion - 20 + 18 Version - 20 + 18 diff --git a/F0-nolib/Servo/Servo.creator.user.20 b/F0-nolib/Servo/Servo.creator.user.20 new file mode 100644 index 0000000..a077e0d --- /dev/null +++ b/F0-nolib/Servo/Servo.creator.user.20 @@ -0,0 +1,204 @@ + + + + + + EnvironmentId + {7bd84e39-ca37-46d3-be9d-99ebea85bc0d} + + + ProjectExplorer.Project.ActiveTarget + 0 + + + ProjectExplorer.Project.EditorSettings + + true + false + true + + Cpp + + CppGlobal + + + + QmlJS + + QmlJSGlobal + + + 2 + KOI8-R + false + 4 + false + 80 + true + true + 1 + true + false + 0 + true + true + 0 + 8 + true + 1 + true + false + true + false + + + + ProjectExplorer.Project.PluginSettings + + + true + + + + ProjectExplorer.Project.Target.0 + + Desktop + Desktop + {65a14f9e-e008-4c1b-89df-4eaa4774b6e3} + 0 + 0 + 0 + + /Big/Data/00__Electronics/STM32/F0-nolib/Servo + + + + all + + false + + + false + true + Сборка + + GenericProjectManager.GenericMakeStep + + 1 + Сборка + + ProjectExplorer.BuildSteps.Build + + + + + clean + + false + + + false + true + Сборка + + GenericProjectManager.GenericMakeStep + + 1 + Очистка + + ProjectExplorer.BuildSteps.Clean + + 2 + false + + По умолчанию + По умолчанию + GenericProjectManager.GenericBuildConfiguration + + 1 + + + 0 + Установка + + ProjectExplorer.BuildSteps.Deploy + + 1 + Конфигурация установки + + ProjectExplorer.DefaultDeployConfiguration + + 1 + + + false + false + 1000 + + true + + false + false + false + false + true + 0.01 + 10 + true + 1 + 25 + + 1 + true + false + true + valgrind + + 0 + 1 + 2 + 3 + 4 + 5 + 6 + 7 + 8 + 9 + 10 + 11 + 12 + 13 + 14 + + 2 + + + Особая программа + + ProjectExplorer.CustomExecutableRunConfiguration + + 3768 + false + true + false + false + true + + + + 1 + + + + ProjectExplorer.Project.TargetCount + 1 + + + ProjectExplorer.Project.Updater.FileVersion + 20 + + + Version + 20 + + diff --git a/F0-nolib/Servo/Servo.files b/F0-nolib/Servo/Servo.files index a21f58e..612e7b0 100644 --- a/F0-nolib/Servo/Servo.files +++ b/F0-nolib/Servo/Servo.files @@ -1,11 +1,11 @@ Makefile adc.c adc.h +effects.c +effects.h hardware.c hardware.h main.c -mainloop.c -mainloop.h protocol.c protocol.h usart.c diff --git a/F0-nolib/Servo/adc.c b/F0-nolib/Servo/adc.c index ede9f9e..ca6872c 100644 --- a/F0-nolib/Servo/adc.c +++ b/F0-nolib/Servo/adc.c @@ -20,7 +20,7 @@ /** * @brief ADC_array - array for ADC channels with median filtering: - * 0..3 - external NTC + * 0..3 - external channels * 4 - internal Tsens * 5 - Vref */ diff --git a/F0-nolib/Servo/effects.c b/F0-nolib/Servo/effects.c new file mode 100644 index 0000000..254edd0 --- /dev/null +++ b/F0-nolib/Servo/effects.c @@ -0,0 +1,111 @@ +/* + * This file is part of the Servo project. + * Copyright 2019 Edward Emelianov . + * + * 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 . + */ + +#include "effects.h" +#include "hardware.h" +#include "usart.h" + +static effect_t current_ef[3] = {EFF_NONE, EFF_NONE, EFF_NONE}; + +#define SPD_STP (25) + +static void eff_madwipe(int n){ + static uint32_t speed[3] = {SPD_STP, SPD_STP, SPD_STP}; + if(onposition(n)){ // move back + if((speed[n]+=SPD_STP) > SG90_STEP) speed[n] = SPD_STP; + int val = 0; + if(getPWM(n) < SG90_MIDPULSE) val = 1; + setPWM(n, val, speed[n]); + } +} + +static void eff_wipe(int n){ + static uint8_t cntr = 0; + if(onposition(n)){ // move back + int val = 0; + if(getPWM(n) < SG90_MIDPULSE) val = 1; + if(++cntr < 4){ // stay a little in outermost positions + setPWM(n, getPWM(n), SG90_STEP/2); + }else{ + cntr = 0; + setPWM(n, val, SG90_STEP/2); + } + } +} + +static void eff_pendulum(int n){ + const uint16_t steps[41] = {0, 10, 21, 33, 47, 62, 79, 97, 117, 140, 165, 193, 224, 258, 295, 337, 383, 434, + 490, 552, 621, 697, 766, 828, 884, 935, 981, 1023, 1060, 1094, 1125, 1153, 1178, + 1201, 1221, 1239, 1256, 1271, 1285, 1297, 1308}; + static int8_t cntr = 0, dir = 1; + if(onposition(n)){ + setPWM(n, SG90_MINPULSE + steps[cntr], SG90_STEP); + cntr += dir; + if(cntr == -1){ // min position + dir = 1; + cntr = 0; // repeat zero position one time + }else if(cntr == 41){ // max position + dir = -1; + cntr = 40; // and this position needs to repeat too + } + } +} + +static void eff_pendsm(int n){ + const uint16_t steps[19] = {0, 6, 10, 15, 22, 30, 40, 52, 66, 82, 101, 123, 148, 177, 210, 247, 289, 336, 389}; + static int8_t cntr = 0, dir = 1; + if(onposition(n)){ + setPWM(n, SG90_MINPULSE + steps[cntr], SG90_STEP); + cntr += dir; + if(cntr == -1){ // min position + dir = 1; + cntr = 1; + }else if(cntr == 19){ // max position + dir = -1; + cntr = 18; + } + } +} + +void proc_effect(){ + for(int i = 0; i < 3; ++i){ + switch(current_ef[i]){ + case EFF_WIPE: + eff_wipe(i); + break; + case EFF_MADWIPE: + eff_madwipe(i); + break; + case EFF_PENDULUM: + eff_pendulum(i); + break; + case EFF_SMPENDULUM: + eff_pendsm(i); + break; + case EFF_NONE: + default: + break; + } + } +} + +effect_t set_effect(int n, effect_t eff){ + if(n < 0 || n > 3) return EFF_NONE; + current_ef[n] = eff; + return eff; +} diff --git a/F0-nolib/Servo/effects.h b/F0-nolib/Servo/effects.h new file mode 100644 index 0000000..f72d94b --- /dev/null +++ b/F0-nolib/Servo/effects.h @@ -0,0 +1,36 @@ +/* + * This file is part of the Servo project. + * Copyright 2019 Edward Emelianov . + * + * 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 . + */ + +#pragma once +#ifndef EFFECTS_H__ +#define EFFECTS_H__ + +#include "stm32f0.h" + +typedef enum{ + EFF_NONE, + EFF_WIPE, + EFF_MADWIPE, + EFF_PENDULUM, + EFF_SMPENDULUM +} effect_t; + +void proc_effect(); +effect_t set_effect(int n, effect_t eff); + +#endif // EFFECTS_H__ diff --git a/F0-nolib/Servo/hardware.c b/F0-nolib/Servo/hardware.c index 938c63b..d4cd5cc 100644 --- a/F0-nolib/Servo/hardware.c +++ b/F0-nolib/Servo/hardware.c @@ -15,9 +15,13 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ + +#include "adc.h" +#include "effects.h" #include "hardware.h" #include "usart.h" -#include "adc.h" + +uint32_t sg90step = SG90DEFSTEP; static inline void iwdg_setup(){ /* Enable the peripheral clock RTC */ @@ -99,13 +103,10 @@ static inline void adc_setup(){ /** * @brief gpio_setup - setup GPIOs for external IO * GPIO pinout: - * PA5 - floating input - Ef of TLE5205 - * PA13 - open drain - IN1 of TLE5205 - * PA14 - open drain - IN2 of TLE5205 - * PF0 - floating input - water level alert - * PF1 - push-pull - external alarm + * PF1 - open drain - ext. LED/laser * PA0..PA3 - ADC_IN0..3 - * PA4, PA6, PA7 - PWM outputs + * PA4 - open drain - onboard LED (always ON when board works) + * PB1, PA6, PA7 - Alt. F. - PWM outputs * Registers * MODER - input/output/alternate/analog (2 bit) * OTYPER - 0 pushpull, 1 opendrain @@ -120,67 +121,54 @@ static inline void adc_setup(){ static inline void gpio_setup(){ // Enable clocks to the GPIO subsystems RCC->AHBENR |= RCC_AHBENR_GPIOAEN | RCC_AHBENR_GPIOBEN | RCC_AHBENR_GPIOFEN; - GPIOA->MODER = - GPIO_MODER_MODER13_O | GPIO_MODER_MODER14_O | - GPIO_MODER_MODER4_AF | GPIO_MODER_MODER6_AF | - GPIO_MODER_MODER7_AF | + // PA6/7 - AF; PB1 - AF + GPIOA->MODER = GPIO_MODER_MODER6_AF | GPIO_MODER_MODER7_AF | GPIO_MODER_MODER0_AI | GPIO_MODER_MODER1_AI | - GPIO_MODER_MODER2_AI | GPIO_MODER_MODER3_AI; - RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; // enable syscfg clock for EXTI - GPIOA->OTYPER = 3 << 13; // 13/14 opendrain + GPIO_MODER_MODER2_AI | GPIO_MODER_MODER3_AI | + GPIO_MODER_MODER4_O; + GPIOA->OTYPER = GPIO_OTYPER_OT_4; + GPIOB->MODER = GPIO_MODER_MODER1_AF; + //RCC->APB2ENR |= RCC_APB2ENR_SYSCFGEN; // enable syscfg clock for EXTI + GPIOF->ODR = 1<<1; GPIOF->MODER = GPIO_MODER_MODER1_O; - // PB1 - interrupt input - /* (2) Select Port B for pin 1 external interrupt by writing 0001 in EXTI1*/ - /* (3) Configure the corresponding mask bit in the EXTI_IMR register */ - /* (4) Configure the Trigger Selection bits of the Interrupt line on rising edge*/ - /* (5) Configure the Trigger Selection bits of the Interrupt line on falling edge*/ - SYSCFG->EXTICR[0] = SYSCFG_EXTICR1_EXTI1_PB; /* (2) */ - EXTI->IMR = EXTI_IMR_MR1; /* (3) */ - //EXTI->RTSR = 0x0000; /* (4) */ - EXTI->FTSR = EXTI_FTSR_TR1; /* (5) */ - /* (6) Enable Interrupt on EXTI0_1 */ - /* (7) Set priority for EXTI0_1 */ - NVIC_EnableIRQ(EXTI0_1_IRQn); /* (6) */ - NVIC_SetPriority(EXTI0_1_IRQn, 3); /* (7) */ + GPIOF->OTYPER = GPIO_OTYPER_OT_1; // alternate functions: - // PA4 - TIM14_CH1 (AF4) - // PA6 - TIM16_CH1 (AF5), PA7 - TIM17_CH1 (AF5) - GPIOA->AFR[0] = (GPIOA->AFR[0] &~ (GPIO_AFRL_AFRL4 | GPIO_AFRL_AFRL6 | GPIO_AFRL_AFRL7)) \ - | (4 << (4 * 4)) | (5 << (6 * 4)) | (5 << (7 * 4)); + // PA6 - TIM3_CH1, PA7 - TIM3_CH2, PB1 - TIM3_CH4 (all - AF1) + GPIOA->AFR[0] = (GPIOA->AFR[0] &~ (GPIO_AFRL_AFRL6 | GPIO_AFRL_AFRL7)) \ + | (1 << (6 * 4)) | (1 << (7 * 4)); + GPIOB->AFR[0] = (GPIOB->AFR[0] &~ (GPIO_AFRL_AFRL1)) \ + | (1 << (1 * 4)) ; +} + +// change period of PWM +// MAX freq - 200Hz!!! +void setTIM3T(uint32_t T){ + if(T < 1000 || T > 65536) return; + TIM3->ARR = T - 1; + // step = ampl / freq(Hz) * 3 + sg90step = SG90_AMPL * T; + sg90step >>= 18; // /262144 } static inline void timers_setup(){ - // timer 14 ch1 - cooler PWM - // timer 16 ch1 - heater PWM - // timer 17 ch1 - pump PWM - RCC->APB1ENR |= RCC_APB1ENR_TIM3EN | RCC_APB1ENR_TIM14EN; // enable clocking for timers 3 and 14 - RCC->APB2ENR |= RCC_APB2ENR_TIM16EN | RCC_APB2ENR_TIM17EN; // & timers 16/17 - // PWM mode 1 (active -> inactive) - TIM14->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1; - TIM16->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1; - TIM17->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1; + // timer 3 ch1, 2, 4 PWM for three servos + RCC->APB1ENR |= RCC_APB1ENR_TIM3EN; + // PWM mode 1 (active -> inactive) on all three channels + TIM3->CCMR1 = TIM_CCMR1_OC1M_2 | TIM_CCMR1_OC1M_1 | + TIM_CCMR1_OC2M_2 | TIM_CCMR1_OC2M_1; + TIM3->CCMR2 = TIM_CCMR2_OC4M_2 | TIM_CCMR2_OC4M_1; // frequency - TIM14->PSC = 59; // 0.8MHz for 3kHz PWM - TIM16->PSC = 18749; // 2.56kHz for 10Hz PWM - TIM17->PSC = 5; // 8MHz for 31kHz PWM + TIM3->PSC = 47; // 1MHz -> 1us per tick // ARR for 8-bit PWM - TIM14->ARR = 254; - TIM16->ARR = 254; - TIM17->ARR = 254; - // start in OFF state - // TIM14->CCR1 = 0; and so on + TIM3->ARR = 19999; // 50Hz // enable main output - TIM14->BDTR |= TIM_BDTR_MOE; - TIM16->BDTR |= TIM_BDTR_MOE; - TIM17->BDTR |= TIM_BDTR_MOE; + TIM3->BDTR |= TIM_BDTR_MOE; // enable PWM output - TIM14->CCER = TIM_CCER_CC1E; - TIM16->CCER = TIM_CCER_CC1E; - TIM17->CCER = TIM_CCER_CC1E; - // enable timers - TIM14->CR1 |= TIM_CR1_CEN; - TIM16->CR1 |= TIM_CR1_CEN; - TIM17->CR1 |= TIM_CR1_CEN; + TIM3->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E | TIM_CCER_CC4E; + TIM3->DIER = TIM_DIER_UIE; //TIM_DIER_CC1IE | TIM_DIER_CC2IE | TIM_DIER_CC4IE; + // enable timer & ARR buffering + TIM3->CR1 |= TIM_CR1_CEN | TIM_CR1_ARPE; + NVIC_EnableIRQ(TIM3_IRQn); } void hw_setup(){ @@ -192,10 +180,79 @@ void hw_setup(){ iwdg_setup(); } +static uint32_t target_Val[3] = {SG90_MIDPULSE, SG90_MIDPULSE, SG90_MIDPULSE}; +static uint32_t target_Speed[3] = {SG90DEFSTEP, SG90DEFSTEP, SG90DEFSTEP}; +static uint8_t onpos[3] = {0,0,0}; +volatile uint32_t *addr[3] = {&TIM3->CCR1, &TIM3->CCR2, &TIM3->CCR4}; +int32_t getPWM(int nch){ + return *addr[nch]; +} + +// return current value +int32_t setPWM(int nch, uint32_t val, uint32_t speed){ + if(nch < 0 || nch > 2) return 0; + if(speed > 0){ + if(speed > SG90_STEP) speed = SG90_STEP; + target_Speed[nch] = speed; + } + uint8_t ch = 1; + if(val >= SG90_MINPULSE && val <= SG90_MAXPULSE) target_Val[nch] = val; + else if(val == 0) target_Val[nch] = SG90_MINPULSE; + else if(val == 1) target_Val[nch] = SG90_MAXPULSE; + else if(val == 2) target_Val[nch] = SG90_MIDPULSE; + else ch = 0; + if(ch){ + onpos[nch] = 0; + } + return *addr[nch]; +} + +uint8_t onposition(int nch){ + return onpos[nch]; +} + +static void chkPWM(int n){ + if(n < 0 || n > 2) return; + uint32_t cur = *addr[n], tg = target_Val[n]; + if(cur == tg){ + onpos[n] = 1; + return; + } + uint32_t diff = tg - cur; + int sign = 1; + if(cur > tg){ + diff = cur - tg; + sign = -1; + } + if(diff > target_Speed[n]) diff = target_Speed[n]; + *addr[n] = cur + sign*diff; +} + +void tim3_isr(){ + /* + if(TIM3->SR & TIM_SR_CC1IF){ // 1st channel + chkPWM(0); + } + if(TIM3->SR & TIM_SR_CC2IF){ // 2nd channel + chkPWM(1); + } + if(TIM3->SR & TIM_SR_CC4IF){ // 3rd channel + chkPWM(2); + }*/ + if(TIM3->SR & TIM_SR_UIF){ + chkPWM(0); + chkPWM(1); + chkPWM(2); + } + TIM3->SR = 0; +} + +/* void exti0_1_isr(){ if (EXTI->PR & EXTI_PR_PR1){ - EXTI->PR |= EXTI_PR_PR1; /* Clear the pending bit */ + EXTI->PR |= EXTI_PR_PR1; // Clear the pending bit ; } } +*/ diff --git a/F0-nolib/Servo/hardware.h b/F0-nolib/Servo/hardware.h index 6e1a37a..1a12013 100644 --- a/F0-nolib/Servo/hardware.h +++ b/F0-nolib/Servo/hardware.h @@ -20,59 +20,21 @@ #define HARDWARE_H #include "stm32f0.h" -// measure flow sensor data each 1 second -#define FLOW_RATE_MS (999) -// previous as string constant -#define FLOWRATESTR "1" - -// each TMEASURE_MS ms calculate temperatures & check them -#define TMEASURE_MS (1000) -// each TCHECK_MS ms check cooler state and regulate temperature -#define TCHECK_MS (10000) - -/* - temperature limits and tolerances - */ -// tolerance: +-1.5degrC -#define TEMP_TOLERANCE (15) -// dT tolerance: +-0.5degrC -#define DT_TOLERANCE (5) -// maximal heater temperature - 80degrC; normal - <60 -#define MAX_HEATER_T (800) -#define NORMAL_HEATER_T (600) -// maximal output temperature - 45degrC; minimal - 10 -#define MAX_OUTPUT_T (450) -#define MIN_OUTPUT_T (100) -// temperature working values: from 15 to 30degrC -#define OUTPUT_T_H (300) -#define OUTPUT_T_L (150) - -/* - other limits & tolerances -*/ -// minimal flow rate - 0.2l per minute -#define MIN_FLOW_RATE (20) -// normal flow rate -#define NORMAL_FLOW_RATE (30) -// minimal PWM values when motors should work -#define MIN_PUMP_PWM (90) -#define MIN_COOLER_PWM (90) - -// PWM setters and getters -#define SET_COOLER_PWM(N) do{TIM14->CCR1 = (uint32_t)N;}while(0) -#define GET_COOLER_PWM() (uint16_t)(TIM14->CCR1) -#define SET_HEATER_PWM(N) do{TIM16->CCR1 = (uint32_t)N;}while(0) -#define GET_HEATER_PWM() (uint16_t)(TIM16->CCR1) -#define SET_PUMP_PWM(N) do{TIM17->CCR1 = (uint32_t)N;}while(0) -#define GET_PUMP_PWM() (uint16_t)(TIM17->CCR1) - -// ext. alarm states -#define ALARM_ON() pin_set(GPIOF, 2) -#define ALARM_OFF() pin_clear(GPIOF, 2) -#define ALARM_STATE() pin_read(GPIOF, 2) +// minimal and maximal pulse length for SG90 +#define SG90_MINPULSE (700) +#define SG90_MAXPULSE (2100) +#define SG90_MIDPULSE ((SG90_MINPULSE+SG90_MAXPULSE)/2) +#define SG90_AMPL (SG90_MAXPULSE-SG90_MINPULSE) +#define SG90DEFSTEP (100) +#define SG90_STEP (sg90step) extern volatile uint32_t Tms; +extern uint32_t sg90step; void hw_setup(void); +int32_t setPWM(int nch, uint32_t val, uint32_t speed); +int32_t getPWM(int nch); +uint8_t onposition(int nch); +void setTIM3T(uint32_t T); #endif // HARDWARE_H diff --git a/F0-nolib/Servo/main.c b/F0-nolib/Servo/main.c index 63eebf4..00f1c6d 100644 --- a/F0-nolib/Servo/main.c +++ b/F0-nolib/Servo/main.c @@ -19,8 +19,8 @@ * MA 02110-1301, USA. */ #include "adc.h" +#include "effects.h" #include "hardware.h" -#include "mainloop.h" #include "protocol.h" #include "usart.h" #include // memcpy @@ -38,12 +38,12 @@ int main(void){ char *txt; hw_setup(); SysTick_Config(6000, 1); - SEND_BLK("Servos controller v0.1\n"); + SEND("Servos controller v0.1\n"); if(RCC->CSR & RCC_CSR_IWDGRSTF){ // watchdog reset occured - SEND_BLK("WDGRESET=1"); + SEND("WDGRESET=1\n"); } if(RCC->CSR & RCC_CSR_SFTRSTF){ // software reset occured - SEND_BLK("SOFTRESET=1"); + SEND("SOFTRESET=1\n"); } RCC->CSR |= RCC_CSR_RMVF; // remove reset flags while (1){ @@ -56,7 +56,7 @@ int main(void){ IWDG->KR = IWDG_REFRESH; } } - mainloop(); + proc_effect(); IWDG->KR = IWDG_REFRESH; usart1_sendbuf(); } diff --git a/F0-nolib/Servo/protocol.c b/F0-nolib/Servo/protocol.c index 52f8c85..335518f 100644 --- a/F0-nolib/Servo/protocol.c +++ b/F0-nolib/Servo/protocol.c @@ -15,81 +15,170 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see . */ +#include "adc.h" +#include "effects.h" #include "hardware.h" #include "protocol.h" #include "usart.h" -#include "adc.h" -#include "mainloop.h" + + extern uint8_t crit_error; #ifdef EBUG +static void putADC(int n){ + put_string("ADC"); + put_char('0' + n); + put_string(" value: "); + put_uint(getADCval(n)); + put_char('\n'); +// while(LINE_BUSY == usart1_sendbuf()); +} + /** * @brief debugging_proc - debugging functions * @param command - rest of cmd */ static void debugging_proc(const char *command){ - const char *ptr = command; + char ch; int i; - switch(*ptr++){ + switch(*command++){ case 'w': - usart1_send("Test watchdog", 0); + SEND("Test watchdog\n"); while(1){nop();} break; case 'A': // raw ADC values depending on next symbol - i = *ptr++ - '0'; - if(i < 0 || i > NUMBER_OF_ADC_CHANNELS){ - usart1_send("Wrong channel nuber!", 0); - return; + ch = *command; + if(ch == 'A' || ch == 'B'){ + for(i = 0; i < NUMBER_OF_ADC_CHANNELS; ++i){ + putADC(i); + } + }else{ + i = ch - '0'; + if(i < 0 || i >= NUMBER_OF_ADC_CHANNELS){ + SEND("Wrong channel nuber!\n"); + }else putADC(i); } - put_string("ADC value: "); - put_uint(getADCval(i)); - put_string(", "); - usart1_sendbuf(); + break; + case 'B': + SEND("B command\n"); break; default: + SEND("wrong command\n"); break; } + while(LINE_BUSY == usart1_sendbuf()){nop();}; } #endif +static void chPWM(const char *command){ + int n = *command++ - '1'; + if(n < 0 || n > 2){ + put_string("Wrong PWM channel number"); + return; + } + int32_t CCR = getPWM(n); + if((command = getnum(command, &CCR))){ + set_effect(n, EFF_NONE); + int32_t speed = SG90_STEP; + if(*command++ == ','){ + getnum(command, &speed); + } + CCR = setPWM(n, CCR, speed); + } + put_string("pulse"); + put_char('1' + n); + put_char('='); + put_int(CCR); + put_char('\n'); +} + +static void set_servoT(const char *buf){ + int32_t T; + if(!getnum(buf, &T) || T < 1000 || T > 65535){ + put_string("Bad period value\n"); + usart1_sendbuf(); + return; + } + put_string("Set period to "); + put_int(T); + put_string(" us\n"); + setTIM3T(T); + usart1_sendbuf(); +} + +static void chk_effect(const char *cmd, effect_t eff, const char *name){ + if(set_effect(*(++cmd)-'1', eff) == eff){ + put_string("Turn on "); + put_string(name); + put_string(" effect\n"); + }else put_string("err\n"); +} + /** * @brief process_command - command parser * @param command - command text (all inside [] without spaces) * @return text to send over terminal or NULL */ char *process_command(const char *command){ - const char *ptr = command; char *ret = NULL; usart1_sendbuf(); // send buffer (if it is already filled) - switch(*ptr++){ + switch(*command){ case '?': // help SEND_BLK( + "1-3[pos[,speed]]- set/get xth pulse length (us) (0,1,2 - min, max, mid)\n" + "fx - servo period (us)" + "Mn - set Mad Wipe effect\n" + "Pn - set Pendulum effect\n" "R - reset\n" + "Sn - set Small Pendulum effect\n" "t - get MCU temperature (approx.)\n" - "V - get Vdd" + "V - get Vdd\n" + "Wn - set Wipe effect\n" ); #ifdef EBUG SEND_BLK("d -> goto debug:\n" - "\tAx - get raw ADCx value\n" - "\tw - test watchdog" + "\tAx - get raw ADCx value (A for all)\n" + "\tw - test watchdog\n" ); #endif break; + case '1': + case '2': + case '3': + chPWM(command); + break; + case 'f': + set_servoT(++command); + break; + case 'M': + chk_effect(command, EFF_MADWIPE, "mad wipe"); + break; + case 'P': + chk_effect(command, EFF_PENDULUM, "pendulum"); + break; case 'R': // reset MCU NVIC_SystemReset(); break; + case 'S': + chk_effect(command, EFF_SMPENDULUM, "small pendulum"); + break; case 't': // get mcu T put_string("MCUTEMP10="); put_int(getMCUtemp()); + put_char('\n'); break; case 'V': // get Vdd put_string("VDD100="); put_uint(getVdd()); + put_char('\n'); + break; + case 'W': + chk_effect(command, EFF_WIPE, "wipe"); break; #ifdef EBUG case 'd': - debugging_proc(ptr); + debugging_proc(++command); return NULL; break; #endif diff --git a/F0-nolib/Servo/servo.bin b/F0-nolib/Servo/servo.bin index c00bb1314f6bc51966316328547b0c56edbd6637..1c373a494426dd73ee9444400236f598414aecd8 100755 GIT binary patch delta 3996 zcmb7H3s4(%8vp*A03py48Ym%9x=El0HfRv4Sn3s$n!+yO-DwN0Zh*84<&ondxjQwq zS8vYR8C%o2qtuyJ=Nun*PLUpW=O`ZDWA1uZXQR_fUk;~rw&$xks|Fz@_w8CvM!nHv z=C}Xv{eAy$zy1HiY5K7u9q535P6wK25KkfwrDg>{X5hx3Ocy48sgw=A3#R4YZc~`> z?ecHN3xH-v@;8lj6KY7Tgs}cP4R-s~ID|im#v`AEJcLVQ5KgZNup1MOVe%qn8;?Zqq){G)5eUB*)zSe7|11jOol&5vG2dhx z8jm7hjXOMxY6uTSuS$2<+~dBd8fI*K%w)%~_bd?Wthl{qo4Z+&Rw>dZMLI2QVJQ<0 zv4}tRhP0VgDRf`#wDiYowU0Eg4cvKNrWT}nR&B!ku}FLDWvLbu-;IgaU}8Z@Toyek zu`E&C8)MH&ZGH!HyEw#b7Au$y;$1cC++|Agzl=li6h8rp4A z5^0r0;&plNr={b56;zZFX7xFHX`D6loV8}9J43NcSL|L=?EWBGYIN?@C54VD@+sGp zD=NhzuLTyKk|37`QWA^&Z6V(FMd+C1O9M?Z0R7=g&{X1J9mn|qJCr~BEWmM;(I~Sa z)G!wZAHhs1(oKk5#6g518jEE_0W>m=_!NEi;o61Rj#$pwC-%XrZOgcPA&|B0h(jkP zO%rpAQCkVyMjUX?0D5pbTMP_upCsm2jSY@}OT?_^j7^_QfQ4lvbcpqc1ny6U1a8!! znNHvwIpI4l*^kUroEhR!YJ&i)H;oPbGbm3H}ggz zP)U^=aeY8pn}z#vO`)6l^PPL0=Ua`te=N(}z&y$~@80ihZX05CMX7wVaBuJ`DGTtd zwy04^D%UXcp#!G6j#Db)JV@)-sPpPQo~o%hPc`8^RW9Db$Cq)QW&F?cIG%KN$0bsU z+I%Nz4Vx^TwUrM$i=CF%VauV8qmbkzrl6;BcapQQjTALb{#EJU_%DJD{3OdJU z;;?x=_b53hkUXt3!E~Nq$J(k#Tbnzy&SuY)`S#!{RYu^}s$%vKUtAU9Z!^{NzcuNE zQ$U=le5#O-Pqk@O&^kwX%KUDJ-ubSlfGyxgJ^Z@!o?FbLtycDY>#%uS*9uDO++%8P z&tr|Pm$21x|A);Mu7NW05Pp4*GPQNCa(Y|aIx`(nF^A3J^!%5oPvwLBn*uP=P&NvE zlT)tXhbmLj;=20@+?w4zm@o4_-OSvfrdP!K1kRp$za9XZo_Hm&YNuAaRN`wdUvl%=w%bzn+m zLrtk%7W(TZAwOOu`O8EqkpEdoi{@9Q6DAG&5Tw? zV)78n11u*m(Et$6uw5=jmT5TML{`Uz1kD+sW09K|2tJtw3ZK>mFcZ$`!L&Hg;fYPI z*Iix?az|Xl6M^>HcDJ|Ymt#%07w@2$4l(1Lin&8nGuqG^^Bq8*hVW!mJ{kWJ11pF> znjt0XwJO_{P{vsDO9x#?T;WN=d>WE1UO2pfm6*VEJxunmuF)vvLVLd37O?j^|u(RTrJ~CzHQl?#eC-1p|iz$AM1LnOz(SA zl8frZc*jMDixE}`nZ*nrbdYnImH@-c%Cs1m^SD=1e(I>NC=k9p+Eyrqf!HrSBz3ql zl``o6QATuua~Z!@#xhaQfVfNgP$}diEX1P}^7aCO2P8_7>I1$l3JDh0rT5j}c;91q z;@R+>ZbS^l0c^mv9AQONAo3AlQCX>Jipy+D0t5Iq$+i@$7f3NqJI+KU^1g|^W3h;E zead8K?Oai1Bd_XAjL)L7v`J1?D_9C^0zqDd4jFjWI$6>6Hv=Y?}%y(W^8SmgLTIS5uN^pQkH(8L7cGStn6-jj4Q zFw>iXvybqJRdHS z5Ji6>#Z|^4f5G9xuYmtV8o={J_P68eo6x%Bv49^iiI+6h)C^e(nZKm7qyp$b5qy~ zgUYXe@Xd~FJcWsUk0TN)H*2e2?J&x`h4F#K0CuI}YLOH7vG_y=BWYR=?McKviL|evmSGHa}3{MyuVAMeD?@PP0=S3)_LQs$- zHaN6+@$b(5JOp{feo>;<{tNY2LqK1{!cW?@YC`-jEkB#p3`0>%vp0C~MM_HiX z+jnm-?eq3^^tLPZ?j96lMB43Xr?+(Yyo)u|Eh_4~J?(>CgWZdY1{yv3yy&BCT(oKI z_H=d8U$-2PE#*qx&dv0Ix7+6pcBiabsok>ZwlrVx;go^^ZLOep_V&>jEnk~Bgzf|0 z{sDT(Gtefq_wJG#qU;z~2iALIANP8?2EDSb5p&&;4fN93&%u7Bhuvs9gnL61@ZH#s zO!q5ge8c;o@I-`oIa@8)iSG`9)s(xPm zSM^)!E9!_kE-sac%f^cdARfb}z)Hx$p9dx2fCliwPv9^-4{tyOOr(xHNP;VMxiwm7 ptaUXt+|_tj)15`irc3bGNGYNm(T<^(!jHihK?p!5>KB15{0DlU%JBdI delta 1864 zcma)6TW}Lq82-=Z(sTo5Yk)Src{WQi&4QB-$S_bO?P|#lu`R6-l-X?B=_V}@Zqf&x zR2;P61?rA7m_aFT3>VInP*8jhD z3T&@d{d*EFZ3f6cRT9PqM33Hlb*osd_lherpWN7SdUf!=(41_vZs%IVn|O-KgA2rc zQy0P=l`K)oApdv?C^tPQ*6K&ZLZ-?V}r8%WxR@2r;*=-jX39&G(HZ{J)nAa^^1i@FV3S7!OS}{Ktl4OjFUuo=7Ozd7 z*qzvJ@y$?R+|opkGcoM9`Gx~Taw_Az7C#A2oTYDMr(clzVhpc@Oj)w|e8&eMGJ4lY zZC6nzuDzkND%-{AXVppRtUNA60*%&FszZ7~9=Ic~s86ZjDbDwXdbLhVV=5GSg?y)y zR#vLw=j1?eL-?E$iL+89I^*gN++((>jKs<&UWt1oB|4|F(wy8G=n78CU4e|ef$xz& z>VAX2SdtID(uqnSU4x^wa~!nODwa7aH#XM{K7^Nd4!EBZiSLuf;AB6=n|6>Z%( zu}@j=KciUalycL1S<#iN$~b*w6d0@Y$l$=JRT>!FJL;8o4K7tyD9wsZbxSr}##p35 z_24N&{@I%hA4@b#LrN^+>{`(Ipey8%@1RH47!}TA%U~pm_VhkAjDYZ%Y9Cyx^Oi1?cYwILH9J^9}&xGKY4@4bSn3 zo+Y=ng=Gkt!Lr#fSDJ&V%(E8fGi>(-pP}*?DA}!uAS#sKSm5D}#H?J&C&Q`28t$;rqdo zC1)Tn7F*Gyf#PbJybbyLOW?ktlc{pyt3~W}GnwCVWC|5La4DyC~gOIjL|4xOy#wsSi~)diO%Q6{hvt5&?l3Y+`M z`cYX_*C7kEIBgmOv80prrYAtW{&y^7e#{YcsRI_OHmrOFc_oCu`9% zihj}+i0cUBldvI{ATA)5BkB<=5pQVCHTM#tZK)9hMsDcdKo}Y>T@`8=BLa9Bovhp1 zMt-OV4tMwUq6u!|YZyLw)62IL!7~P)Ivv{TboPhwRh;&Ijl<3gKAsIaPF#sQwJ&QL zYqB{IZd8nijl_59(Pd^ryjnkZ+Bv*%J7*le@!rO}`tjf|pwH$@i`-#TbuXoB>7k*+ zL)xyD?F6*fS9(^k)yI%LGIaO|Jr+F@QwI+W18CiKLK{x<*r5aahiOcW?%zK&O7|aC rh7R#aL+3*heKI= UARTBUFSZ - 1){ - if(ALL_OK != usart1_sendbuf()) return 1; + for(int i = 0; i < 72000000 && ALL_OK != usart1_sendbuf(); ++i) + if(i == 72000000) return 1; } trbuf[trbufidx++] = c; return 0; @@ -84,10 +85,11 @@ int put_uint(uint32_t N){ */ TXstatus usart1_sendbuf(){ int len = trbufidx; - trbufidx = 0; if(len == 0) return ALL_OK; else if(len > UARTBUFSZ) len = UARTBUFSZ; - return usart1_send(trbuf, len); + TXstatus s = usart1_send(trbuf, len); + if(s == ALL_OK) trbufidx = 0; + return s; } void USART1_config(){ @@ -125,8 +127,14 @@ void USART1_config(){ NVIC_EnableIRQ(USART1_IRQn); /* (4) */ } +#ifdef EBUG +#define TMO 0 +#else +#define TMO 1 +#endif + void usart1_isr(){ - static uint8_t timeout = 1 // == 0 for human interface without timeout + static uint8_t timeout = TMO // == 0 for human interface without timeout ,nctr = 0 // counter of '#' received ,incmd = 0 // ==1 - inside command ; @@ -235,7 +243,7 @@ TXstatus usart1_send(const char *str, int len){ if(len == 0) return ALL_OK; DMA1_Channel2->CCR &= ~DMA_CCR_EN; memcpy(tbuf, str, len); - tbuf[len++] = '\n'; +// tbuf[len++] = '\n'; DMA1_Channel2->CNDTR = len; DMA1_Channel2->CCR |= DMA_CCR_EN; // start transmission return ALL_OK; @@ -252,7 +260,7 @@ TXstatus usart1_send_blocking(const char *str, int len){ USART1->TDR = *str++; while(!(USART1->ISR & USART_ISR_TXE)); } - USART1->TDR = '\n'; +// USART1->TDR = '\n'; while(!(USART1->ISR & USART_ISR_TC)); txrdy = 1; return ALL_OK; diff --git a/F0-nolib/Servo/usart.h b/F0-nolib/Servo/usart.h index 9b7f8dd..55fd101 100644 --- a/F0-nolib/Servo/usart.h +++ b/F0-nolib/Servo/usart.h @@ -38,8 +38,8 @@ typedef enum{ #define usart1ovr() (bufovr) // send constant string -#define SEND_BLK(x) do{while(LINE_BUSY == usart1_send_blocking(x, sizeof(x)-1));}while(0) -#define SEND(x) do{while(LINE_BUSY == usart1_send(x, sizeof(x)-1));}while(0) +#define SEND_BLK(x) do{while(LINE_BUSY == usart1_send_blocking(x, sizeof(x)-1)) IWDG->KR = IWDG_REFRESH;}while(0) +#define SEND(x) do{while(LINE_BUSY == usart1_send(x, sizeof(x)-1)) IWDG->KR = IWDG_REFRESH;}while(0) extern uint8_t bufovr;