| 1 | // %flair:license{
|
---|
| 2 | // This file is part of the Flair framework distributed under the
|
---|
| 3 | // CECILL-C License, Version 1.0.
|
---|
| 4 | // %flair:license}
|
---|
| 5 | // created: 2013/11/14
|
---|
| 6 | // filename: Bldc_impl.cpp
|
---|
| 7 | //
|
---|
| 8 | // author: Guillaume Sanahuja
|
---|
| 9 | // Copyright Heudiasyc UMR UTC/CNRS 7253
|
---|
| 10 | //
|
---|
| 11 | // version: $Id: $
|
---|
| 12 | //
|
---|
| 13 | // purpose: Virtual class for brushless drivers
|
---|
| 14 | //
|
---|
| 15 | //
|
---|
| 16 | /*********************************************************************/
|
---|
| 17 | #include "Bldc_impl.h"
|
---|
| 18 | #include "Bldc.h"
|
---|
| 19 | #include <GridLayout.h>
|
---|
| 20 | #include <GroupBox.h>
|
---|
| 21 | #include <DoubleSpinBox.h>
|
---|
| 22 | #include <cvmatrix.h>
|
---|
| 23 | #include <Label.h>
|
---|
| 24 | #include <PushButton.h>
|
---|
| 25 | #include <DataPlot1D.h>
|
---|
| 26 | #include <TabWidget.h>
|
---|
| 27 | #include <Tab.h>
|
---|
| 28 | #include <FrameworkManager.h>
|
---|
| 29 | #include <sstream>
|
---|
| 30 |
|
---|
| 31 | using std::string;
|
---|
| 32 | using std::ostringstream;
|
---|
| 33 | using namespace flair::core;
|
---|
| 34 | using namespace flair::gui;
|
---|
| 35 | using namespace flair::actuator;
|
---|
| 36 |
|
---|
| 37 | Bldc_impl::Bldc_impl(Bldc* self,Layout* layout,string name,uint8_t motors_count) {
|
---|
| 38 | this->self=self;
|
---|
| 39 | this->motors_count=motors_count;
|
---|
| 40 | this->layout=layout;
|
---|
| 41 | are_enabled=false;
|
---|
| 42 | is_running=false;
|
---|
| 43 | tested_motor=-1;
|
---|
| 44 |
|
---|
| 45 | values=(float*)malloc(motors_count*sizeof(float));
|
---|
| 46 | power=(float*)malloc(motors_count*sizeof(float));
|
---|
| 47 | for(int i=0;i<motors_count;i++) power[i]=1;
|
---|
| 48 |
|
---|
| 49 | //station sol
|
---|
| 50 | GroupBox* groupbox=new GroupBox(layout->NewRow(),"bldc");
|
---|
| 51 | flight_time=new Label(groupbox->NewRow(),"flight time");
|
---|
| 52 | min_value=new DoubleSpinBox(groupbox->NewRow(),"min value:",0,1,.1,2);
|
---|
| 53 | max_value=new DoubleSpinBox(groupbox->LastRowLastCol(),"max value:",0,1,.1,2);
|
---|
| 54 | test_value=new DoubleSpinBox(groupbox->LastRowLastCol(),"test value:",0,1,0.1);
|
---|
| 55 |
|
---|
| 56 | int index=0;
|
---|
| 57 | button_test=(PushButton**)malloc(motors_count*sizeof(PushButton*));
|
---|
| 58 | for(int i=0;i<motors_count/2;i++) {
|
---|
| 59 | for(int j=0;j<2;j++) {
|
---|
| 60 | ostringstream test_name;
|
---|
| 61 | test_name << "test motor " << index;
|
---|
| 62 | button_test[index]=new PushButton(groupbox->At(2+i,j),test_name.str());
|
---|
| 63 | index++;
|
---|
| 64 | }
|
---|
| 65 | }
|
---|
| 66 |
|
---|
| 67 | //flight time
|
---|
| 68 | FILE *file;
|
---|
| 69 | file=fopen("/etc/flight_time","r");
|
---|
| 70 | if (file == NULL) {
|
---|
| 71 | time_sec=0;
|
---|
| 72 | } else {
|
---|
| 73 | char ligne[32];
|
---|
| 74 | fgets(ligne, 32, file);
|
---|
| 75 | time_sec=atoi(ligne);
|
---|
| 76 | fclose(file);
|
---|
| 77 | }
|
---|
| 78 | flight_time->SetText("total flight time: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
|
---|
| 79 | }
|
---|
| 80 |
|
---|
| 81 | Bldc_impl::Bldc_impl(Bldc* self,uint8_t motors_count) {
|
---|
| 82 | this->self=self;
|
---|
| 83 | this->motors_count=motors_count;
|
---|
| 84 | values=NULL;
|
---|
| 85 | button_test=NULL;
|
---|
| 86 | power=NULL;
|
---|
| 87 | }
|
---|
| 88 |
|
---|
| 89 | Bldc_impl::~Bldc_impl() {
|
---|
| 90 | if(values!=NULL) free(values);
|
---|
| 91 | if(button_test!=NULL) free(button_test);
|
---|
| 92 | if(power!=NULL) free(power);
|
---|
| 93 | }
|
---|
| 94 |
|
---|
| 95 | void Bldc_impl::UseDefaultPlot(TabWidget* tab) {
|
---|
| 96 | Tab* plot_tab=new Tab(tab,"speeds");
|
---|
| 97 | plots=new DataPlot1D(plot_tab->NewRow(),"values",0,1);
|
---|
| 98 | for(int i=0;i<motors_count;i++) {
|
---|
| 99 | plots->AddCurve(self->output->Element(i));
|
---|
| 100 | }
|
---|
| 101 | }
|
---|
| 102 |
|
---|
| 103 | void Bldc_impl::UpdateFrom(const io_data *data) {
|
---|
| 104 | cvmatrix* input=(cvmatrix*)data;
|
---|
| 105 | bool is_motor_running=false;
|
---|
| 106 |
|
---|
| 107 | if(values==NULL) return;//nothing to do in simulator
|
---|
| 108 |
|
---|
| 109 | if(input->Rows()!=motors_count) {
|
---|
| 110 | self->Err("nb motors mismatch\n");
|
---|
| 111 | return;
|
---|
| 112 | }
|
---|
| 113 |
|
---|
| 114 | input->GetMutex();
|
---|
| 115 | for(int i=0;i<motors_count;i++) {
|
---|
| 116 | if(input->ValueNoMutex(i,0)!=0) is_motor_running=true;
|
---|
| 117 | if(are_enabled) {
|
---|
| 118 | //Printf("%i %f %f\n",i,input->ValueNoMutex(i,0),power[i]);
|
---|
| 119 | values[i]=power[i]*Sat(input->ValueNoMutex(i,0));
|
---|
| 120 | //Printf("%i %f\n",i,values[i]);
|
---|
| 121 | } else {
|
---|
| 122 | values[i]=0;
|
---|
| 123 | }
|
---|
| 124 | }
|
---|
| 125 | input->ReleaseMutex();
|
---|
| 126 |
|
---|
| 127 | if(are_enabled && is_motor_running && !is_running) {
|
---|
| 128 | flight_start_time = GetTime();
|
---|
| 129 | is_running=true;
|
---|
| 130 | }
|
---|
| 131 | if((!are_enabled || !is_motor_running) && is_running) {
|
---|
| 132 | Time now= GetTime();
|
---|
| 133 | int t_sec;
|
---|
| 134 | FILE *file;
|
---|
| 135 | char ligne[32];
|
---|
| 136 |
|
---|
| 137 | t_sec=(now - flight_start_time)/1000000000;
|
---|
| 138 | time_sec+=t_sec;
|
---|
| 139 |
|
---|
| 140 | Printf("temps de vol: %is = %imin\n",t_sec,t_sec/60);
|
---|
| 141 | //Printf("temps de vol total: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
|
---|
| 142 | flight_time->SetText("total flight time: %is = %imin = %ih\n",time_sec,time_sec/60,time_sec/3600);
|
---|
| 143 |
|
---|
| 144 | file=fopen("/etc/flight_time","w");
|
---|
| 145 | if (file == NULL) {
|
---|
| 146 | Printf("Erreur a l'ouverture du fichier d'info vol\n");
|
---|
| 147 | } else {
|
---|
| 148 | sprintf(ligne,"%i",time_sec);
|
---|
| 149 | fputs(ligne,file);
|
---|
| 150 | fclose(file);
|
---|
| 151 | }
|
---|
| 152 | is_running=false;
|
---|
| 153 | }
|
---|
| 154 |
|
---|
| 155 | for(int i=0;i<motors_count;i++) {
|
---|
| 156 | if(button_test[i]->Clicked()==true) {
|
---|
| 157 | if(!are_enabled) {
|
---|
| 158 | tested_motor=i;
|
---|
| 159 | test_start_time=GetTime();
|
---|
| 160 | LockUserInterface();
|
---|
| 161 | } else {
|
---|
| 162 | self->Warn("testing motor is not possible when enabled\n");
|
---|
| 163 | }
|
---|
| 164 | }
|
---|
| 165 | }
|
---|
| 166 | if(tested_motor!=-1) {
|
---|
| 167 | for(int i=0;i<motors_count;i++) {
|
---|
| 168 | values[i]=0;
|
---|
| 169 | }
|
---|
| 170 | values[tested_motor]=test_value->Value();
|
---|
| 171 |
|
---|
| 172 | if(GetTime()>(test_start_time+2*1000000000)) {
|
---|
| 173 | tested_motor=-1;
|
---|
| 174 | UnlockUserInterface();
|
---|
| 175 | }
|
---|
| 176 | }
|
---|
| 177 |
|
---|
| 178 | self->SetMotors(values);
|
---|
| 179 | self->output->SetDataTime(data->DataTime());
|
---|
| 180 | self->ProcessUpdate(self->output);
|
---|
| 181 | }
|
---|
| 182 |
|
---|
| 183 | float Bldc_impl::Sat(float value) {
|
---|
| 184 | float result=value;
|
---|
| 185 |
|
---|
| 186 | if(result<min_value->Value()) {
|
---|
| 187 | result=min_value->Value();
|
---|
| 188 | }
|
---|
| 189 | if(result>max_value->Value()) {
|
---|
| 190 | result=max_value->Value();
|
---|
| 191 | }
|
---|
| 192 |
|
---|
| 193 | return result;
|
---|
| 194 | }
|
---|
| 195 |
|
---|
| 196 | void Bldc_impl::LockUserInterface(void) const {
|
---|
| 197 | layout->setEnabled(false);
|
---|
| 198 | }
|
---|
| 199 |
|
---|
| 200 | void Bldc_impl::UnlockUserInterface(void) const {
|
---|
| 201 | layout->setEnabled(true);
|
---|
| 202 | }
|
---|