source: robolliurex/trunk/fuentes/src/robocore.cpp @ 4393

Last change on this file since 4393 was 4393, checked in by hectorgh, 2 years ago

fixing udev rule

File size: 10.1 KB
Line 
1
2
3#include "robocore.h"
4#include "robomenus.h"
5#include "robolliurex.h"
6
7
8#include <lsf-1.0/system.hpp>
9#include <lsf-1.0/filesystem.hpp>
10#include <gtk/gtk.h>
11
12#include <pthread.h>
13#include <iostream>
14#include <cstdio>
15#include <sstream>
16#include <unistd.h>
17#include <vector>
18#include <iomanip>
19
20using namespace net::lliurex::robolliurex;
21using namespace std;
22
23
24/*
25        ************* MainWindow ************
26*/
27MainWindow::MainWindow() : X11Window(800,600,0)
28{
29        have_batt=false;
30        batt=0.0f;
31       
32        srf_background = RoboCore::GetCore()->rsrc->GetSurface("blueprint.png");
33       
34       
35}
36
37void MainWindow::Flip()
38{
39        cairo_set_source_surface(cairo,srf_background,0,0);
40        cairo_paint(cairo);
41       
42        cairo_text_extents_t te;
43        cairo_font_extents_t fe;
44       
45        stringstream ss;
46        ss<<"Version: "<<VERSION;
47       
48        cairo_select_font_face (cairo, "Ubuntu", CAIRO_FONT_SLANT_NORMAL, CAIRO_FONT_WEIGHT_NORMAL);
49        cairo_set_font_size (cairo, 11.0);
50        cairo_text_extents (cairo, ss.str().c_str(), &te);
51        cairo_font_extents (cairo, &fe);
52       
53        cairo_surface_t * target = cairo_get_target(cairo);
54        float width = cairo_image_surface_get_width(target);
55        float height = cairo_image_surface_get_height(target);
56       
57       
58       
59        cairo_move_to (cairo,5.0f,height-fe.height);
60        cairo_set_source_rgb (cairo, 0.95, 0.95, 0.95);
61        cairo_show_text (cairo, ss.str().c_str());     
62       
63        if(have_batt)
64        {
65                ss.str("");
66                ss<<"Batt: "<<std::setprecision(2) << std::fixed<<batt<<" V";
67                               
68                cairo_move_to (cairo,width-te.width-3.0f,fe.height+3.0f);
69                cairo_set_source_rgb (cairo, 0.95, 0.95, 0.95);
70                cairo_show_text (cairo, ss.str().c_str());
71        }
72               
73        X11Window::Flip();
74}
75
76
77void MainWindow::SetBattery(float level)
78{
79        have_batt=true;
80        batt=level;
81}
82
83
84
85RoboCore * RoboCore::singleton = nullptr;
86string RoboCore::comm_name[]={"/dev/ttyS0","/dev/ttyS1","usb"};
87
88RoboCore::RoboCore() : Application("net.lliurex.robolliurex")
89{
90        //set outselves as main application
91        Application::Set(this);
92       
93        cout<<"RoboCore::Constructor"<<endl;
94       
95        is_firmware_downloading=false;
96        is_comm_testing=false;
97        is_program_downloading=false;
98        is_battery_requesting=false;
99               
100       
101       
102}
103
104
105
106void RoboCore::Init()
107{
108
109
110        //load resources
111        rsrc = new RsrcManager(RSRC_PATH); 
112
113        /* guessing if usb tower is plugged */
114       
115       
116        vector<string> modules;
117       
118        //ask lsf for current kernel modules
119        modules=system::GetModuleList(); 
120       
121        usb_tower=false;
122       
123        //look for legousbtower
124        for(int n=0;n<modules.size();n++) 
125        {
126                if(modules[n]=="legousbtower")
127                {
128                        usb_tower=true;
129                }
130        }
131       
132        if(usb_tower)
133        {
134                cout<<"* Found Lego USB tower"<<endl;
135                comm_port = RBC_PORT_USB;
136               
137                vector<string> towers = filesystem::List("/dev/usb/legousbtower*");
138               
139                if(towers.size()==0)
140                {
141                        cout<<"* Warning, Lego driver is loaded but something failed!"<<endl;
142                }
143                else
144                {
145                        string usbp="usb:"+towers[0];
146                        RoboCore::comm_name[comm_port]=usbp;
147                        cout<<"Using tower device: "<<towers[0]<<endl;
148                }
149               
150        }
151        else
152        {
153                cout<<"* Lego USB tower not found"<<endl;
154                comm_port = RBC_PORT_COM1;
155                cout<<"Using: "<<RoboCore::comm_name[comm_port]<<endl;
156        }
157       
158       
159       
160       
161        /* gui setup */
162       
163        gtk_init(0,nullptr); //gtk start up
164       
165        window = new MainWindow();
166        AddWindow(window);
167       
168       
169        //drag & drop setup
170        vector<string>targets={"text/uri-list"};
171        window->SetDndTargets(targets);
172       
173        window->SetTitle("Robolliurex");
174        window->AddLayer(new MainScreen());
175        window->AddLayer(new SetupScreen());
176        window->AddLayer(new PilotMenu());
177        window->AddLayer(new COMScreen());
178        window->AddLayer(new FirmwareScreen());
179        window->AddLayer(new TestScreen());
180        window->AddLayer(new DropScreen());
181        window->AddLayer(new Pilot1());
182        window->AddLayer(new Pilot2());
183        window->AddLayer(new Pilot3());
184        window->AddLayer(new Pilot4());
185}
186
187
188RoboCore::~RoboCore()
189{
190        cout<<"RoboCore::Destructor"<<endl;
191        delete rsrc;
192}
193
194RoboCore * RoboCore::GetCore()
195{
196        if(RoboCore::singleton==nullptr)
197        {
198                RoboCore::singleton=new RoboCore();
199        }
200       
201        return RoboCore::singleton;
202}
203
204void RoboCore::Destroy()
205{
206        if(RoboCore::singleton!=nullptr)delete RoboCore::singleton;
207}
208
209void * auxDownloadFirmwareBlink(void * param)
210{
211       
212        while(RoboCore::GetCore()->is_firmware_downloading)
213        {
214                Message * msg = new Message(RBW_MSG_FIRM_STATUS);
215                msg->data["status"]=new MessageDataInt(RBC_FIRM_STATUS_DOWNLOADING);
216                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
217                usleep(150000);
218        }
219}
220
221void * auxDownloadFirmware(void * param)
222{
223       
224        /* nqc -Susb -firmware firm328.lgo */
225        Message * msg;
226        FILE * fp;
227        char buff[32];
228        int status;
229               
230        string cmd = string("nqc -S") + RoboCore::comm_name[RoboCore::GetCore()->comm_port] + " -firmware " + FIRM_PATH;
231        cout<<cmd<<endl;
232        fp = popen( cmd.c_str(),"r" );
233       
234        if(fp==nullptr)
235        {
236                msg = new Message(RBW_MSG_FIRM_STATUS);
237                msg->data["status"]=new MessageDataInt(RBC_FIRM_STATUS_ERROR);
238                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
239               
240                return nullptr;
241        }
242       
243        status = pclose(fp);
244       
245        RoboCore::GetCore()->is_firmware_downloading=false;
246       
247        if(status==0)
248        {
249                msg = new Message(RBW_MSG_FIRM_STATUS);
250                msg->data["status"]=new MessageDataInt(RBC_FIRM_STATUS_COMPLETED);
251                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
252        }
253        else
254        {
255                msg = new Message(RBW_MSG_FIRM_STATUS);
256                msg->data["status"]=new MessageDataInt(RBC_FIRM_STATUS_ERROR);
257                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
258        }
259       
260        return nullptr;
261}
262
263void RoboCore::DownloadFirmware()
264{
265        if(!is_firmware_downloading)
266        {
267                is_firmware_downloading=true;
268               
269                pthread_t aux_thread;
270                pthread_create(&aux_thread,nullptr,auxDownloadFirmware,nullptr);
271               
272                pthread_t blink_thread;
273                pthread_create(&blink_thread,nullptr,auxDownloadFirmwareBlink,nullptr);
274       
275        }
276}
277
278void * auxTestCommBlink(void * param)
279{
280        while(RoboCore::GetCore()->is_comm_testing==true)
281        {
282                Message * msg = new Message(RBW_MSG_TEST_STATUS);
283                msg->data["status"]=new MessageDataInt(RBC_TEST_STATUS_RUNNING);
284                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
285                usleep(150000);
286        }
287}
288
289void * auxTestComm(void * param)
290{
291       
292        Message * msg;
293        FILE * fp;
294        char buff[32];
295        int status;
296       
297       
298        string cmd = string("nqc -S") + RoboCore::comm_name[RoboCore::GetCore()->comm_port] + " -raw 10";
299        cout<<cmd<<endl;
300        fp = popen( cmd.c_str(),"r" );
301       
302        if(fp==nullptr)
303        {
304                RoboCore::GetCore()->is_comm_testing=false;
305                msg = new Message(RBW_MSG_TEST_STATUS);
306                msg->data["status"]=new MessageDataInt(RBC_TEST_STATUS_ERROR);
307                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
308               
309                return nullptr;
310        }
311       
312        while(fgets(buff, sizeof(buff), fp)!=nullptr)
313        {
314               
315        }
316       
317        status = pclose(fp);
318       
319        if(status==0)
320        {
321                RoboCore::GetCore()->is_comm_testing=false;
322                msg = new Message(RBW_MSG_TEST_STATUS);
323                msg->data["status"]=new MessageDataInt(RBC_TEST_STATUS_OK);
324                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
325        }
326        else
327        {
328                RoboCore::GetCore()->is_comm_testing=false;
329                msg = new Message(RBW_MSG_TEST_STATUS);
330                msg->data["status"]=new MessageDataInt(RBC_TEST_STATUS_ERROR);
331                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
332        }
333       
334        return nullptr;
335}
336
337void RoboCore::TestComm()
338{
339        if(!is_comm_testing)
340        {
341                is_comm_testing=true;
342                pthread_t aux_thread;
343                pthread_create(&aux_thread,nullptr,auxTestComm,nullptr);
344               
345                pthread_t blink_thread;
346                pthread_create(&blink_thread,nullptr,auxTestCommBlink,nullptr);
347        }
348}
349
350void * auxDownloadProgram(void * param)
351{
352        Message * msg = new Message(RBW_MSG_RUN_STATUS);
353        msg->data["status"]=new MessageDataInt(RBC_RUN_STATUS_DOWNLOADING); 
354        RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
355       
356        FILE * fp;
357        char buff[32];
358        int status=0;
359       
360       
361        string cmd = string("nqc -S") + RoboCore::comm_name[RoboCore::GetCore()->comm_port] + " -TRCX2 -d " + RoboCore::GetCore()->program_path;
362       
363        fp = popen( cmd.c_str(),"r" );
364       
365        if(fp==nullptr)
366        {
367                msg = new Message(RBW_MSG_RUN_STATUS);
368                msg->data["status"]=new MessageDataInt(RBC_RUN_STATUS_NQC_ERROR); 
369                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
370               
371                RoboCore::GetCore()->is_program_downloading=false;
372                return nullptr;
373        }
374       
375        while(fgets(buff, sizeof(buff), fp)!=nullptr)
376        {
377                cout << buff;
378        }
379       
380        status=pclose(fp);
381       
382        if(status!=0)
383        {
384                msg = new Message(RBW_MSG_RUN_STATUS);
385                msg->data["status"]=new MessageDataInt(RBC_RUN_STATUS_COMM_ERROR); 
386                RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
387               
388                RoboCore::GetCore()->is_program_downloading=false;
389                return nullptr;
390        }
391       
392        msg = new Message(RBW_MSG_RUN_STATUS);
393        msg->data["status"]=new MessageDataInt(RBC_RUN_STATUS_COMPLETED); 
394        RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
395       
396
397        RoboCore::GetCore()->is_program_downloading=false;
398       
399        RoboCore::GetCore()->GetBattery();
400       
401        return nullptr;
402}
403
404void RoboCore::DownloadProgram(string path)
405{
406        if(!is_program_downloading)
407        {
408                is_program_downloading=true;
409                pthread_t aux_thread;
410                program_path=path;
411                pthread_create(&aux_thread,nullptr,auxDownloadProgram,nullptr);
412        }
413}
414
415
416void * auxGetBattery(void * param)
417{
418       
419        FILE * fp;
420        char buff[32];
421        int status=0;
422               
423       
424        string cmd = string("nqc -S") + RoboCore::comm_name[RoboCore::GetCore()->comm_port] + " -raw 30";
425               
426        fp = popen( cmd.c_str(),"r" );
427       
428        if(fp==nullptr)
429        {
430                return nullptr;
431        }
432       
433        while(fgets(buff, sizeof(buff), fp)!=nullptr)
434        {
435                cout << buff;
436        }
437       
438        status=pclose(fp);
439       
440        if(status!=0)
441        {
442                return nullptr;
443        }
444       
445        stringstream ss;
446       
447        int low,high;
448        float batt;
449       
450        ss.str(buff);
451        ss>>hex>>low>>high;
452       
453        batt=((high<<8) + low)/1000.0f;
454       
455        cout<<"Batt: "<<batt<<endl;
456       
457        Message * msg = new Message(RBW_MSG_BATTERY_STATUS);
458        msg->data["value"]=new MessageDataFloat(batt); 
459        RoboCore::GetCore()->window->SendMessage(nullptr,nullptr,msg);
460
461        RoboCore::GetCore()->is_battery_requesting=false;
462       
463        return nullptr;
464}
465
466void RoboCore::GetBattery()
467{
468        if(!is_battery_requesting)
469        {
470                is_battery_requesting=true;
471                pthread_t aux_thread;
472                pthread_create(&aux_thread,nullptr,auxGetBattery,nullptr);
473                               
474        }
475}
476
477
478int RoboCore::CustomEventsDispatch()
479{
480        int events = 0;
481       
482        while(gtk_events_pending())
483        {
484                events++;
485                gtk_main_iteration();
486        }
487       
488        return events;
489       
490}
491
492void RoboCore::OnMessage(BaseWindow * window,Layer * layer, Widget * widget,MessageEvent * event)
493{
494
495        Message * msg = event->msg;
496       
497        //catch battery status messages
498        if(msg->id==RBW_MSG_BATTERY_STATUS)
499        {
500                float value;
501               
502                value = (static_cast<MessageDataFloat *>(msg->data["value"]))->value;
503               
504                this->window->SetBattery(value);
505        }
506}
Note: See TracBrowser for help on using the repository browser.