mirror of
https://github.com/eddyem/stm32samples.git
synced 2025-12-06 10:45:11 +03:00
112 lines
3.3 KiB
C
112 lines
3.3 KiB
C
/*
|
|
* This file is part of the Servo project.
|
|
* Copyright 2019 Edward Emelianov <eddy@sao.ru>.
|
|
*
|
|
* 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 "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;
|
|
}
|