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

Last change on this file since 89 was 89, checked in by jrpelegrina, 3 years ago

First release to xenial

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