Commit 0276bedc authored by Arnaud Blanchard's avatar Arnaud Blanchard

use armadillo

parent d7f46bf0
......@@ -15,13 +15,14 @@ project(raspinobo)
find_package(blc_channel REQUIRED)
find_package(blc_program REQUIRED)
find_package(Armadillo REQUIRED)
find_library(BCM_HOST bcm_host PATHS /opt/vc/lib/ )
add_definitions(${BL_DEFINITIONS} -std=c++14 -Wall -W -fdiagnostics-color=always -march=native)
include_directories(${BL_INCLUDE_DIRS} /opt/vc/include)
include_directories(${BL_INCLUDE_DIRS} /opt/vc/include ${ARMADILLO_INCLUDE_DIRS})
add_executable(raspinobo raspinobo.cpp bl_raspi.cpp bl_raspi_core.cpp)
target_link_libraries(raspinobo ${BL_LIBRARIES} ${BCM_HOST})
target_link_libraries(raspinobo ${BL_LIBRARIES} ${BCM_HOST} ${ARMADILLO_LIBRARIES})
if (NOT APPLE)
#Give the raw capability to the target /dev/mem
......
......@@ -14,6 +14,10 @@
#include <time.h>
#include <math.h>
#include <armadillo>
using namespace arma;
#include <vector>
static const int DEBUG_BUFFER_SIZE=256;
......@@ -45,11 +49,11 @@ inline void update_time(){
clock_gettime(CLOCK_MONOTONIC_COARSE, &timestamp);
current_ns=timestamp.tv_nsec;
delta_ns=DELTA_NS(current_ns, previous_ns);
// delta_ns=DELTA_NS(current_ns, previous_ns);
// DEBUG_VALUE(current_ns);
if (it_since_clock){
/* if (it_since_clock){
current_ns_per_it=(delta_ns+1)/it_since_clock;
// if (current_ns_per_it/ns_per_it)
......@@ -70,7 +74,7 @@ inline void update_time(){
if (delta_ns >= input.tau) input.activated=1;
else input.activated=(input.tau-delta_ns)/ns_per_it+1;
}
}
}*/
}
Ohm_input::Ohm_input(int pin, int us_tau, uint32_t *buffer):index(0), flag(FLAG(pin)), pin(pin), buffer(buffer), activated(us_tau*1000){
......@@ -124,7 +128,9 @@ void *thread_loop(void*){
*raspi.gpfset0=0;
// initial_us=3000000*(int)(*raspi.ptime_us/3000000)-3000000;
update_time();
// update_time();
vec A = linspace(0.9, 1.1, 10);
for(auto &input : ohm_inputs){
*raspi.gpfset0|=input.flag; //Set pin high
......@@ -138,7 +144,6 @@ void *thread_loop(void*){
raspi.set_pin_dir(output.pin, raspi.INPUT);
}
uint32_t results;
while(run_loop){
clock_gettime(CLOCK_MONOTONIC, &timestamp);
......@@ -154,16 +159,17 @@ void *thread_loop(void*){
}
}
}
// if (bl_raspi_iterations_nb==0){
for(auto &output : volt_outputs){
if (*output.value_ptr > rand()/(RAND_MAX/255)){
// if (bl_raspi_iterations_nb%32==output.pin){
/* if (*output.value_ptr > rand()/(RAND_MAX/255)){
raspi.set_pin_dir(output.pin, raspi.OUTPUT);
}
else{
raspi.set_pin_dir(output.pin, raspi.INPUT);
}
}
//}
// }*/
}
//Output volts
// FOR (i, bl_raspi_volt_outputs_nb){
......@@ -204,26 +210,18 @@ void *thread_loop(void*){
return NULL;
}
void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){
void bl_raspi_start_loop(){
pthread_t thread;
pthread_attr_t attr={{(0)}};
struct sched_param sched_param;
run_loop=1;
debug_channel.create_or_open("/raspinobo.debug", BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, DEBUG_BUFFER_SIZE);
/*if (RC_results==NULL) bl_raspi_RC_inputs=MANY_ALLOCATIONS(ohm_inputs.size(), uint32_t);
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_RR), 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");
BLC_PTHREAD_CHECK(pthread_create(&thread, nullptr, thread_loop, nullptr), "Creating bl_raspi thread");
}
void bl_raspi_stop_loop(){
......
......@@ -52,7 +52,7 @@ extern volatile int bl_raspi_iterations_nb;
void bl_raspi_add_RC_input(int pin, int us_tau, uint32_t *buffer);
void bl_raspi_add_voltage_output(int pin, uint8_t *value_ptr, char const *filename, char const* filename_input);
void bl_raspi_start_loop(uint32_t *RC_results, unsigned char const* voltage_commands);
void bl_raspi_start_loop();
void bl_raspi_stop_loop();
#endif
This diff is collapsed.
......@@ -24,13 +24,40 @@ static const int RIGHT_MOTOR_PIN=20;
using namespace std;
float gaussian(float mu, float var){
}
static uchar gaussian_cdf(uint32_t s, float mu, float var){
return static_cast<uchar>((1+erf((s-mu)/sqrt(2*var)))*255/2); //var+1 is to be sure it is not null
}
struct neuron{
uint32_t *input_ptr;
uint32_t min=0, max=UINT32_MAX;
float mu=0, var=0;
uint32_t it=1;
int pos;
uchar *output_ptr;
neuron(uint32_t *value_ptr):input_ptr(value_ptr),pos(pos){
}
void update (){
uint32_t s=*input_ptr;
float err;
err=s-mu;
mu+=err/(float)it;
if (it != 1) var+=(pow(s-mu, 2)-var)/(float)(it-1); //This is not an exact estimator as mu should be the final one, not the stochastic one
it++;
*output_ptr= gaussian_cdf(s, mu, var+1); //(1+erf((s-mu)/sqrt(2*var+1)))*255/2.; //var+1 is to be sure it is not null
}
};
float gaussian(float mu, float var){
}
static blc_channel motor_channel, sensor_channel, neurons_channel;
int main(int argc, char **argv){
......@@ -69,15 +96,12 @@ int main(int argc, char **argv){
blc_channel buffer_left_channel("/left_buffer", BLC_CHANNEL_WRITE, 'UI32', 'NDEF', 1, buffer_size);
blc_channel buffer_right_channel("/right_buffer", BLC_CHANNEL_WRITE, 'UI32', 'NDEF', 1, buffer_size);
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, ohm_inputs.size());
sensor_channel.create_or_open(gpio_channel_name, BLC_CHANNEL_WRITE, 'UI32', 'NDEF', 1, 2);
sensor_channel.publish();
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);
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);
......@@ -86,28 +110,23 @@ int main(int argc, char **argv){
bl_raspi_add_RC_input(LEFT_POT_PIN, 300, &sensor_channel.uints32[0]);//"left_pot_file.bin");
bl_raspi_add_RC_input(RIGHT_POT_PIN, 300, &sensor_channel.uints32[1]); //"right_pot_file.bin");
bl_raspi_add_voltage_output(LEFT_MOTOR_PIN, &motor_channel.uchars[0], NULL, NULL);
bl_raspi_add_voltage_output(RIGHT_MOTOR_PIN, &motor_channel.uchars[1], NULL, NULL);
bl_raspi_add_voltage_output(LEFT_MOTOR_PIN, &motor_channel.uchars[0], nullptr, nullptr);
bl_raspi_add_voltage_output(RIGHT_MOTOR_PIN, &motor_channel.uchars[1], nullptr, nullptr);
bl_raspi_start_loop(sensor_channel.uints32, motor_channel.uchars);
bl_raspi_start_loop();
previous_iterations_nb=0;
struct neuron{
uint32_t min=0, max=UINT32_MAX;
float mu=0, var=0;
uint32_t it=1;
uchar update (uint32_t s){
float err;
err=s-mu;
mu+=err/(float)it;
if (it != 1) var+=(pow(s-mu, 2)-var)/(float)(it-1); //This is not an exact estimator as mu should be the final one, not the stochastic one
it++;
return gaussian_cdf(s, mu, var+1); //(1+erf((s-mu)/sqrt(2*var+1)))*255/2.; //var+1 is to be sure it is not null
}
};
vector<struct neuron> neurons;//=(struct neuron*)ranges_channel.uints32;
neurons.emplace_back(&sensor_channel.uints32[0]);
neurons.emplace_back(&sensor_channel.uints32[1]);
neurons_channel.create_or_open("/pinobo.neurons", BLC_CHANNEL_WRITE, 'UIN8', 'NDEF', 1, neurons.size()+2);
for (int i; i < neurons.size(); i++ ){
neurons[i].output_ptr = &neurons_channel.uchars[i];
}
struct neuron neurons[2];//=(struct neuron*)ranges_channel.uints32;
int i;
int error;
......@@ -123,6 +142,8 @@ int main(int argc, char **argv){
uint32_t previous_ns, delay_ns;
previous_ns=0;
uchar gain;
BLC_COMMAND_LOOP(us_period){
if (blc_loop_iteration!=0) blc_eprint_cursor_up(1);
......@@ -131,6 +152,23 @@ int main(int argc, char **argv){
else delay_ns = 1000000000 - previous_ns + timestamp.tv_nsec;
previous_ns=timestamp.tv_nsec;
for (auto &neuron:neurons){
neuron.update();
}
gain=neurons_channel.uchars[3];
int error = neurons_channel.uchars[2] - neurons_channel.uchars[1];
if (error > 0)
{
motor_channel.uchars[0]=CLIP_UCHAR(error*gain/64);
motor_channel.uchars[1]=0;
}
else
{
motor_channel.uchars[1]=CLIP_UCHAR(-error*gain/64);
motor_channel.uchars[0]=0;
}
fprintf(stderr, "Freq:%.4fMHz\n n0:%d n0mu:%f n0var:%f", (float)(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;
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment