LailaTheElf/rcrf-wifi
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add options for servo controll

Browse Source
This commit is contained in:
Laila van Reenen 2024-08-11 11:14:38 +02:00
parent 259d4b91d8
commit a846058098
Signed by: LailaTheElf
GPG Key ID: 1F4E6EE3E6DDF769
7 changed files with 293 additions and 34 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
plot.sh Executable file
View File

@ -0,0 +1,24 @@
#!/bin/bash
function sample() {
sigrok-cli -d fx2lafw:conn=1.13 --config 'samplerate=1 MHz' -C D5,D7 --time 400ms -P servo:data=D5 -P servo:data=D7
}
samples=$(sample | head -n2 | sed -e 's/servo-[12]: //')
cha=$(echo "$samples" | head -n1)
cha_c=$(( ( `echo $cha | tr -d '.'` - 1000 ) / 10 ))
chb=$(echo "$samples" | tail -n1)
chb_c=$(( ( `echo $chb | tr -d '.'` - 1000 ) / 10 ))
echo -n " |"
printf "%0.s " $(seq 1 49)
echo -n "|"
printf "%0.s " $(seq 1 49)
echo "|"
echo -n "A: $cha "
printf "%0.s#" $(seq 1 $cha_c)
echo
echo -n "B: $chb "
printf "%0.s#" $(seq 1 $chb_c)
echo

24
rx_esp32/servo/__init__.py Normal file
View File

@ -0,0 +1,24 @@
##
## This file is part of the libsigrokdecode project.
##
## Copyright (C) 2014 Torsten Duwe <duwe@suse.de>
##
## 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 2 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/>.
##
'''
Servo decoder
'''
from .pd import Decoder

141
rx_esp32/servo/pd.py Normal file
View File

@ -0,0 +1,141 @@
##
## This file is base of the timeing decoder from the libsigrokdecode project made by
## Torsten Duwe <duwe@suse.de> and Sebastien Bourdelin <sebastien.bourdelin@savoirfairelinux.com>
## and converted to a Servo decoder by FReenen <git@finnvanreenen.nl>
##
## 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 2 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/>.
##
import sigrokdecode as srd
class SamplerateError(Exception):
pass
class Decoder(srd.Decoder):
api_version = 3
id = 'servo'
name = 'Servo'
longname = 'Servo PWM signal (1 to 2 ms pulses every 20 ms)'
desc = '1 to 2 ms pulses every 20 ms'
license = 'gplv2+'
inputs = ['logic']
outputs = []
tags = ['Encoding']
channels = (
{'id': 'data', 'name': 'Data', 'desc': 'Data line'},
)
options = (
{'id': 'polarity', 'desc': 'Polarity', 'default': 'active-high',
'values': ('active-low', 'active-high')},
)
annotations = (
('duty-cycle', 'Duty cycle'),
('period', 'Period'),
)
annotation_rows = (
('duty-cycle', 'Duty cycle', (0,)),
('period', 'Period', (1,)),
)
# binary = (
# ('raw', 'RAW file'),
# )
def __init__(self):
self.reset()
def reset(self):
self.samplerate = None
self.ss_block = self.es_block = None
def metadata(self, key, value):
if key == srd.SRD_CONF_SAMPLERATE:
self.samplerate = value
def start(self):
self.out_ann = self.register(srd.OUTPUT_ANN)
self.out_binary = self.register(srd.OUTPUT_BINARY)
self.out_average = \
self.register(srd.OUTPUT_META,
meta=(float, 'Average', 'PWM base (cycle) frequency'))
def putx(self, data):
self.put(self.ss_block, self.es_block, self.out_ann, data)
def putp(self, period_t):
# Adjust granularity.
# if period_t == 0 or period_t >= 1:
# period_s = '%.1f s' % (period_t)
# elif period_t <= 1e-12:
# period_s = '%.1f fs' % (period_t * 1e15)
# elif period_t <= 1e-9:
# period_s = '%.1f ps' % (period_t * 1e12)
# elif period_t <= 1e-6:
# period_s = '%.1f ns' % (period_t * 1e9)
# elif period_t <= 1e-3:
# period_s = '%.1f μs' % (period_t * 1e6)
# else:
# period_s = '%.1f ms' % (period_t * 1e3)
period_s = '%.3f' % (period_t * 1e3)
self.put(self.ss_block, self.es_block, self.out_ann, [1, [period_s]])
def putb(self, data):
self.put(self.ss_block, self.es_block, self.out_binary, data)
def decode(self):
if not self.samplerate:
raise SamplerateError('Cannot decode without samplerate.')
num_cycles = 0
average = 0
# Wait for an "active" edge (depends on config). This starts
# the first full period of the inspected signal waveform.
self.wait({0: 'f' if self.options['polarity'] == 'active-low' else 'r'})
self.first_samplenum = self.samplenum
# Keep getting samples for the period's middle and terminal edges.
# At the same time that last sample starts the next period.
while True:
# Get the next two edges. Setup some variables that get
# referenced in the calculation and in put() routines.
start_samplenum = self.samplenum
self.wait({0: 'f'})
end_samplenum = self.samplenum
self.wait({0: 'r'})
self.ss_block = start_samplenum
self.es_block = self.samplenum
pulse = float((end_samplenum - start_samplenum) / self.samplerate)
ratio = (pulse - 0.0015) / 0.0005
# Report the duty cycle in percent.
# percent = float(ratio * 100)
# self.putx([0, ['%f%%' % percent]])
# Report the duty cycle in the binary output.
# self.putb([0, bytes([int(ratio * 128)])])
# Report the period in units of time.
# print("servo pulse: " + str(pulse))
self.putp(pulse)
# Update and report the new duty cycle average.
# num_cycles += 1
# average += percent
# self.put(self.first_samplenum, self.es_block, self.out_average,
# float(average / num_cycles))

9
rx_esp32/src/config.h
View File

@ -12,13 +12,8 @@ static uint8_t BoatId = 1;
#define HISTORY // enable cli history
#define SERVOS_CH0 {LEDC_CHANNEL_0, 7},
#define SERVOS_CH1 {LEDC_CHANNEL_1, 21}
#define SERVOS_CH2
#define SERVOS_CH3
#define SERVOS_CH4
#define SERVOS_CH5
#define SERVOS_CH6
#define SERVOS_CH0_PIN 7
#define SERVOS_CH1_PIN 21
#define WS_RX_BUFFER_LEN 1024
#define WS_TX_BUFFER_LEN 128

110
rx_esp32/src/servos.c
View File

@ -14,27 +14,33 @@
#define SERVO_DUTY_MIN ( 410) // 2**13 * (1000/20000) // 1000us of the 20ms
#define SERVO_DUTY_MAX ( 819) // 2**13 * (2000/20000) // 2000us of the 20ms
// #define SERVO_DUTY_MAX (1024) // 2**13 * (2500/20000) // 2500us of the 20ms
#define SERVO_DUTY_DEFUALT ( 614) // 2**13 * (1500/20000) // 2500us of the 20ms
#define SERVO_DUTY_MID ( 614) // 2**13 * (1500/20000) // 2500us of the 20ms
#define SERVO_DUTY_DIFF (SERVO_DUTY_MAX - SERVO_DUTY_MIN)
typedef struct ServoCh_t {
ledc_channel_t channel;
int gpio;
} ServoCh_t;
static ServoCh_t Server_chs[] = {
SERVOS_CH0
SERVOS_CH1
SERVOS_CH2
SERVOS_CH3
SERVOS_CH4
SERVOS_CH5
SERVOS_CH6
};
servo_config_t Servos[2];
bool Servos_ch_swap = false;
void servo_init(void)
{
Servos[0].pin = SERVOS_CH0_PIN;
Servos[0].ledc_ch = LEDC_CHANNEL_0;
Servos[0].pulse_min = SERVO_DUTY_MIN;
Servos[0].pulse_mid = SERVO_DUTY_MID;
Servos[0].pulse_max = SERVO_DUTY_MAX - 100;
Servos[0].mid_delay = 10;
Servos[0].mid_delay_timer = 0;
Servos[0].reversed = false;
Servos[1].pin = SERVOS_CH1_PIN;
Servos[1].ledc_ch = LEDC_CHANNEL_1;
Servos[1].pulse_min = SERVO_DUTY_MIN;
Servos[1].pulse_mid = SERVO_DUTY_MID;
Servos[1].pulse_max = SERVO_DUTY_MAX;
Servos[1].mid_delay = 0;
Servos[1].mid_delay_timer = 0;
Servos[1].reversed = false;
// Prepare and then apply the LEDC PWM timer configuration
ledc_timer_config_t ledc_timer = {
.speed_mode = SERVO_LEDC_MODE,
@ -45,35 +51,87 @@ void servo_init(void)
};
ledc_timer_config(&ledc_timer);
for (int i = sizeof(Server_chs)/sizeof(ServoCh_t)-1; i >= 0; i--)
for (int i = sizeof(Servos)/sizeof(servo_config_t)-1; i >= 0; i--)
{
// Prepare and then apply the LEDC PWM channel configuration
ledc_channel_config_t ledc_channel = {
.speed_mode = SERVO_LEDC_MODE,
.channel = Server_chs[i].channel,
.channel = Servos[i].ledc_ch,
.timer_sel = SERVO_LEDC_TIMER,
.intr_type = LEDC_INTR_DISABLE,
.gpio_num = Server_chs[i].gpio,
.duty = SERVO_DUTY_DEFUALT, // Set duty to 0%
.gpio_num = Servos[i].pin,
.duty = Servos[i].pulse_mid,
.hpoint = 0
};
ledc_channel_config(&ledc_channel);
LOG_D("servo_init: inited servo channel %d (ledc: %u, gpio: %d)", i, Server_chs[i].channel, Server_chs[i].gpio);
LOG_D("servo_init: inited servo channel %d (ledc: %u, gpio: %d)", i, Servos[i].ledc_ch, Servos[i].pin);
}
}
void servo_deinit(void)
{
ledc_timer_pause(SERVO_LEDC_MODE, SERVO_LEDC_TIMER);
for (int i = sizeof(Server_chs)/sizeof(ServoCh_t)-1; i > 0; i--)
for (int i = sizeof(Servos)/sizeof(servo_config_t)-1; i >= 0; i--)
{
ledc_stop(SERVO_LEDC_MODE, Server_chs[i].channel, 0);
ledc_stop(SERVO_LEDC_MODE, Servos[i].ledc_ch, 0);
}
}
void servo_set(uint8_t ch, uint8_t pos)
{
uint32_t duty = (uint32_t) ((double)pos * (double)SERVO_DUTY_DIFF/(double)UINT8_MAX);
duty += (SERVO_DUTY_MIN + (SERVO_DUTY_DIFF/2));
if (Servos_ch_swap)
{
if (ch == 0)
{
ch = 1;
}
else
{
ch = 0;
}
}
if (Servos[ch].reversed)
{
pos = -((int16_t)pos) + 255;
}
double pos_double = ((double) pos) - 128;
if (pos_double < 0)
{
if (Servos[ch].mid_delay_timer > 0)
{
Servos[ch].mid_delay_timer--;
pos_double = 0.0;
}
else
{
Servos[ch].mid_delay_timer = -Servos[ch].mid_delay;
}
}
else if (pos_double > 0)
{
if (Servos[ch].mid_delay_timer < 0)
{
Servos[ch].mid_delay_timer++;
pos_double = 0.0;
}
else
{
Servos[ch].mid_delay_timer = Servos[ch].mid_delay;
}
}
uint32_t duty;
if (pos_double > 0.0)
{
duty = (pos_double/128.0 * (Servos[ch].pulse_max - Servos[ch].pulse_mid)) + Servos[ch].pulse_mid;
}
else
{
duty = (pos_double/128.0 * (Servos[ch].pulse_mid - Servos[ch].pulse_min)) + Servos[ch].pulse_mid;
}
if (duty < SERVO_DUTY_MIN)
{
duty = SERVO_DUTY_MIN;
@ -82,6 +140,6 @@ void servo_set(uint8_t ch, uint8_t pos)
{
duty = SERVO_DUTY_MAX;
}
ledc_set_duty(SERVO_LEDC_MODE, Server_chs[ch].channel, duty);
ledc_update_duty(SERVO_LEDC_MODE, Server_chs[ch].channel);
ledc_set_duty(SERVO_LEDC_MODE, Servos[ch].ledc_ch, duty);
ledc_update_duty(SERVO_LEDC_MODE, Servos[ch].ledc_ch);
}

17
rx_esp32/src/servos.h
View File

@ -2,6 +2,23 @@
#define SERVOS_H
#include <stdint.h>
#include <stdbool.h>
#include <driver/ledc.h>
typedef struct {
int pin;
ledc_channel_t ledc_ch;
uint16_t pulse_min;
uint16_t pulse_mid;
uint16_t pulse_max;
uint8_t mid_delay;
int16_t mid_delay_timer;
bool reversed;
} servo_config_t;
extern servo_config_t Servos[2];
extern bool Servos_ch_swap;
void servo_init(void);
void servo_deinit(void);

2
rx_esp32/src/ws.c
View File

@ -166,7 +166,7 @@ void ws_sendData(ws_client_t client)
{
if ((client->txBuffer_wp > 0) && esp_websocket_client_is_connected(client->handle))
{
LOG_D("ws_sendData: (%d b) '%.*s'", client->txBuffer_wp, client->txBuffer_wp, (char *)&client->txBuffer[0]);
// LOG_D("ws_sendData: (%d b) '%.*s'", client->txBuffer_wp, client->txBuffer_wp, (char *)&client->txBuffer[0]);
esp_websocket_client_send_text(client->handle, (char *)&client->txBuffer[0], client->txBuffer_wp, 1000 * portTICK_PERIOD_MS);
client->txBuffer_wp = 0;
}