source: level2/competencies/SamgarV1Modules/expressive-behaviours-uh/L1PioneerDisplay.cpp @ 1706

Revision 459, 4.5 KB checked in by KDucasse, 10 years ago (diff)
Line 
1/*
2        PROGRAM:        Level 1 - Hardware Level of Pioneer's Display Module (communicating with SSC-32 card) 
3        Author :        K . L. Koay
4        Date   :    05 May 2010
5
6*/
7#include <Windows.h>
8#include <iostream>
9#include <cstdlib>
10#include <SamgarMainClass.h>
11
12using namespace std;
13//using namespace yarp;
14
15int main(int argc, char* argv[])
16{
17        Network yarp;            //name                                 //Category      //subcategory   
18        SamgarModule Display("PioneerBaseLights",       "Display",      "Lights", SamgarModule::run);
19        Display.AddPortS("L1BIn");
20        Bottle BehaviourIn;
21        char command[100];
22
23        printf("\n argc = %d, argv[1] = %s", argc, argv[1]);
24//Setting up serial port in Linux   ----------------------
25//      int fd = open("/dev/ttyUSB2",O_RDWR|O_NDELAY); //opening a port to the lights and arms
26//      if (fd==-1)     {
27//              printf("\nfd = Error opening USB2\n");
28//      }
29//      tcgetattr(fd,&options);
30//      cfsetispeed(&options,B115200);
31//      cfsetospeed(&options,B115200);
32//      options.c_cflag &= ~PARENB; //no parity bit
33//      options.c_cflag &= ~CSTOPB; //one stop bit
34//      options.c_cflag &= ~CSIZE; //clear out the current data size setting
35//      options.c_cflag |= CS8; //8-bits per work
36//      options.c_cc[VMIN]=0;
37//      options.c_cc[VTIME]=1;
38//      options.c_cflag |= (CLOCAL | CREAD); 
39  /*
40  //turn off echo, canonical mode, extended processing, signals
41  options.c_lflag &= ~(ECHO | ICANON | IEXTEN | ISIG);
42  //turn off break sig, cr->nl, parity off, 8 bit strip, flow control
43  options.c_iflag &= ~(BRKINT | ICRNL | INPCK | ISTRIP | IXON);
44  //clear size, turn off parity bit
45  options.c_cflag &= ~(CSIZE | PARENB);
46  //set size to 8 bits
47  options.c_cflag |= CS8;
48  //turn output processing off
49  options.c_oflag &= ~(OPOST);
50  //Set time and bytes to read at once
51  options.c_cc[VTIME] = 0;
52  options.c_cc[VMIN] = 0;  */
53//      if(tcsetattr(fd, TCSAFLUSH, &options)!=0){
54//              printf("\n Error applying setting USB2 \n");
55//      }
56//=====================================================
57
58HANDLE fileHandle;
59/*fileHandle = (/*argv[1] "COM4", GENERIC_READ | GENERIC_WRITE, 0, OPEN_EXISTING, 0, 0);
60if (fileHandle == INVALID_HANDLE_VALUE){
61        printf("\n Error open com port");
62}
63*/
64
65DCB dcb;
66//FillMemory(&dcb, sizeof(dcb),0);
67fileHandle = CreateFile(/*argv[1]*/"COM8", GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, 0, NULL);
68if (fileHandle == INVALID_HANDLE_VALUE){
69        printf("\n Error open com port");
70}
71//SetupComm(fileHandle, 128, 128);
72
73GetCommState(fileHandle, &dcb);
74dcb.DCBlength = sizeof(dcb);
75dcb.BaudRate = 115200;
76dcb.ByteSize = 8;
77dcb.Parity = NOPARITY;
78dcb.StopBits = ONESTOPBIT;
79dcb.fBinary = TRUE;
80dcb.fParity = TRUE;
81
82/*SetCommState(fileHandle, &dcb);
83if(!BuildCommDCB("115200,n,8,1",&dcb))
84{       printf("\n Error setting port");
85        return false;
86}*/
87
88if(!SetCommState(fileHandle, &dcb)){
89        printf("\n Error applying setting to port\n");
90        return false;
91}
92printf("\n Applied Setting to port %s",argv[1]);
93
94if(!SetupComm(fileHandle,1024,1024)){
95        printf("\n Error setting port buffer");
96        return false;
97}
98printf("\n Comm port buffer set to 1024");
99
100COMMTIMEOUTS cmt;
101cmt.ReadIntervalTimeout =1000;
102cmt.ReadTotalTimeoutMultiplier = 1000;
103cmt.ReadTotalTimeoutConstant = 1000;
104cmt.WriteTotalTimeoutMultiplier =1000;
105cmt.WriteTotalTimeoutConstant = 1000;
106
107if(!SetCommTimeouts(fileHandle, &cmt)){
108        printf("error setting timeouts for fileHandle");
109}
110
111DWORD read = -1;
112//ReadFile(fileHandle, data, size, &read, NULL)
113DWORD write = -1;
114//WriteFile(fileHandle, data, size, &write,NULL)
115
116printf("\nReady.... ");
117
118int Front, Left, Back, Right;
119
120while (1){
121        if (Display.GetBottleData("L1BIn", &BehaviourIn, SamgarModule::interupt)) {
122                printf("got something in\n");
123                Front   = BehaviourIn.get(0).asInt()*(2500/100);
124                Left    = BehaviourIn.get(1).asInt()*(2500/100);
125                Back    = BehaviourIn.get(2).asInt()*(2500/100);
126                Right   = BehaviourIn.get(3).asInt()*(2500/100);
127                sprintf(command,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r", Front,Left,Back,Right);
128                /*
129                sprintf(command,"#12 P%d #13 P%d #14 P%d #15 P%d T100\r",
130                        (int)(BehaviourIn.get(0).asInt()*(2500/100)),
131                        (int)(BehaviourIn.get(1).asInt()*(2500/100)),
132                        (int)(BehaviourIn.get(2).asInt()*(2500/100)),
133                        (int)(BehaviourIn.get(3).asInt()*(2500/100)) );
134                */
135                printf("Command is %s\n",command);
136                WriteFile(fileHandle, command, sizeof(command), &write, NULL);
137                BehaviourIn.clear();
138                //printf("\n %s ",command);
139                sprintf(command,"Q\r");
140                while (WriteFile(fileHandle, command, sizeof(command), &write, NULL)=='.'){}
141        }
142        Display.SucceedFail(true,0);
143  }   
144}
Note: See TracBrowser for help on using the repository browser.