Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
blaar
raspinobo
Commits
6e124a1b
Commit
6e124a1b
authored
Jul 14, 2019
by
ARnaud Blanchard
Browse files
Nig improvement but not yet working
parent
6c573609
Changes
5
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
6e124a1b
...
...
@@ -22,7 +22,7 @@ add_executable(raspinobo raspinobo.cpp bl_raspi.cpp)
target_link_libraries
(
raspinobo
${
BL_LIBRARIES
}
)
if
(
NOT APPLE
)
#Give the raw capbility to the target /dev/mem
#Give the raw cap
a
bility to the target /dev/mem
add_custom_command
(
TARGET raspinobo POST_BUILD COMMAND sudo setcap
"cap_dac_override+ep cap_sys_rawio+ep cap_sys_nice=eip"
$<TARGET_FILE:raspinobo>
)
endif
()
README.md
View file @
6e124a1b
...
...
@@ -4,6 +4,15 @@ Setup the raspberry has an access point
More details here in the original tuatorial ( https://github.com/SurferTim/documentation/blob/6bc583965254fa292a470990c40b145f553f6b34/configuration/wireless/access-point.md )
Check the frequency:
`sudo cat /sys/devices/system/cpu/cpufreq/policy0/cpuinfo_cur_freq`
Check temperature:
`/opt/vc/bin/vcgencmd measure_temp`
Video tracking
...
...
bl_raspi.cpp
View file @
6e124a1b
...
...
@@ -15,15 +15,23 @@
#define GPIO_MAX 32
#define FLAG(pos) (1<<pos)
typedef
struct
typedef
struct
Ohm_input
{
uint32_t
us_timestamps
;
uint32_t
flag
;
uint32_t
tau
;
uint32_t
rest_ns
,
spike_ns
;
uint8_t
pin
;
uint8_t
activated
;
FILE
*
file
;
}
ohm_input_type
;
uint32_t
activated
;
uint32_t
*
buffer
;
uint32_t
previous_iteration
;
int32_t
index
;
int64_t
stop_ns
;
ohm_input_type
();
inline
void
rest
();
inline
void
spike
();
}
Ohm_input
;
typedef
struct
{
...
...
@@ -38,10 +46,10 @@ typedef struct
}
volt_output_type
;
typedef
struct
bl_raspi
{
static
const
unsigned
int
system_timers_base_addr
=
0x3F003000
;
static
const
unsigned
int
gpio_base_addr
=
0x3F200000
;
// gpio registers base address for raspberry 2 et 3 0x20200000 sinon
//Three pulling resistor (~50KOhm modes in input direction
static
const
unsigned
int
PULL_OFF
=
0
;
static
const
unsigned
int
PULL_DOWN
=
1
;
...
...
@@ -52,58 +60,103 @@ typedef struct bl_raspi{
// two possible states for output pins
static
const
unsigned
int
LOW
=
0
;
static
const
unsigned
int
HIGH
=
1
;
volatile
uint32_t
*
system_timers_map
,
*
gpio_map
;
volatile
uint32_t
*
ptime_us
;
volatile
uint32_t
*
gpfsels
,
*
gpfset0
,
*
gpclr0
,
*
gplev0
,
*
gppud
,
*
gppudclk0
;
//1 could be used if more than 32 pins
bl_raspi
();
~
bl_raspi
();
void
set_pin_dir
(
int
pin
,
int
dir
);
//input or output
void
set_pin_mode
(
int
pin
,
int
mode
);
//pulling resistance
}
bl_raspi
;
int
bl_raspi_RC_inputs_nb
=
0
;
int
bl_raspi_volt_outputs_nb
=
0
;
uint32_t
*
bl_raspi_RC_inputs
=
NULL
;
uchar
const
*
bl_raspi_volt_outputs
=
NULL
;
int
bl_raspi_iterations_nb
=
0
;
static
const
int
CHUNK_ITERATIONS
=
128
;
uint32_t
it_since_clock
=
0
;
uint32_t
current_ns
,
previous_ns
;
float
ns_per_it
=
1
;
bl_raspi
raspi
;
Ohm_input
*
ohm_inputs
=
nullptr
;
static
int
run_loop
;
static
ohm_input_type
*
ohm_inputs
=
NULL
;
static
volt_output_type
*
volt_outputs
=
NULL
;
inline
void
update_time
(){
struct
timespec
timestamp
;
int32_t
current_ns_per_it
;
int32_t
delta_ns
;
int
i
;
clock_gettime
(
CLOCK_MONOTONIC
,
&
timestamp
);
current_ns
=
timestamp
.
tv_nsec
;
delta_ns
=
current_ns
-
previous_ns
;
ns_per_it
+=
(((
float
)
delta_ns
/
(
float
)
it_since_clock
)
-
ns_per_it
)
/
10.
f
;
previous_ns
=
current_ns
;
it_since_clock
=
0
;
FOR
(
i
,
bl_raspi_RC_inputs_nb
){
if
(
ohm_inputs
[
i
].
activated
){
if
(
ohm_inputs
[
i
].
stop_ns
-
current_ns
>
0
)
ohm_inputs
[
i
].
activated
=
(
float
)(
ohm_inputs
[
i
].
stop_ns
-
current_ns
)
/
ns_per_it
;
else
ohm_inputs
[
i
].
activated
=
1
;
}
}
}
ohm_input_type
::
ohm_input_type
()
:
index
(
0
){
}
static
bl_raspi
raspi
;
inline
void
ohm_input_type
::
rest
(){
raspi
.
set_pin_dir
(
pin
,
raspi
.
INPUT
);
update_time
();
buffer
[
index
]
=
rest_ns
-
spike_ns
;
index
++
;
if
(
index
==
buffer_size
)
index
=
0
;
rest_ns
=
current_ns
;
// fprintf(stderr, "%lld %f\n", current_ns-spike_ns, ns_per_it);
}
inline
void
ohm_input_type
::
spike
(){
raspi
.
set_pin_dir
(
pin
,
raspi
.
OUTPUT
);
tau
=
10000
;
update_time
();
spike_ns
=
current_ns
;
buffer
[
index
]
=
current_ns
-
rest_ns
;
index
++
;
if
(
index
==
buffer_size
)
index
=
0
;
stop_ns
=
current_ns
+
tau
*
1000
;
activated
=
(
float
)(
tau
*
1000
)
/
ns_per_it
;
}
volatile
uint32_t
*
map_memory
(
uint32_t
base_addr
,
size_t
size
){
int
mem_fd
;
volatile
uint32_t
*
mapped_addr
;
SYSTEM_ERROR_CHECK
(
mem_fd
=
open
(
"/dev/mem"
,
O_RDWR
|
O_SYNC
),
0
,
"Maybe you need to be sudo or use set_cap on the binary"
);
/* mmap IO */
SYSTEM_ERROR_CHECK
(
mapped_addr
=
(
volatile
unsigned
*
)
mmap
(
NULL
,
//Any adddress in our space will do
size
,
//Map length
PROT_READ
|
PROT_WRITE
|
PROT_EXEC
,
// Enable reading & writting to mapped memory
#ifdef MAP_LOCKED
#ifdef MAP_LOCKED
MAP_SHARED
|
MAP_LOCKED
,
//Shared with other processes
#else
#else
MAP_SHARED
,
// Darwin (OSX) misses MAP_LOCKED
#endif
#endif
mem_fd
,
//File to map
base_addr
//Offset to base address
),
MAP_FAILED
,
"Mapping physical memory. You are not on raspberry or you do not have the rights. ( use setcap or sudo)."
);
SYSTEM_SUCCESS_CHECK
(
close
(
mem_fd
),
0
,
"We cannot close fd_meme (i.e. we can close /dev/mem)"
);
return
mapped_addr
;
}
bl_raspi
::
bl_raspi
(){
//maps timers
system_timers_map
=
map_memory
(
system_timers_base_addr
,
0x1C
);
ptime_us
=
system_timers_map
+
1
;
//+1 is 4bytes because ptimes is 32(4*8)bits step
//map gpio
gpio_map
=
map_memory
(
gpio_base_addr
,
0xB4
);
gpfsels
=
gpio_map
;
...
...
@@ -139,14 +192,13 @@ void bl_raspi::set_pin_dir(int pinnum, int dir){
}
}
void
bl_raspi_add_RC_input
(
int
pin
,
int
us_tau
,
char
const
*
filename
){
void
bl_raspi_add_RC_input
(
int
pin
,
int
us_tau
,
uint32_t
*
buffer
){
ohm_input_type
input
;
input
.
flag
=
FLAG
(
pin
);
input
.
tau
=
us_tau
;
input
.
pin
=
pin
;
input
.
activated
=
1
;
if
(
filename
)
SYSTEM_ERROR_CHECK
(
input
.
file
=
fopen
(
filename
,
"w"
),
NULL
,
NULL
);
else
input
.
file
=
NULL
;
input
.
buffer
=
buffer
;
APPEND_ITEM
(
&
ohm_inputs
,
&
bl_raspi_RC_inputs_nb
,
&
input
);
}
...
...
@@ -154,30 +206,33 @@ void bl_raspi_add_voltage_output(int pin, char const *filename, char const* file
volt_output_type
output
;
output
.
flag
=
FLAG
(
pin
);
output
.
pin
=
pin
;
if
(
filename
)
SYSTEM_ERROR_CHECK
(
output
.
file
=
fopen
(
filename
,
"w"
),
NULL
,
NULL
);
if
(
filename_input
)
SYSTEM_ERROR_CHECK
(
output
.
file_input
=
fopen
(
filename_input
,
"w"
),
NULL
,
NULL
);
APPEND_ITEM
(
&
volt_outputs
,
&
bl_raspi_volt_outputs_nb
,
&
output
);
}
void
*
thread_loop
(
void
*
){
static
const
unsigned
int
coeff
=
5
;
static
const
unsigned
int
coeff
=
1
;
struct
timespec
timestamp
;
uint32_t
result
;
int
i
;
struct
timespec
delay_sleep
;
uint32_t
initial_us
,
relative_us
,
current_us
,
delta_us
;
delay_sleep
.
tv_sec
=
0
;
delay_sleep
.
tv_nsec
=
1000
;
uint32_t
initial_ns
,
delta_ns
;
int32_t
previous_ns
,
current_ns
;
int32_t
var_ns_per_iteration
;
int32_t
error
;
*
raspi
.
gpfset0
=
0
;
initial_us
=
3000000
*
(
int
)(
*
raspi
.
ptime_us
/
3000000
)
-
3000000
;
// initial_us=3000000*(int)(*raspi.ptime_us/3000000)-3000000;
update_time
();
FOR
(
i
,
bl_raspi_RC_inputs_nb
){
*
raspi
.
gpfset0
|=
ohm_inputs
[
i
].
flag
;
//Set pin high
raspi
.
set_pin_mode
(
ohm_inputs
[
i
].
pin
,
raspi
.
PULL_OFF
);
raspi
.
set_pin_dir
(
ohm_inputs
[
i
].
pin
,
raspi
.
OUTPUT
);
ohm_inputs
[
i
].
us_timestamps
=*
raspi
.
ptime_us
;
ohm_inputs
[
i
].
activated
=
1
;
ohm_inputs
[
i
].
stop_ns
=
current_ns
+
ohm_inputs
[
i
].
tau
*
1000
;
ohm_inputs
[
i
].
spike_ns
=
current_ns
;
ohm_inputs
[
i
].
rest_ns
=
current_ns
;
}
FOR
(
i
,
bl_raspi_volt_outputs_nb
)
{
...
...
@@ -188,49 +243,20 @@ void *thread_loop(void*){
}
while
(
run_loop
){
result
=*
(
raspi
.
gplev0
);
relative_us
=*
(
raspi
.
ptime_us
);
if
(
initial_us
>
current_us
)
current_us
=
UINT32_MAX
-
initial_us
+
relative_us
;
else
current_us
=
relative_us
-
initial_us
;
// Input RC
FOR
(
i
,
bl_raspi_RC_inputs_nb
){
if
(
ohm_inputs
[
i
].
activated
)
{
delta_us
=
current_us
-
ohm_inputs
[
i
].
us_timestamps
;
if
(
delta_us
>
coeff
*
ohm_inputs
[
i
].
tau
){
raspi
.
set_pin_dir
(
ohm_inputs
[
i
].
pin
,
raspi
.
INPUT
);
ohm_inputs
[
i
].
activated
=
0
;
if
(
ohm_inputs
[
i
].
file
)
{
fwrite
(
&
current_us
,
sizeof
(
uint32_t
),
1
,
ohm_inputs
[
i
].
file
);
fwrite
(
&
delta_us
,
sizeof
(
uint32_t
),
1
,
ohm_inputs
[
i
].
file
);
}
ohm_inputs
[
i
].
us_timestamps
=
current_us
;
// if (i==0) fprintf(stderr, "end %uld delta_ns %f\n", (*us_time), delta_ns);
ohm_inputs
[
i
].
activated
--
;
if
(
ohm_inputs
[
i
].
activated
==
0
){
ohm_inputs
[
i
].
rest
();
}
}
else
{
if
((
result
&
ohm_inputs
[
i
].
flag
)
==
0
){
raspi
.
set_pin_dir
(
ohm_inputs
[
i
].
pin
,
raspi
.
OUTPUT
);
delta_us
=
current_us
-
ohm_inputs
[
i
].
us_timestamps
;
bl_raspi_RC_inputs
[
i
]
=
delta_us
;
ohm_inputs
[
i
].
activated
=
1
;
// if (i==0) fprintf(stderr, "start %lu ", (*ptime));
// clock_gettime(CLOCK_MONOTONIC, &timer);
// current_ns=(timer.tv_sec-initial_s)*(1000000000.f)+timer.tv_nsec;
ohm_inputs
[
i
].
us_timestamps
=
current_us
;
if
(
ohm_inputs
[
i
].
file
){
fwrite
(
&
current_us
,
sizeof
(
uint32_t
),
1
,
ohm_inputs
[
i
].
file
);
fwrite
(
&
(
delta_us
),
sizeof
(
uint32_t
),
1
,
ohm_inputs
[
i
].
file
);
}
if
((
*
(
raspi
.
gplev0
)
&
ohm_inputs
[
i
].
flag
)
==
0
){
ohm_inputs
[
i
].
spike
();
}
}
}
//Output volts
FOR
(
i
,
bl_raspi_volt_outputs_nb
){
/*COmputing current ( not yet working)
...
...
@@ -242,9 +268,9 @@ void *thread_loop(void*){
}
volt_outputs[0].activated_input=1;
}*/
delta_
u
s
=
current_
u
s
-
volt_outputs
[
i
].
us_timestamps
;
if
(
delta_
u
s
>
10
){
volt_outputs
[
i
].
average
+=
(
volt_outputs
[
i
].
activated
-
volt_outputs
[
i
].
average
)
*
(
delta_
u
s
/
100
);
delta_
n
s
=
current_
n
s
-
volt_outputs
[
i
].
us_timestamps
;
if
(
delta_
n
s
>
10
){
volt_outputs
[
i
].
average
+=
(
volt_outputs
[
i
].
activated
-
volt_outputs
[
i
].
average
)
*
(
delta_
n
s
/
100
);
if
(
bl_raspi_volt_outputs
[
i
]
>
rand
()
/
(
RAND_MAX
/
255
)){
// volt_outputs[i].average*100){ //Comparing average time up with the requested one//){
raspi
.
set_pin_dir
(
volt_outputs
[
i
].
pin
,
raspi
.
OUTPUT
);
volt_outputs
[
i
].
activated
=
1
;
...
...
@@ -252,15 +278,17 @@ void *thread_loop(void*){
else
{
if
(
volt_outputs
[
i
].
activated
==
1
){
volt_outputs
[
i
].
activated_input
=
0
;
volt_outputs
[
i
].
us_timestamps_input
=
current_
u
s
;
volt_outputs
[
i
].
us_timestamps_input
=
current_
n
s
;
raspi
.
set_pin_dir
(
volt_outputs
[
i
].
pin
,
raspi
.
INPUT
);
volt_outputs
[
i
].
activated
=
0
;
}
}
volt_outputs
[
i
].
us_timestamps
=
current_
u
s
;
volt_outputs
[
i
].
us_timestamps
=
current_
n
s
;
}
}
bl_raspi_iterations_nb
++
;
it_since_clock
++
;
if
(
it_since_clock
==
(
1
<<
16
))
update_time
();
}
//Stop voltages before living for security
FOR
(
i
,
bl_raspi_volt_outputs_nb
)
raspi
.
set_pin_dir
(
volt_outputs
[
i
].
pin
,
raspi
.
INPUT
);
...
...
@@ -270,7 +298,7 @@ void *thread_loop(void*){
void
bl_raspi_start_loop
(
uint32_t
*
RC_results
,
uchar
const
*
voltage_commands
){
pthread_t
thread
;
pthread_attr_t
attr
;
pthread_attr_t
attr
=
{
0
}
;
struct
sched_param
sched_param
;
run_loop
=
1
;
...
...
@@ -278,12 +306,12 @@ void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){
else
bl_raspi_RC_inputs
=
RC_results
;
if
(
voltage_commands
==
NULL
)
bl_raspi_volt_outputs
=
MANY_ALLOCATIONS
(
bl_raspi_volt_outputs_nb
,
uchar
);
else
bl_raspi_volt_outputs
=
voltage_commands
;
BLC_PTHREAD_CHECK
(
pthread_attr_init
(
&
attr
),
"Init pthread attribute"
);
BLC_PTHREAD_CHECK
(
pthread_attr_setinheritsched
(
&
attr
,
PTHREAD_EXPLICIT_SCHED
),
NULL
);
BLC_PTHREAD_CHECK
(
pthread_attr_setschedpolicy
(
&
attr
,
SCHED_FIFO
),
NULL
);
sched_param
.
sched_priority
=
20
;
BLC_PTHREAD_CHECK
(
pthread_attr_setschedparam
(
&
attr
,
&
sched_param
),
NULL
);
/*
BLC_PTHREAD_CHECK(pthread_attr_init(&attr), "Init pthread attribute");
BLC_PTHREAD_CHECK(pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED), NULL);
BLC_PTHREAD_CHECK(pthread_attr_setschedpolicy(&attr, SCHED_FIFO), NULL);
sched_param.sched_priority=
18
;
BLC_PTHREAD_CHECK(pthread_attr_setschedparam(&attr, &sched_param), NULL);
*/
//
BLC_PTHREAD_CHECK
(
pthread_create
(
&
thread
,
&
attr
,
thread_loop
,
NULL
),
"Creating bl_raspi thread"
);
}
...
...
bl_raspi.h
View file @
6e124a1b
...
...
@@ -7,11 +7,12 @@ extern uint32_t *bl_raspi_RC_inputs;
extern
unsigned
char
const
*
bl_raspi_volt_outputs
;
extern
int
bl_raspi_RC_inputs_nb
;
extern
int
bl_raspi_volt_outputs_nb
;
extern
int
buffer_size
;
/** Number of iterations of the loop. Useful to check the rate of the refreshing loop */
extern
int
bl_raspi_iterations_nb
;
extern
volatile
int
bl_raspi_iterations_nb
;
void
bl_raspi_add_RC_input
(
int
pin
,
int
us_tau
,
char
const
*
filename
);
void
bl_raspi_add_RC_input
(
int
pin
,
int
us_tau
,
uint32_t
*
buffer
);
void
bl_raspi_add_voltage_output
(
int
pin
,
char
const
*
filename
,
char
const
*
filename_input
);
void
bl_raspi_start_loop
(
uint32_t
*
RC_results
,
unsigned
char
const
*
voltage_commands
);
...
...
raspinobo.cpp
View file @
6e124a1b
...
...
@@ -7,14 +7,17 @@
#include
<math.h>
#include
<iostream>
#include
<fstream>
#include
<chrono>
#include
<thread>
#include
<chrono>
#include
<time.h>
#include
"blc_program.h"
#include
"blc_channel.h"
#include
"bl_raspi.h"
static
const
int
SOUND_PIN
=
13
;
static
const
int
LEFT_POT_PIN
=
6
;
static
const
int
RIGHT_POT_PIN
=
12
;
static
const
int
LEFT_MOTOR_PIN
=
26
;
...
...
@@ -34,7 +37,7 @@ static uchar gaussian_cdf(uint32_t s, float mu, float var){
blc_channel
motor_channel
,
sensor_channel
,
neurons_channel
;
int
main
(
int
argc
,
char
**
argv
){
blc_channel
gain_channel
,
target_channel
;
blc_channel
gain_channel
,
target_channel
,
buffer_channel
;
char
const
*
gpio_channel_name
,
*
period_str
,
*
volts_channel_name
,
*
target_channel_name
,
*
tracking
,
*
gain_name
;
int
us_period
,
previous_iterations_nb
;
char
const
*
values_debug
;
...
...
@@ -62,11 +65,16 @@ int main(int argc, char **argv){
if
(
gpio_channel_name
==
NULL
)
EXIT_ON_ERROR
(
"The ouput channel (option -o) must not be null"
);
// target_channel.create_or_open(target_channel_name, BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 1);
buffer_size
=
1024
;
buffer_channel
.
create_or_open
(
"/buffer"
,
BLC_CHANNEL_WRITE
,
'
UI32
'
,
'
NDEF
'
,
1
,
buffer_size
);
bl_raspi_add_RC_input
(
SOUND_PIN
,
1
,
buffer_channel
.
uints32
);
//"left_pot_file.bin")
/*
bl_raspi_add_RC_input(LEFT_POT_PIN, 333, NULL);//"left_pot_file.bin");
bl_raspi_add_RC_input(RIGHT_POT_PIN, 333, NULL); //"right_pot_file.bin");
bl_raspi_add_voltage_output(LEFT_MOTOR_PIN, NULL, NULL);
bl_raspi_add_voltage_output
(
RIGHT_MOTOR_PIN
,
NULL
,
NULL
);
bl_raspi_add_voltage_output(RIGHT_MOTOR_PIN, NULL, NULL);
*/
motor_channel
.
create_or_open
(
volts_channel_name
,
BLC_CHANNEL_WRITE
,
'
UIN8
'
,
'
NDEF
'
,
1
,
2
);
sensor_channel
.
create_or_open
(
gpio_channel_name
,
BLC_CHANNEL_WRITE
,
'
UI32
'
,
'
NDEF
'
,
1
,
bl_raspi_RC_inputs_nb
);
...
...
@@ -74,11 +82,12 @@ int main(int argc, char **argv){
neurons_channel
.
create_or_open
(
"/pinobo.neurons"
,
BLC_CHANNEL_WRITE
,
'
UIN8
'
,
'
NDEF
'
,
1
,
3
);
target_channel
.
create_or_open
(
target_channel_name
,
BLC_CHANNEL_READ
,
'
UIN8
'
,
'
NDEF
'
,
1
,
2
);
gain_channel
.
create_or_open
(
"/pinobo.gain"
,
BLC_CHANNEL_READ
,
'
UIN8
'
,
'
NDEF
'
,
1
,
1
);
gain_channel
.
create_or_open
(
"/pinobo.gain"
,
BLC_CHANNEL_READ
,
'
UIN8
'
,
'
NDEF
'
,
1
,
1
);
motor_channel
.
uchars
[
0
]
=
0
;
motor_channel
.
uchars
[
1
]
=
0
;
bl_raspi_start_loop
(
sensor_channel
.
uints32
,
motor_channel
.
uchars
);
previous_iterations_nb
=
0
;
...
...
@@ -109,53 +118,22 @@ int main(int argc, char **argv){
uchar
right_motor
;
}
line
;
SYSTEM_ERROR_CHECK
(
record_file
=
fopen
(
"pinobo.data"
,
"w"
),
NULL
,
"Opening 'pinobo.data'"
);
chrono
::
high_resolution_clock
::
time_point
tp
=
chrono
::
high_resolution_clock
::
now
();
//SYSTEM_ERROR_CHECK(record_file=fopen("pinobo.data", "w"), NULL, "Opening 'pinobo.data'");
struct
timespec
timestamp
;
uint32_t
previous_ns
,
delay_ns
;
previous_ns
=
0
;
BLC_COMMAND_LOOP
(
us_period
){
if
(
blc_loop_iteration
!=
0
)
blc_eprint_cursor_up
(
1
);
FOR
(
i
,
2
){
neurons_channel
.
uchars
[
i
]
=
neurons
[
i
].
update
(
sensor_channel
.
uints32
[
i
]);
}
fprintf
(
stderr
,
"Freq:%fHz
\n
n0:%d n0mu:%f n0var:%f"
,
(
bl_raspi_iterations_nb
-
previous_iterations_nb
)
/
((
float
)
us_period
/
1000000.
f
),
neurons_channel
.
uchars
[
2
],
neurons
[
0
].
mu
,
neurons
[
0
].
var
);
int
tmp
=
(
255
-
neurons_channel
.
uchars
[
1
]);
int
avg
=
128
+
(
neurons_channel
.
uchars
[
0
]
-
neurons_channel
.
uchars
[
1
])
/
2
;
int
dist_avg
=
abs
(
avg
-
neurons_channel
.
uchars
[
2
]);
int
dist_tmp
=
abs
(
tmp
-
neurons_channel
.
uchars
[
2
]);
int
dist_0
=
abs
(
neurons_channel
.
uchars
[
0
]
-
neurons_channel
.
uchars
[
2
]);
if
(
avg
<=
dist_tmp
){
if
(
avg
<=
dist_0
)
neurons_channel
.
uchars
[
2
]
=
avg
;
else
neurons_channel
.
uchars
[
2
]
=
neurons_channel
.
uchars
[
0
];
}
else
if
(
dist_0
<
dist_tmp
)
neurons_channel
.
uchars
[
2
]
=
neurons_channel
.
uchars
[
0
];
else
neurons_channel
.
uchars
[
2
]
=
tmp
;
clock_gettime
(
CLOCK_MONOTONIC
,
&
timestamp
);
if
(
timestamp
.
tv_nsec
>
previous_ns
)
delay_ns
=
timestamp
.
tv_nsec
-
previous_ns
;
else
delay_ns
=
1000000000
-
previous_ns
+
timestamp
.
tv_nsec
;
previous_ns
=
timestamp
.
tv_nsec
;
if
(
gain_channel
.
uchars
[
0
]
!=
0
)
{
if
(
gain_channel
.
uchars
[
0
]
>
30
)
error
=
(
target_channel
.
uchars
[
0
]
-
neurons_channel
.
uchars
[
2
])
*
gain_channel
.
uchars
[
0
]
/
128
;
else
error
=
(
target_channel
.
uchars
[
0
]
-
128
)
*
gain_channel
.
uchars
[
0
]
/
128
+
error
/
2
;
if
(
neurons_channel
.
uchars
[
2
]
==
0
)
error
=-
20
;
else
if
(
neurons_channel
.
uchars
[
2
]
==
255
)
error
=
20
;
if
(
abs
(
error
)
>
1
){
motor_channel
.
uchars
[
0
]
+=
(
CLIP_UCHAR
(
-
error
)
-
motor_channel
.
uchars
[
0
]);
motor_channel
.
uchars
[
1
]
+=
(
CLIP_UCHAR
(
+
error
)
-
motor_channel
.
uchars
[
1
]);
}
else
{
motor_channel
.
uchars
[
0
]
=
0
;
motor_channel
.
uchars
[
1
]
=
0
;
}
}
line
.
time
=
chrono
::
duration_cast
<
chrono
::
microseconds
>
(
chrono
::
high_resolution_clock
::
now
()
-
tp
).
count
();
line
.
left_pot
=
sensor_channel
.
uints32
[
0
];
line
.
right_pot
=
sensor_channel
.
uints32
[
1
];
line
.
left_motor
=
motor_channel
.
uchars
[
0
];
line
.
right_motor
=
motor_channel
.
uchars
[
1
];
fwrite
(
&
line
,
sizeof
(
line
),
1
,
record_file
);
fprintf
(
stderr
,
"%f, Freq:%fMHz
\n
n0:%d n0mu:%f n0var:%f"
,
(
float
)
delay_ns
,
(
bl_raspi_iterations_nb
-
previous_iterations_nb
)
/
((
float
)(
delay_ns
/
1000
)),
neurons_channel
.
uchars
[
2
],
neurons
[
0
].
mu
,
neurons
[
0
].
var
);
previous_iterations_nb
=
bl_raspi_iterations_nb
;
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment