Commit c10f528b authored by Arnaud Blanchard's avatar Arnaud Blanchard

improve asservissement

parent 4f0b7483
......@@ -67,7 +67,7 @@ typedef struct bl_raspi{
int bl_raspi_RC_inputs_nb=0;
int bl_raspi_volt_outputs_nb=0;
uint32_t *bl_raspi_RC_inputs=NULL;
uint32_t const *bl_raspi_volt_outputs=NULL;
uchar const *bl_raspi_volt_outputs=NULL;
int bl_raspi_iterations_nb=0;
static int run_loop;
......@@ -243,9 +243,9 @@ void *thread_loop(void*){
volt_outputs[0].activated_input=1;
}*/
delta_us=current_us-volt_outputs[i].us_timestamps;
if (delta_us > 100){
if (delta_us > 10){
volt_outputs[i].average+=(volt_outputs[i].activated-volt_outputs[i].average)*(delta_us/100);
if (bl_raspi_volt_outputs[i] > rand()/(RAND_MAX/100)){// volt_outputs[i].average*100){ //Comparing average time up with the requested one//){
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;
}
......@@ -260,18 +260,15 @@ void *thread_loop(void*){
volt_outputs[i].us_timestamps=current_us;
}
}
bl_raspi_iterations_nb++;
}
//Stop voltages before living for security
FOR(i, bl_raspi_volt_outputs_nb) raspi.set_pin_dir(volt_outputs[i].pin, raspi.INPUT);
FOR(i, bl_raspi_RC_inputs_nb) raspi.set_pin_dir(ohm_inputs[i].pin, raspi.INPUT);
return NULL;
}
void bl_raspi_start_loop(uint32_t *RC_results, uint32_t const *voltage_commands){
void bl_raspi_start_loop(uint32_t *RC_results, uchar const *voltage_commands){
pthread_t thread;
pthread_attr_t attr;
struct sched_param sched_param;
......@@ -279,7 +276,7 @@ void bl_raspi_start_loop(uint32_t *RC_results, uint32_t const *voltage_commands)
if (RC_results==NULL) bl_raspi_RC_inputs=MANY_ALLOCATIONS(bl_raspi_RC_inputs_nb, uint32_t);
else bl_raspi_RC_inputs=RC_results;
if (voltage_commands==NULL) bl_raspi_volt_outputs=MANY_ALLOCATIONS(bl_raspi_volt_outputs_nb, uint32_t);
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");
......
......@@ -4,7 +4,7 @@
#include <stdint.h>
extern uint32_t *bl_raspi_RC_inputs;
extern uint32_t const *bl_raspi_volt_outputs;
extern unsigned char const *bl_raspi_volt_outputs;
extern int bl_raspi_RC_inputs_nb;
extern int bl_raspi_volt_outputs_nb;
......@@ -14,7 +14,7 @@ extern int bl_raspi_iterations_nb;
void bl_raspi_add_RC_input(int pin, int us_tau, char const *filename);
void bl_raspi_add_voltage_output(int pin, char const *filename, char const* filename_input);
void bl_raspi_start_loop(uint32_t *RC_results, uint32_t const* voltage_commands);
void bl_raspi_start_loop(uint32_t *RC_results, unsigned char const* voltage_commands);
void bl_raspi_stop_loop();
#endif
......@@ -9,6 +9,7 @@
#include <fstream>
#include <chrono>
#include <thread>
#include <chrono>
#include "blc_program.h"
#include "blc_channel.h"
......@@ -19,12 +20,22 @@ static const int RIGHT_POT_PIN=12;
static const int LEFT_MOTOR_PIN=26;
static const int RIGHT_MOTOR_PIN=20;
blc_channel motor_channel, sensor_channel;
using namespace std;
float gaussian(float mu, float var){
}
static uchar gaussian_cdf(uint32_t s, float mu, float var){
return (1+erf((s-mu)/sqrt(2*var)))*255/2; //var+1 is to be sure it is not null
}
blc_channel motor_channel, sensor_channel, neurons_channel;
int main(int argc, char **argv){
char const *gpio_channel_name, *period_str, *default_output, *volts_channel_name;
blc_channel gain_channel, target_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;
......@@ -32,41 +43,123 @@ int main(int argc, char **argv){
char const *currentFile = "current.bin";
char const *forceFile = "force_file.bin";
volts_channel_name="/toto";
SYSTEM_ERROR_CHECK(asprintf((char**)&default_output, "/%s%d", basename(argv[0]), getpid()), -1, "Generating default name"); //This will not be free but it is only allocate once
uint64_t useconds;
FILE *record_file;
blc_program_set_description("Pinobo");
blc_program_add_option(&values_debug, 'd', "debug", NULL, "Crete a blc_channel '/o_raspi_gpio_values' too set the sent values", NULL);
blc_program_add_option(&period_str, 'p', "period", "integer", "refresh period in us", "10000"); //100Hz
blc_program_add_option(&gpio_channel_name, 'o', "output", "blc_channel-out", "Channel containing the values of the gpio.", default_output);
// blc_program_add_parameter(&target_channel_name, "blc_channel-in", 1, "Channel defining the volts", NULL );
// blc_program_add_parameter(&volts_channel_name, "blc_channel-in", 1, "Channel defining the volts", NULL );
blc_program_add_option(&gpio_channel_name, 'o', "output", "blc_channel-out", "Channel containing the values of the gpio.", "/pinobo.sensors");
// blc_program_add_option(&gain_name, 'g', "gain", "blc_channel-in", "Channel defining the speed", "/pinobo.gain" );
blc_program_add_option(&target_channel_name, 'T', "target", "blc_channel-in", "Channel defining the volts", "/pinobo.target" );
blc_program_add_parameter(&volts_channel_name, "blc_channel-in", 0, "Channel defining the volts", "/pinobo.motors" );
blc_program_init(&argc, &argv, blc_quit);
SSCANF(1, period_str, "%d", &us_period);
if (gpio_channel_name==NULL) EXIT_ON_ERROR("The ouput channel (option -o) must not be null");
if (volts_channel_name) motor_channel.create_or_open(volts_channel_name, BLC_CHANNEL_WRITE, 'UI32', 'NDEF', 1, 2);
// target_channel.create_or_open(target_channel_name, BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 1);
bl_raspi_add_RC_input(LEFT_POT_PIN, 333, "left_pot_file.bin");
bl_raspi_add_RC_input(RIGHT_POT_PIN, 333, "right_pot_file.bin");
// target_channel.create_or_open(target_channel_name, BLC_CHANNEL_WRITE, 'FL32', 'NDEF', 1, 1);
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);
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);
sensor_channel.publish();
motor_channel.uints32[0]=0;
motor_channel.uints32[1]=0;
bl_raspi_start_loop(sensor_channel.uints32, motor_channel.uints32);
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);
motor_channel.uchars[0]=0;
motor_channel.uchars[1]=0;
bl_raspi_start_loop(sensor_channel.uints32, motor_channel.uchars);
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
}
};
struct neuron neurons[2];//=(struct neuron*)ranges_channel.uints32;
int i;
int error;
struct line{
uint32_t time;
uint32_t left_pot;
uint32_t right_pot;
uchar left_motor;
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();
previous_iterations_nb=bl_raspi_iterations_nb;
BLC_COMMAND_LOOP(us_period){
if (blc_loop_iteration!=0) blc_eprint_cursor_up(1);
fprintf(stderr, "Freq:%fHz\n", (bl_raspi_iterations_nb-previous_iterations_nb)/((float)us_period/1000000.f));
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;
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);
previous_iterations_nb=bl_raspi_iterations_nb;
}
fclose(record_file);
bl_raspi_stop_loop();
return EXIT_SUCCESS;
}
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