Alvaro Cassinelli / Mbed 2 deprecated laserUI

Laser Sensing Display for UI interfaces in the real world

Dependencies:   mbed

Fork of skinGames_forktest by Alvaro Cassinelli

main.cpp@41:74e24a0e6e50, 2013-10-16 (annotated)

Committer:
mbedalvaro
Date:
Wed Oct 16 17:26:13 2013 +0000
Revision:
41:74e24a0e6e50
Parent:
40:3ba2b0ea9f33
Child:
42:5f21a710ebc5
it compiles, but I cannot send osc packets, and I am looking at a black square

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbedalvaro 40:3ba2b0ea9f33 1 /*
mbedalvaro 40:3ba2b0ea9f33 2
mbedalvaro 40:3ba2b0ea9f33 3 - NOTE: I could instantiate at least 960 points! (as 30 objects containing each 32 points)
mbedalvaro 40:3ba2b0ea9f33 4 ... but then again I could not make only 36 1-point object on a grid? what's going on? an object takes MUCH MORE MEMORY than just
mbedalvaro 40:3ba2b0ea9f33 5 the trajectory and 3d vector array... but where?
mbedalvaro 40:3ba2b0ea9f33 6 */
mbedalvaro 40:3ba2b0ea9f33 7
mbedalvaro 40:3ba2b0ea9f33 8 #include "mbed.h"
mbedalvaro 40:3ba2b0ea9f33 9 #include "mbedOSC.h"
mbedalvaro 40:3ba2b0ea9f33 10
mbedalvaro 41:74e24a0e6e50 11 // mbed IP address (server):
mbedalvaro 40:3ba2b0ea9f33 12 #ifdef DHCP
mbedalvaro 40:3ba2b0ea9f33 13 EthernetNetIf eth;
mbedalvaro 40:3ba2b0ea9f33 14 #else
mbedalvaro 40:3ba2b0ea9f33 15 EthernetNetIf eth(
mbedalvaro 41:74e24a0e6e50 16 IpAddr(10,0,0,2), //IP Address of the mbed
mbedalvaro 40:3ba2b0ea9f33 17 IpAddr(255,255,255,0), //Network Mask
mbedalvaro 40:3ba2b0ea9f33 18 IpAddr(10,0,0,1), //Gateway
mbedalvaro 40:3ba2b0ea9f33 19 IpAddr(10,0,0,1) //DNS
mbedalvaro 40:3ba2b0ea9f33 20 );
mbedalvaro 40:3ba2b0ea9f33 21 #endif
mbedalvaro 40:3ba2b0ea9f33 22
mbedalvaro 41:74e24a0e6e50 23 //uint8_t serverMac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
mbedalvaro 41:74e24a0e6e50 24 uint8_t serverIp[] = { 10, 0, 0, 2 };
mbedalvaro 41:74e24a0e6e50 25 int serverPort = 10000;
mbedalvaro 41:74e24a0e6e50 26
mbedalvaro 41:74e24a0e6e50 27 //uint8_t destIp[] = {10, 0, 0, 3};
mbedalvaro 41:74e24a0e6e50 28 uint8_t destIp[] = {255, 255, 255, 255}; // broadcast, so we can use several computers for sound, etc
mbedalvaro 41:74e24a0e6e50 29 int destPort = 12000;
mbedalvaro 41:74e24a0e6e50 30
mbedalvaro 41:74e24a0e6e50 31 char *topAddress="/mbed";
mbedalvaro 41:74e24a0e6e50 32 char *subAddress[3]={ "/test1" , "/test2" , "/test3" };
mbedalvaro 41:74e24a0e6e50 33
mbedalvaro 40:3ba2b0ea9f33 34 OSCMessage recMes;
mbedalvaro 40:3ba2b0ea9f33 35 OSCMessage sendMes;
mbedalvaro 40:3ba2b0ea9f33 36
mbedalvaro 40:3ba2b0ea9f33 37 OSCClass osc;
mbedalvaro 41:74e24a0e6e50 38 //OSCClass osc(&recMes); // instantiate OSC communication object, and set the receiver container from the OSC packets
mbedalvaro 40:3ba2b0ea9f33 39 void processOSC(UDPSocketEvent e);
mbedalvaro 40:3ba2b0ea9f33 40
mbedalvaro 40:3ba2b0ea9f33 41
mbedalvaro 40:3ba2b0ea9f33 42 #include "WrapperFunctions.h"
mbedalvaro 40:3ba2b0ea9f33 43
mbedalvaro 40:3ba2b0ea9f33 44 // The following is because I still did not "wrap" the LaserRenderer methods (so we have to use the object lsr directly, for instance to set a matrix: lsr.setIdentityPose()...)
mbedalvaro 40:3ba2b0ea9f33 45 #include "LaserRenderer.h"
mbedalvaro 40:3ba2b0ea9f33 46 extern LaserRenderer lsr;
mbedalvaro 40:3ba2b0ea9f33 47
mbedalvaro 40:3ba2b0ea9f33 48 // for tests:
mbedalvaro 40:3ba2b0ea9f33 49 extern Scene scene;
mbedalvaro 40:3ba2b0ea9f33 50 extern laserSensingDisplay lsd;
mbedalvaro 40:3ba2b0ea9f33 51
mbedalvaro 40:3ba2b0ea9f33 52 // =======================================================
mbedalvaro 40:3ba2b0ea9f33 53
mbedalvaro 40:3ba2b0ea9f33 54 // Timers:
mbedalvaro 40:3ba2b0ea9f33 55 Timer t; // we can read in ms or us
mbedalvaro 40:3ba2b0ea9f33 56
mbedalvaro 40:3ba2b0ea9f33 57 // Tickers:
mbedalvaro 40:3ba2b0ea9f33 58 //Ticker timerForRendering; // now in the WrapperFunctions.cpp file
mbedalvaro 40:3ba2b0ea9f33 59 //Ticker timerForSendingData; // better use a timer, so as not to interrupt the exact laser display ticker
mbedalvaro 40:3ba2b0ea9f33 60 unsigned long lastTimeCreated;
mbedalvaro 40:3ba2b0ea9f33 61 unsigned long lastTimeSensed;
mbedalvaro 40:3ba2b0ea9f33 62
mbedalvaro 40:3ba2b0ea9f33 63 // ======================================================================================================================
mbedalvaro 40:3ba2b0ea9f33 64 // DEFAULT CALIBRATION MATRICES (Extrinsics and Intrinsics).
mbedalvaro 40:3ba2b0ea9f33 65 // -> would be better to load them from a system file!! (TO DO)
mbedalvaro 40:3ba2b0ea9f33 66 // Note: last row is completed automatically in the loadExtrinsicMatrix and loadProjMatrix methods
mbedalvaro 40:3ba2b0ea9f33 67 // Extrinsics of usb camera to projector (note: calibration was in cm, so the translation vector is in cm)
mbedalvaro 40:3ba2b0ea9f33 68 float E1[12] = {9.8946330287356954e-01, -5.0979372852926171e-03, -1.4469410251272136e-01, 5.8356271792647112e+00,
mbedalvaro 40:3ba2b0ea9f33 69 -4.7536907255693065e-03, 9.9769720674821438e-01, -6.7658599389111715e-02, -6.0821695799283804e+00,
mbedalvaro 40:3ba2b0ea9f33 70 1.4470582120637832e-01, 6.7633532232509466e-02, 9.8716053943963022e-01, -8.2545447546057940e+00
mbedalvaro 40:3ba2b0ea9f33 71 };
mbedalvaro 40:3ba2b0ea9f33 72
mbedalvaro 40:3ba2b0ea9f33 73 // 9.9540692993883650e-01, -8.5578652572728450e-03, -9.5350966287597705e-02, 2.2256570914518332e+00,
mbedalvaro 40:3ba2b0ea9f33 74 // 1.4872282274740087e-02, 9.9772789680840523e-01, 6.5710418886328711e-02, -6.9310510316834923e+00,
mbedalvaro 40:3ba2b0ea9f33 75 // 9.4571978141945845e-02, -6.6826692814433777e-02, 9.9327253766416224e-01, 1.0699927478132011e+01
mbedalvaro 40:3ba2b0ea9f33 76 //};
mbedalvaro 40:3ba2b0ea9f33 77
mbedalvaro 40:3ba2b0ea9f33 78
mbedalvaro 40:3ba2b0ea9f33 79 // INTRINSICS of laser projector:
mbedalvaro 40:3ba2b0ea9f33 80 float K1[6] = { 5.9957148864529627e+03, 0.0000000e+000, 2.8869455045867635e+03,
mbedalvaro 40:3ba2b0ea9f33 81 0.0000000e+000, 5.9647451141521778e+03, 2.9005238051336592e+03
mbedalvaro 40:3ba2b0ea9f33 82 };
mbedalvaro 40:3ba2b0ea9f33 83
mbedalvaro 40:3ba2b0ea9f33 84 // 7.0776317995422105e+03, 0., 2.4203185896583996e+03,
mbedalvaro 40:3ba2b0ea9f33 85 // 0., 7.2889265345161484e+03, 1.7718110988625751e+03};
mbedalvaro 40:3ba2b0ea9f33 86
mbedalvaro 40:3ba2b0ea9f33 87 // SCALE FACTOR for the SCAN:
mbedalvaro 40:3ba2b0ea9f33 88 float scaleFactorProjector1 = 1.0; //scale factor for the projector image (the scan is, say, 600x600, but the resolution is 4096x4096)
mbedalvaro 40:3ba2b0ea9f33 89
mbedalvaro 40:3ba2b0ea9f33 90 // ======================================================================================================================
mbedalvaro 40:3ba2b0ea9f33 91 // LASER TERMINAL:
mbedalvaro 40:3ba2b0ea9f33 92 // We assume that the "terminal" is operating on an a4 page. Units are in cm.
mbedalvaro 40:3ba2b0ea9f33 93 V2 cursorPosition(0,0);
mbedalvaro 40:3ba2b0ea9f33 94
mbedalvaro 40:3ba2b0ea9f33 95 void createTextScene();
mbedalvaro 40:3ba2b0ea9f33 96 float angle;
mbedalvaro 40:3ba2b0ea9f33 97 string textToDisplay = "HELLO";
mbedalvaro 40:3ba2b0ea9f33 98 float fontWidth = 2.5, fontHeight = 2.5; // if calibration is in cm, then this is in cm.
mbedalvaro 40:3ba2b0ea9f33 99
mbedalvaro 40:3ba2b0ea9f33 100
mbedalvaro 40:3ba2b0ea9f33 101 // ==============================================================================================================
mbedalvaro 40:3ba2b0ea9f33 102 // Some global functions and variables (find a better way to integrate this!)
mbedalvaro 40:3ba2b0ea9f33 103 extern "C" void mbed_reset();
mbedalvaro 40:3ba2b0ea9f33 104
mbedalvaro 40:3ba2b0ea9f33 105 void createSceneTest();
mbedalvaro 41:74e24a0e6e50 106 void setOrthographicView();
mbedalvaro 40:3ba2b0ea9f33 107
mbedalvaro 40:3ba2b0ea9f33 108 void interpretData();
mbedalvaro 40:3ba2b0ea9f33 109 void processOSC();
mbedalvaro 40:3ba2b0ea9f33 110 void processSerial();
mbedalvaro 40:3ba2b0ea9f33 111
mbedalvaro 40:3ba2b0ea9f33 112 int touchedTimes = 0;
mbedalvaro 40:3ba2b0ea9f33 113 enum sensingModes {NO_SENSING, SENSING_WHOLE_SCENE, SENSING_PER_OBJECT};
mbedalvaro 40:3ba2b0ea9f33 114 sensingModes currentSensingMode=SENSING_PER_OBJECT;//NO_SENSING;
mbedalvaro 40:3ba2b0ea9f33 115
mbedalvaro 40:3ba2b0ea9f33 116 // ==============================================================================================================
mbedalvaro 40:3ba2b0ea9f33 117 int main()
mbedalvaro 40:3ba2b0ea9f33 118 {
mbedalvaro 40:3ba2b0ea9f33 119
mbedalvaro 40:3ba2b0ea9f33 120 t.start();
mbedalvaro 40:3ba2b0ea9f33 121 lastTimeCreated = t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 122 lastTimeSensed = t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 123 angle = 0;
mbedalvaro 40:3ba2b0ea9f33 124
mbedalvaro 40:3ba2b0ea9f33 125 // Setup:
mbedalvaro 40:3ba2b0ea9f33 126 // (1) Hardware init (laser powers, positions...):
mbedalvaro 40:3ba2b0ea9f33 127 IO.init();
mbedalvaro 40:3ba2b0ea9f33 128
mbedalvaro 40:3ba2b0ea9f33 129 // OSC initialization:
mbedalvaro 41:74e24a0e6e50 130 // Set the Ethernet port:
mbedalvaro 41:74e24a0e6e50 131 printf("Setting up...\r\n");
mbedalvaro 40:3ba2b0ea9f33 132 EthernetErr ethErr = eth.setup();
mbedalvaro 40:3ba2b0ea9f33 133 if (ethErr) {
mbedalvaro 41:74e24a0e6e50 134 printf("Error %d in setup.\r\n", ethErr);
mbedalvaro 40:3ba2b0ea9f33 135 return -1;
mbedalvaro 40:3ba2b0ea9f33 136 }
mbedalvaro 41:74e24a0e6e50 137 printf("Setup OK\r\n");
mbedalvaro 40:3ba2b0ea9f33 138
mbedalvaro 41:74e24a0e6e50 139 //(1) Sending message:
mbedalvaro 40:3ba2b0ea9f33 140 // Set IP and Port:
mbedalvaro 40:3ba2b0ea9f33 141 sendMes.setIp( destIp );
mbedalvaro 40:3ba2b0ea9f33 142 sendMes.setPort( destPort );
mbedalvaro 40:3ba2b0ea9f33 143 // Set data:
mbedalvaro 41:74e24a0e6e50 144 sendMes.setTopAddress(topAddress);
mbedalvaro 40:3ba2b0ea9f33 145
mbedalvaro 40:3ba2b0ea9f33 146 //setting osc functionnality:
mbedalvaro 40:3ba2b0ea9f33 147 //(2) Receiving:
mbedalvaro 40:3ba2b0ea9f33 148 // recMes.setIp( serverIp ); // not needed?
mbedalvaro 40:3ba2b0ea9f33 149 osc.setReceiveMessage(&recMes); // this sets the receiver container for the OSC packets (we can avoid doing this if we use osc.getMessage() to get messages)
mbedalvaro 40:3ba2b0ea9f33 150 osc.begin(serverPort, &processOSC); // binds the upd (osc) messages to an arbitrary listening port ("server" port), and callback function
mbedalvaro 41:74e24a0e6e50 151
mbedalvaro 40:3ba2b0ea9f33 152 //===================================================================
mbedalvaro 40:3ba2b0ea9f33 153
mbedalvaro 40:3ba2b0ea9f33 154 /*
mbedalvaro 40:3ba2b0ea9f33 155 //EXAMPLE 1: simple geometry ======================================================
mbedalvaro 40:3ba2b0ea9f33 156 // (1) Calibration matrices (Projection, and extrinsics - if needed):
mbedalvaro 40:3ba2b0ea9f33 157 // Typically, the projection matrix is set from the PC side, or loaded by default (from file system).
mbedalvaro 40:3ba2b0ea9f33 158 lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being.
mbedalvaro 40:3ba2b0ea9f33 159
mbedalvaro 40:3ba2b0ea9f33 160 // (2) Create the scene in "local" coordinates:
mbedalvaro 40:3ba2b0ea9f33 161 createSceneTest(); // Create a default scene (for tests)
mbedalvaro 40:3ba2b0ea9f33 162
mbedalvaro 40:3ba2b0ea9f33 163 // (3) Set a default GLOBAL POSE and PROJECTION MATRIX (if we changed it) for rendering and start displaying:
mbedalvaro 40:3ba2b0ea9f33 164 // - Using the current K (that correct for the center of the camera too!):
mbedalvaro 40:3ba2b0ea9f33 165 //lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 166 //lsr.translate(0, 0, 2000);
mbedalvaro 40:3ba2b0ea9f33 167 // - or doing orthoprojection:
mbedalvaro 40:3ba2b0ea9f33 168 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 169 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 170 lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0);
mbedalvaro 40:3ba2b0ea9f33 171 lsr.rotateZ(45);
mbedalvaro 40:3ba2b0ea9f33 172 drawScene();
mbedalvaro 40:3ba2b0ea9f33 173
mbedalvaro 40:3ba2b0ea9f33 174 // *** AND IMPORTANT *** the FIRST time, we need to set the display engine (ie, attach the interrupt). This is not needed afterwards, ONLY if we change the scene.
mbedalvaro 40:3ba2b0ea9f33 175 lsr.startDisplay();
mbedalvaro 40:3ba2b0ea9f33 176
mbedalvaro 40:3ba2b0ea9f33 177 // (4) Main loop:
mbedalvaro 40:3ba2b0ea9f33 178 // Change the POSE when necessary (also called "viewing transformation" or "global modelview" in OpenGL jargon), render and display:
mbedalvaro 40:3ba2b0ea9f33 179 while (1) {
mbedalvaro 40:3ba2b0ea9f33 180
mbedalvaro 40:3ba2b0ea9f33 181 if (pc.readable())
mbedalvaro 40:3ba2b0ea9f33 182 processSerial(); // here, the pose can be changed by a pc command
mbedalvaro 40:3ba2b0ea9f33 183 hardwareKnobs();
mbedalvaro 40:3ba2b0ea9f33 184
mbedalvaro 40:3ba2b0ea9f33 185 if (0) { //((t.read_ms()-lastTimeCreated)>10) {
mbedalvaro 40:3ba2b0ea9f33 186 // for tests:
mbedalvaro 40:3ba2b0ea9f33 187 myLed2 = !myLed2;
mbedalvaro 40:3ba2b0ea9f33 188
mbedalvaro 40:3ba2b0ea9f33 189 angle += 1;
mbedalvaro 40:3ba2b0ea9f33 190 if (angle > 360)
mbedalvaro 40:3ba2b0ea9f33 191 angle = 0;
mbedalvaro 40:3ba2b0ea9f33 192
mbedalvaro 40:3ba2b0ea9f33 193 // Set the current global pose:
mbedalvaro 40:3ba2b0ea9f33 194 // lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
mbedalvaro 40:3ba2b0ea9f33 195 // lsr.translate(0, 0, 1000);
mbedalvaro 40:3ba2b0ea9f33 196 // lsr.rotateZ(angle);
mbedalvaro 40:3ba2b0ea9f33 197 // lsr.rotateX(2.5 * angle);
mbedalvaro 40:3ba2b0ea9f33 198
mbedalvaro 40:3ba2b0ea9f33 199 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 200 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 201 lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0);
mbedalvaro 40:3ba2b0ea9f33 202 lsr.rotateZ(angle);
mbedalvaro 40:3ba2b0ea9f33 203 // lsr.rotateY( angle);
mbedalvaro 40:3ba2b0ea9f33 204
mbedalvaro 40:3ba2b0ea9f33 205 // Then, render (and display) the whole scene (note: we DEFINITELY don't want to call this all the time, only when the RT matrix changes)
mbedalvaro 40:3ba2b0ea9f33 206 drawScene();
mbedalvaro 40:3ba2b0ea9f33 207
mbedalvaro 40:3ba2b0ea9f33 208 lastTimeCreated = t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 209 } // end auto-change pose loop
mbedalvaro 40:3ba2b0ea9f33 210
mbedalvaro 40:3ba2b0ea9f33 211 if ((t.read_ms() - lastTimeSensed) > 50) { // check data sensing every xx ms
mbedalvaro 40:3ba2b0ea9f33 212 // Check objects and change colors or other properties as a function of the touch:
mbedalvaro 40:3ba2b0ea9f33 213 // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h
mbedalvaro 40:3ba2b0ea9f33 214 // (note: we assume we are drawing the text).
mbedalvaro 40:3ba2b0ea9f33 215 // There are two ways to query for an object input: using the ID, or from its pointer (scene->objectArray[i]) if we know it. This is faster.
mbedalvaro 40:3ba2b0ea9f33 216 for (int i = 0; i < scene.totalObjects(); i++) {
mbedalvaro 40:3ba2b0ea9f33 217 BaseObject* ptrObj = scene.objectArray[i];
mbedalvaro 40:3ba2b0ea9f33 218 if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData()
mbedalvaro 40:3ba2b0ea9f33 219 ptrObj->setColor(0x02);
mbedalvaro 40:3ba2b0ea9f33 220
mbedalvaro 40:3ba2b0ea9f33 221 // char str[15];
mbedalvaro 40:3ba2b0ea9f33 222 // sprintf(str, "%d", touchedTimes);
mbedalvaro 40:3ba2b0ea9f33 223 // textToDisplay=string(str);//string(itoa(touchedTimes));
mbedalvaro 40:3ba2b0ea9f33 224 // touchedTimes++;
mbedalvaro 40:3ba2b0ea9f33 225 // createTextScene();
mbedalvaro 40:3ba2b0ea9f33 226
mbedalvaro 40:3ba2b0ea9f33 227 } else
mbedalvaro 40:3ba2b0ea9f33 228 ptrObj->setColor(0x01);
mbedalvaro 40:3ba2b0ea9f33 229 }
mbedalvaro 40:3ba2b0ea9f33 230 lastTimeSensed = t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 231 } // end sensing loop
mbedalvaro 40:3ba2b0ea9f33 232
mbedalvaro 40:3ba2b0ea9f33 233 } // "end" infite loop
mbedalvaro 40:3ba2b0ea9f33 234 // =================================
mbedalvaro 40:3ba2b0ea9f33 235 */
mbedalvaro 40:3ba2b0ea9f33 236
mbedalvaro 40:3ba2b0ea9f33 237 // EXAMPLE 2: 3d text ======================================================
mbedalvaro 40:3ba2b0ea9f33 238
mbedalvaro 40:3ba2b0ea9f33 239 // (1) Calibration matrices:
mbedalvaro 40:3ba2b0ea9f33 240 // Typically, the projection and extrinsic matrix is set from the PC side, or loaded by default (from file system).
mbedalvaro 40:3ba2b0ea9f33 241 lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being.
mbedalvaro 40:3ba2b0ea9f33 242 lsr.loadExtrinsicsMatrix(E1);
mbedalvaro 40:3ba2b0ea9f33 243
mbedalvaro 40:3ba2b0ea9f33 244 // (2) Create the scene in "local" coordinates:
mbedalvaro 41:74e24a0e6e50 245 //createTextScene(); // Create a default scene (for tests)
mbedalvaro 41:74e24a0e6e50 246 setOrthographicView();
mbedalvaro 40:3ba2b0ea9f33 247
mbedalvaro 40:3ba2b0ea9f33 248 // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
mbedalvaro 40:3ba2b0ea9f33 249 lsr.loadProjMatrix(K1, scaleFactorProjector1);
mbedalvaro 40:3ba2b0ea9f33 250 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 251 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 40:3ba2b0ea9f33 252
mbedalvaro 40:3ba2b0ea9f33 253 // DEFAULT position in CAMERA coordinates (calibration was in cm):
mbedalvaro 40:3ba2b0ea9f33 254 lsr.translate(0, 0, 130);
mbedalvaro 40:3ba2b0ea9f33 255
mbedalvaro 40:3ba2b0ea9f33 256 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
mbedalvaro 40:3ba2b0ea9f33 257
mbedalvaro 40:3ba2b0ea9f33 258 // *** AND IMPORTANT *** the FIRST time, we need to set the display engine (ie, attach the interrupt). This is not needed afterwards, ONLY if we change the scene.
mbedalvaro 40:3ba2b0ea9f33 259 startDisplay();
mbedalvaro 40:3ba2b0ea9f33 260
mbedalvaro 40:3ba2b0ea9f33 261 // (4) Main loop:
mbedalvaro 40:3ba2b0ea9f33 262 while(true) {
mbedalvaro 40:3ba2b0ea9f33 263
mbedalvaro 40:3ba2b0ea9f33 264 // =========== Query hardware events (knobs, ethernet, serial port) ============
mbedalvaro 40:3ba2b0ea9f33 265 Net::poll(); // This polls the network connection for new activity, without keeping on calling this you won't receive any OSC!
mbedalvaro 40:3ba2b0ea9f33 266 if (pc.readable()) processSerial(); // here, the pose can be changed by a pc command
mbedalvaro 40:3ba2b0ea9f33 267 hardwareKnobs();
mbedalvaro 40:3ba2b0ea9f33 268 // =============================================================================
mbedalvaro 40:3ba2b0ea9f33 269
mbedalvaro 40:3ba2b0ea9f33 270 // Automatic change of pose for tests
mbedalvaro 40:3ba2b0ea9f33 271 if (0) {//(t.read_ms()-lastTimeCreated)>20) {
mbedalvaro 40:3ba2b0ea9f33 272 angle+=.2;
mbedalvaro 40:3ba2b0ea9f33 273 if (angle>360) angle=0;
mbedalvaro 40:3ba2b0ea9f33 274
mbedalvaro 40:3ba2b0ea9f33 275 lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
mbedalvaro 40:3ba2b0ea9f33 276 lsr.flipY();
mbedalvaro 40:3ba2b0ea9f33 277 lsr.translate(0,0,1000);
mbedalvaro 40:3ba2b0ea9f33 278 lsr.rotateZ(angle);
mbedalvaro 40:3ba2b0ea9f33 279 //lsr.rotateX(angle);
mbedalvaro 40:3ba2b0ea9f33 280
mbedalvaro 40:3ba2b0ea9f33 281 drawScene();
mbedalvaro 40:3ba2b0ea9f33 282
mbedalvaro 40:3ba2b0ea9f33 283 lastTimeCreated=t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 284 }
mbedalvaro 40:3ba2b0ea9f33 285
mbedalvaro 40:3ba2b0ea9f33 286 if ((currentSensingMode!=NO_SENSING)&&((t.read_ms()-lastTimeSensed)>80)) { // check data sensing every xx ms
mbedalvaro 40:3ba2b0ea9f33 287 // Check objects and change colors or other properties as a function of the touch:
mbedalvaro 40:3ba2b0ea9f33 288 // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h
mbedalvaro 40:3ba2b0ea9f33 289 // (note: we assume we are drawing the text).
mbedalvaro 40:3ba2b0ea9f33 290 // There are two ways to query for an object input: using the ID, or from its pointer (scene->objectArray[i]) if we know it. This is faster.
mbedalvaro 40:3ba2b0ea9f33 291 switch(currentSensingMode) {
mbedalvaro 40:3ba2b0ea9f33 292 case SENSING_WHOLE_SCENE: // Sensing whole scene ----------
mbedalvaro 40:3ba2b0ea9f33 293 if (scene.sense()) {
mbedalvaro 40:3ba2b0ea9f33 294
mbedalvaro 40:3ba2b0ea9f33 295 // COUNTING:
mbedalvaro 40:3ba2b0ea9f33 296 // char str[15];
mbedalvaro 40:3ba2b0ea9f33 297 // sprintf(str, "%d", touchedTimes);
mbedalvaro 40:3ba2b0ea9f33 298 // textToDisplay=string(str);//string(itoa(touchedTimes));
mbedalvaro 40:3ba2b0ea9f33 299 // touchedTimes++;
mbedalvaro 40:3ba2b0ea9f33 300 // createTextScene();
mbedalvaro 40:3ba2b0ea9f33 301 // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
mbedalvaro 40:3ba2b0ea9f33 302 // lsr.loadProjMatrix(K1, scaleFactorProjector1);
mbedalvaro 40:3ba2b0ea9f33 303 // lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 304 // lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 40:3ba2b0ea9f33 305 // This means that the current coordinate frame is the CAMERA, and calibration was in cm:
mbedalvaro 40:3ba2b0ea9f33 306 // lsr.translate(0, 0, 100);
mbedalvaro 40:3ba2b0ea9f33 307 ////lsr.flipY();
mbedalvaro 40:3ba2b0ea9f33 308 // drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
mbedalvaro 40:3ba2b0ea9f33 309 // }
mbedalvaro 40:3ba2b0ea9f33 310 }
mbedalvaro 40:3ba2b0ea9f33 311 break;
mbedalvaro 40:3ba2b0ea9f33 312 case SENSING_PER_OBJECT: // Per-object sensing and action:
mbedalvaro 40:3ba2b0ea9f33 313 for (int i=0; i<textToDisplay.length(); i++) {
mbedalvaro 40:3ba2b0ea9f33 314 BaseObject* ptrObj=scene.objectArray[i];
mbedalvaro 40:3ba2b0ea9f33 315 if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData()
mbedalvaro 40:3ba2b0ea9f33 316 ptrObj->setColor(0x02); // make it green
mbedalvaro 40:3ba2b0ea9f33 317
mbedalvaro 40:3ba2b0ea9f33 318 // for fun: let's rotate THIS letter:
mbedalvaro 40:3ba2b0ea9f33 319 //lsr.pushPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 320 //lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 321 //lsr.rotateX(180);
mbedalvaro 40:3ba2b0ea9f33 322 //transformObject(ptrObj);
mbedalvaro 40:3ba2b0ea9f33 323 //lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 324 // we need to redraw the scene (ie, reproject), or at least this object:
mbedalvaro 40:3ba2b0ea9f33 325 //lsr.renderObject(ptrObj, lsr.RT);
mbedalvaro 40:3ba2b0ea9f33 326
mbedalvaro 40:3ba2b0ea9f33 327 // or "highlight" the object by putting "cartouche" around it object (note: do this only if it did NOT have
mbedalvaro 40:3ba2b0ea9f33 328 //addCartoucheObject(ptrObj);
mbedalvaro 40:3ba2b0ea9f33 329
mbedalvaro 40:3ba2b0ea9f33 330 //pc.putc(i+48);
mbedalvaro 40:3ba2b0ea9f33 331
mbedalvaro 40:3ba2b0ea9f33 332 } else
mbedalvaro 40:3ba2b0ea9f33 333 ptrObj->setColor(0x01); // make it blue
mbedalvaro 40:3ba2b0ea9f33 334 }
mbedalvaro 40:3ba2b0ea9f33 335 break;
mbedalvaro 40:3ba2b0ea9f33 336 default:
mbedalvaro 40:3ba2b0ea9f33 337 break;
mbedalvaro 40:3ba2b0ea9f33 338 }
mbedalvaro 40:3ba2b0ea9f33 339 lastTimeSensed=t.read_ms();
mbedalvaro 40:3ba2b0ea9f33 340 } // end sensing
mbedalvaro 40:3ba2b0ea9f33 341
mbedalvaro 40:3ba2b0ea9f33 342 } // infinite while loop
mbedalvaro 40:3ba2b0ea9f33 343 // =================================
mbedalvaro 40:3ba2b0ea9f33 344 }
mbedalvaro 40:3ba2b0ea9f33 345
mbedalvaro 41:74e24a0e6e50 346 void setOrthographicView() {
mbedalvaro 41:74e24a0e6e50 347 clearScene();
mbedalvaro 41:74e24a0e6e50 348 // set color:
mbedalvaro 41:74e24a0e6e50 349 lsr.setColor(0x02); // green
mbedalvaro 41:74e24a0e6e50 350 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 41:74e24a0e6e50 351
mbedalvaro 41:74e24a0e6e50 352 lsr.setOrthoProjection();
mbedalvaro 41:74e24a0e6e50 353 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 41:74e24a0e6e50 354 }
mbedalvaro 41:74e24a0e6e50 355
mbedalvaro 40:3ba2b0ea9f33 356 void createSceneTest()
mbedalvaro 40:3ba2b0ea9f33 357 {
mbedalvaro 40:3ba2b0ea9f33 358 lsr.pushPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 359
mbedalvaro 40:3ba2b0ea9f33 360 // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
mbedalvaro 40:3ba2b0ea9f33 361 clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)
mbedalvaro 40:3ba2b0ea9f33 362
mbedalvaro 40:3ba2b0ea9f33 363 // (2) Set displaying attributes (colors and local pose):
mbedalvaro 40:3ba2b0ea9f33 364 // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
mbedalvaro 40:3ba2b0ea9f33 365 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
mbedalvaro 40:3ba2b0ea9f33 366 // (b) set color:
mbedalvaro 40:3ba2b0ea9f33 367 lsr.setColor(0x02);
mbedalvaro 40:3ba2b0ea9f33 368
mbedalvaro 40:3ba2b0ea9f33 369 // Add a first object: using openGL-like syntaxis (here, a closed square):
mbedalvaro 40:3ba2b0ea9f33 370 // begin(1); // start creating object with identifier = 1
mbedalvaro 40:3ba2b0ea9f33 371 // vertex(10,10,1);
mbedalvaro 40:3ba2b0ea9f33 372 // vertex(10,100,1);
mbedalvaro 40:3ba2b0ea9f33 373 // vertex(100,100,1);
mbedalvaro 40:3ba2b0ea9f33 374 // vertex(10,100,1);
mbedalvaro 40:3ba2b0ea9f33 375 // vertex(10,10,1); //
mbedalvaro 40:3ba2b0ea9f33 376 // end();
mbedalvaro 40:3ba2b0ea9f33 377
mbedalvaro 40:3ba2b0ea9f33 378 // Add another objects using "object primitives":
mbedalvaro 40:3ba2b0ea9f33 379 //begin(2);
mbedalvaro 40:3ba2b0ea9f33 380 //lsr.translate(-100,-100,0); // recenter the square
mbedalvaro 40:3ba2b0ea9f33 381 //square(200, 10); // this square has a corner in (0,0), and by default, z=0
mbedalvaro 40:3ba2b0ea9f33 382 //end();
mbedalvaro 40:3ba2b0ea9f33 383
mbedalvaro 40:3ba2b0ea9f33 384 // A "cube" (with one corner at the origin):
mbedalvaro 40:3ba2b0ea9f33 385 //begin(3);
mbedalvaro 40:3ba2b0ea9f33 386 //lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
mbedalvaro 40:3ba2b0ea9f33 387 //lsr.translate(100,-100,-100); // recenter the cube (no need to push pop because we are seting the identity above)
mbedalvaro 40:3ba2b0ea9f33 388 //lsr.rotateY(30);
mbedalvaro 40:3ba2b0ea9f33 389 //cube(200,10); // cube(200, 20);
mbedalvaro 40:3ba2b0ea9f33 390 //end();
mbedalvaro 40:3ba2b0ea9f33 391
mbedalvaro 40:3ba2b0ea9f33 392 // A grid:
mbedalvaro 40:3ba2b0ea9f33 393 // IMPORTANT: a grid is a multi-object built, and DOES NOT NEED to be between a begin and end
mbedalvaro 40:3ba2b0ea9f33 394 lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
mbedalvaro 40:3ba2b0ea9f33 395 lsr.translate(-500, -500, 0);
mbedalvaro 40:3ba2b0ea9f33 396 grid(1000, 1000, 5, 5, 1); //grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
mbedalvaro 40:3ba2b0ea9f33 397 //gridCircles(300, 250, 6, 5, 10, 6); // gridCircles(float sizeX, float sizeY, int nx, int ny, float radius, int nbpointsCircle)
mbedalvaro 40:3ba2b0ea9f33 398
mbedalvaro 40:3ba2b0ea9f33 399 // A circle:
mbedalvaro 40:3ba2b0ea9f33 400 //begin(4);
mbedalvaro 40:3ba2b0ea9f33 401 //circle(200, 100);
mbedalvaro 40:3ba2b0ea9f33 402 //end();
mbedalvaro 40:3ba2b0ea9f33 403
mbedalvaro 40:3ba2b0ea9f33 404 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 40:3ba2b0ea9f33 405 // rendering is done in the drawScene() function.
mbedalvaro 40:3ba2b0ea9f33 406
mbedalvaro 40:3ba2b0ea9f33 407 // pc.printf("Number of objects in the scene: %d\n", scene.totalObjects());
mbedalvaro 40:3ba2b0ea9f33 408 // pc.printf("Number of points in the scene: %d\n", scene.totalPoints());
mbedalvaro 40:3ba2b0ea9f33 409
mbedalvaro 40:3ba2b0ea9f33 410 lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 411 }
mbedalvaro 40:3ba2b0ea9f33 412
mbedalvaro 40:3ba2b0ea9f33 413 void createTextScene()
mbedalvaro 40:3ba2b0ea9f33 414 {
mbedalvaro 40:3ba2b0ea9f33 415 lsr.pushPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 416 //This function just creates text in local coordinates. We have two options: to create it as a unique object, or as separate objects (one per letter).
mbedalvaro 40:3ba2b0ea9f33 417 // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height):
mbedalvaro 40:3ba2b0ea9f33 418
mbedalvaro 40:3ba2b0ea9f33 419 // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
mbedalvaro 40:3ba2b0ea9f33 420 clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)
mbedalvaro 40:3ba2b0ea9f33 421
mbedalvaro 40:3ba2b0ea9f33 422 // (2) Set displaying attributes (colors and pose):
mbedalvaro 40:3ba2b0ea9f33 423 // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
mbedalvaro 40:3ba2b0ea9f33 424 lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
mbedalvaro 40:3ba2b0ea9f33 425 //lsr.flipY();
mbedalvaro 40:3ba2b0ea9f33 426 //lsr.translate(2,6,0);
mbedalvaro 40:3ba2b0ea9f33 427
mbedalvaro 40:3ba2b0ea9f33 428 lsr.flipY();
mbedalvaro 40:3ba2b0ea9f33 429 lsr.flipX();
mbedalvaro 40:3ba2b0ea9f33 430 lsr.translate(-15,-10,0);
mbedalvaro 40:3ba2b0ea9f33 431
mbedalvaro 40:3ba2b0ea9f33 432 // (b) set current color:
mbedalvaro 40:3ba2b0ea9f33 433 lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...).
mbedalvaro 40:3ba2b0ea9f33 434
mbedalvaro 40:3ba2b0ea9f33 435 // A string as a UNIQUE object:
mbedalvaro 40:3ba2b0ea9f33 436 // begin(1); // start creating an object with index 1
mbedalvaro 40:3ba2b0ea9f33 437 // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates
mbedalvaro 40:3ba2b0ea9f33 438 // string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight);
mbedalvaro 40:3ba2b0ea9f33 439 //end();
mbedalvaro 40:3ba2b0ea9f33 440
mbedalvaro 40:3ba2b0ea9f33 441 // A string as a *collection* of object-letters:
mbedalvaro 40:3ba2b0ea9f33 442 //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0);
mbedalvaro 40:3ba2b0ea9f33 443 for (unsigned short i = 0; i < textToDisplay.length(); i++) {
mbedalvaro 40:3ba2b0ea9f33 444 char ch = textToDisplay.at(i);
mbedalvaro 40:3ba2b0ea9f33 445 if (ch != ' ') {
mbedalvaro 40:3ba2b0ea9f33 446 begin(i);
mbedalvaro 40:3ba2b0ea9f33 447 letter3d(ch, fontWidth, fontHeight);
mbedalvaro 40:3ba2b0ea9f33 448 end();
mbedalvaro 40:3ba2b0ea9f33 449 }
mbedalvaro 40:3ba2b0ea9f33 450 lsr.translate(1.1*fontWidth, 0, 0);
mbedalvaro 40:3ba2b0ea9f33 451 }
mbedalvaro 40:3ba2b0ea9f33 452
mbedalvaro 40:3ba2b0ea9f33 453 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 40:3ba2b0ea9f33 454 // rendering is done in the drawScene() function.
mbedalvaro 40:3ba2b0ea9f33 455
mbedalvaro 40:3ba2b0ea9f33 456 lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 457
mbedalvaro 40:3ba2b0ea9f33 458 }
mbedalvaro 40:3ba2b0ea9f33 459
mbedalvaro 40:3ba2b0ea9f33 460 // ================= INTERPRET COMMAND =========================
mbedalvaro 40:3ba2b0ea9f33 461 // NOTE: the following arrays are GLOBAL (used in processOSC and processSerial, as well as in interpretData function):
mbedalvaro 40:3ba2b0ea9f33 462 // max of two addresses (top and sub), of a max length of 24 characters:
mbedalvaro 40:3ba2b0ea9f33 463 char address[2][24];
mbedalvaro 40:3ba2b0ea9f33 464 //long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs)
mbedalvaro 40:3ba2b0ea9f33 465 int data[2]; // this is to have -1 as NO DATA, to detect errors.
mbedalvaro 40:3ba2b0ea9f33 466 int numArgs;
mbedalvaro 40:3ba2b0ea9f33 467
mbedalvaro 40:3ba2b0ea9f33 468 // An auxiliary buffer to store parameters data, or matrices, or points to load as vertices.
mbedalvaro 40:3ba2b0ea9f33 469 // Note: for matrices (pose, projection), the maximum size we need is 12 floats, but as we will also receive vertices, let's give enough space (assuming
mbedalvaro 40:3ba2b0ea9f33 470 // we can send 3d points, this means 3 floats per point; let's limit the number of points per object to 128 (a lot)
mbedalvaro 40:3ba2b0ea9f33 471 float auxDataBuffer[128];
mbedalvaro 40:3ba2b0ea9f33 472 int auxDataBuffer_index=0;
mbedalvaro 40:3ba2b0ea9f33 473
mbedalvaro 40:3ba2b0ea9f33 474 //================== OSC messageReceivedCallback function =====================
mbedalvaro 40:3ba2b0ea9f33 475 //============= RECEIVE OSC COMMANDS =========================
mbedalvaro 40:3ba2b0ea9f33 476 // This is the callback function called when there are packets on the listening socket. It is not nice to have it
mbedalvaro 40:3ba2b0ea9f33 477 // here, but for the time being having a "wrapping global" is the simplest solution (we cannot pass a member-function pointer
mbedalvaro 40:3ba2b0ea9f33 478 // as handler to the upd object).
mbedalvaro 40:3ba2b0ea9f33 479 void processOSC(UDPSocketEvent e) {
mbedalvaro 40:3ba2b0ea9f33 480 osc.onUDPSocketEvent(e);
mbedalvaro 40:3ba2b0ea9f33 481
mbedalvaro 40:3ba2b0ea9f33 482 if (osc.newMessage) {
mbedalvaro 41:74e24a0e6e50 483
mbedalvaro 41:74e24a0e6e50 484 pc.putc('A');
mbedalvaro 41:74e24a0e6e50 485
mbedalvaro 40:3ba2b0ea9f33 486 // Acquire the addresses and arguments and put them in the GLOBAL variables to be processed by the interpretData function (common to serial and OSC):
mbedalvaro 40:3ba2b0ea9f33 487 strcpy(address[0],"");
mbedalvaro 40:3ba2b0ea9f33 488 strcpy(address[1],"");
mbedalvaro 40:3ba2b0ea9f33 489 for (int i=0; i<recMes.getAddressNum(); i++) strcpy(address[i],recMes.getAddress(i)); // NOTE: up to the rest of the program to check if address[1] is really not null
mbedalvaro 40:3ba2b0ea9f33 490 // Acquire data:
mbedalvaro 40:3ba2b0ea9f33 491 numArgs=recMes.getArgNum();
mbedalvaro 40:3ba2b0ea9f33 492 data[0]=-1;
mbedalvaro 40:3ba2b0ea9f33 493 data[1]=-1;
mbedalvaro 40:3ba2b0ea9f33 494 for (int i=0; i<numArgs; i++) data[i]=(int)recMes.getArgInt(i);
mbedalvaro 40:3ba2b0ea9f33 495
mbedalvaro 40:3ba2b0ea9f33 496 // Finally, interpret the command or data:
mbedalvaro 41:74e24a0e6e50 497 interpretData();
mbedalvaro 41:74e24a0e6e50 498
mbedalvaro 40:3ba2b0ea9f33 499 }
mbedalvaro 40:3ba2b0ea9f33 500 }
mbedalvaro 40:3ba2b0ea9f33 501
mbedalvaro 40:3ba2b0ea9f33 502
mbedalvaro 40:3ba2b0ea9f33 503
mbedalvaro 40:3ba2b0ea9f33 504 //interpretData(const char& address[2][], const int& data[2]) {
mbedalvaro 40:3ba2b0ea9f33 505 void interpretData() {
mbedalvaro 40:3ba2b0ea9f33 506
mbedalvaro 40:3ba2b0ea9f33 507 // ========== DATA (all this is a hack because my OSC class does not accept BUNDLES...) =============
mbedalvaro 40:3ba2b0ea9f33 508 // Add data to auxiliary data buffer:
mbedalvaro 40:3ba2b0ea9f33 509 if (!strcmp(address[0], "data")) {
mbedalvaro 40:3ba2b0ea9f33 510 // numArgs is the number of arguments (in my OSC version this is max 2):
mbedalvaro 40:3ba2b0ea9f33 511 for (int i=0; i<numArgs; i++) auxDataBuffer[auxDataBuffer_index] = data[i];
mbedalvaro 40:3ba2b0ea9f33 512 auxDataBuffer_index+=numArgs;
mbedalvaro 40:3ba2b0ea9f33 513 // note: the index will be reset to 0 or whatever needed when a command USING data from the "d" message is received.
mbedalvaro 40:3ba2b0ea9f33 514 }
mbedalvaro 40:3ba2b0ea9f33 515
mbedalvaro 40:3ba2b0ea9f33 516 // ========== COMMANDS (using data on auxDataBuffer or directly as arguments =========================
mbedalvaro 40:3ba2b0ea9f33 517
mbedalvaro 40:3ba2b0ea9f33 518 else if ( !strcmp(address[0], "mbedReset" ) ) mbed_reset();
mbedalvaro 40:3ba2b0ea9f33 519
mbedalvaro 41:74e24a0e6e50 520 else if ( !strcmp(address[0], "testPower" ) ) {
mbedalvaro 41:74e24a0e6e50 521 // TO DO
mbedalvaro 41:74e24a0e6e50 522 }
mbedalvaro 41:74e24a0e6e50 523
mbedalvaro 40:3ba2b0ea9f33 524 // Enable/disable projection:
mbedalvaro 40:3ba2b0ea9f33 525 else if ( !strcmp(address[0], "stopDisplay" )) stopDisplay();
mbedalvaro 40:3ba2b0ea9f33 526 else if (!strcmp(address[0], "resumeDisplay")) resumeDisplay();
mbedalvaro 40:3ba2b0ea9f33 527 else if (!strcmp(address[0], "showLimits")) {
mbedalvaro 40:3ba2b0ea9f33 528 int value=data[0]; // argument is the number of seconds for display
mbedalvaro 40:3ba2b0ea9f33 529 if (value!=-1) {
mbedalvaro 40:3ba2b0ea9f33 530 showLimitsMirrors(50, value);
mbedalvaro 40:3ba2b0ea9f33 531 }
mbedalvaro 40:3ba2b0ea9f33 532 }
mbedalvaro 40:3ba2b0ea9f33 533 else if (!strcmp(address[0], "setSensingMode")) {
mbedalvaro 40:3ba2b0ea9f33 534 int value=data[0];
mbedalvaro 40:3ba2b0ea9f33 535 if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
mbedalvaro 40:3ba2b0ea9f33 536 switch(value) {
mbedalvaro 40:3ba2b0ea9f33 537 case 0:
mbedalvaro 40:3ba2b0ea9f33 538 currentSensingMode=NO_SENSING;
mbedalvaro 40:3ba2b0ea9f33 539 break;
mbedalvaro 40:3ba2b0ea9f33 540 case 1:
mbedalvaro 40:3ba2b0ea9f33 541 currentSensingMode=SENSING_WHOLE_SCENE;
mbedalvaro 40:3ba2b0ea9f33 542 break;
mbedalvaro 40:3ba2b0ea9f33 543 case 2:
mbedalvaro 40:3ba2b0ea9f33 544 currentSensingMode=SENSING_PER_OBJECT;
mbedalvaro 40:3ba2b0ea9f33 545 break;
mbedalvaro 40:3ba2b0ea9f33 546 default:
mbedalvaro 40:3ba2b0ea9f33 547 break;
mbedalvaro 40:3ba2b0ea9f33 548 }
mbedalvaro 40:3ba2b0ea9f33 549 }
mbedalvaro 40:3ba2b0ea9f33 550 }
mbedalvaro 40:3ba2b0ea9f33 551
mbedalvaro 40:3ba2b0ea9f33 552 // Change state machine color (objects created from this point on will have the state machine color)
mbedalvaro 40:3ba2b0ea9f33 553 else if (!strcmp(address[0], "setStateColor")) { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
mbedalvaro 40:3ba2b0ea9f33 554 int value=data[0];
mbedalvaro 40:3ba2b0ea9f33 555 if (value!=-1) lsr.setColor(value);
mbedalvaro 40:3ba2b0ea9f33 556 }
mbedalvaro 40:3ba2b0ea9f33 557
mbedalvaro 40:3ba2b0ea9f33 558 // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
mbedalvaro 40:3ba2b0ea9f33 559 else if (!strcmp(address[0], "setAllObjectsColor")) { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
mbedalvaro 40:3ba2b0ea9f33 560 int value=data[0];
mbedalvaro 40:3ba2b0ea9f33 561 if (value!=-1) changeColorScene(value);
mbedalvaro 40:3ba2b0ea9f33 562 }
mbedalvaro 40:3ba2b0ea9f33 563
mbedalvaro 40:3ba2b0ea9f33 564
mbedalvaro 40:3ba2b0ea9f33 565 // =================== CREATION and DESTRUCTION of objects =========================================================
mbedalvaro 40:3ba2b0ea9f33 566 else if (!strcmp(address[0], "clearScene")) {
mbedalvaro 40:3ba2b0ea9f33 567 clearScene();
mbedalvaro 40:3ba2b0ea9f33 568 }
mbedalvaro 40:3ba2b0ea9f33 569
mbedalvaro 40:3ba2b0ea9f33 570 else if (!strcmp(address[0], "deleteObject")) { // second address is object identifier (a string that will be converted
mbedalvaro 40:3ba2b0ea9f33 571 int idobject=atoi(address[1]);
mbedalvaro 40:3ba2b0ea9f33 572 deleteObject(idobject);
mbedalvaro 40:3ba2b0ea9f33 573 }
mbedalvaro 40:3ba2b0ea9f33 574
mbedalvaro 40:3ba2b0ea9f33 575 // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
mbedalvaro 40:3ba2b0ea9f33 576 // been translated previously - this may be optional, we could first send the pose, then the points...)
mbedalvaro 40:3ba2b0ea9f33 577 // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
mbedalvaro 40:3ba2b0ea9f33 578 // setting Z to 1, or by setting the projection matrix to a unit diagonal but the last term in the diagonal is set to 0.
mbedalvaro 40:3ba2b0ea9f33 579 // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
mbedalvaro 40:3ba2b0ea9f33 580 // NOTE: the following string of ASCII characters is assumed to have been input:
mbedalvaro 40:3ba2b0ea9f33 581 // posX#posY#sizeX#sizeY#nx#ny#:
mbedalvaro 40:3ba2b0ea9f33 582 // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
mbedalvaro 40:3ba2b0ea9f33 583 // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
mbedalvaro 40:3ba2b0ea9f33 584 else if (!strcmp(address[0], "gridScene")) {
mbedalvaro 40:3ba2b0ea9f33 585 // Data is now ordered in the auxDataBuffer array:
mbedalvaro 40:3ba2b0ea9f33 586 float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
mbedalvaro 40:3ba2b0ea9f33 587 auxDataBuffer_index=0; // to restart loading the auxiliary buffer 'auxDataBuffer'
mbedalvaro 40:3ba2b0ea9f33 588 clearScene();
mbedalvaro 40:3ba2b0ea9f33 589 // set color:
mbedalvaro 40:3ba2b0ea9f33 590 lsr.setColor(0x02); // green
mbedalvaro 40:3ba2b0ea9f33 591 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 40:3ba2b0ea9f33 592
mbedalvaro 40:3ba2b0ea9f33 593 // set local pose matrix to identity:
mbedalvaro 40:3ba2b0ea9f33 594 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 595 // translate to the required position:
mbedalvaro 40:3ba2b0ea9f33 596 lsr.translate(posX, posY, 0); // since the projection and pose are going to be set
mbedalvaro 40:3ba2b0ea9f33 597 // to Identity (projection identity will automatically make the projection orthographic), this means that these
mbedalvaro 40:3ba2b0ea9f33 598 // values are in "laser projector pixels" (in fact angles).
mbedalvaro 40:3ba2b0ea9f33 599 // Create the collection of dot objects:
mbedalvaro 40:3ba2b0ea9f33 600 // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
mbedalvaro 40:3ba2b0ea9f33 601 grid(sizeX, sizeY, nx, ny, 1);
mbedalvaro 40:3ba2b0ea9f33 602 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 40:3ba2b0ea9f33 603 // rendering is done in the drawScene() function.
mbedalvaro 40:3ba2b0ea9f33 604
mbedalvaro 40:3ba2b0ea9f33 605 // Now we can render the scene once:
mbedalvaro 40:3ba2b0ea9f33 606 // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
mbedalvaro 40:3ba2b0ea9f33 607 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 608 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 40:3ba2b0ea9f33 609 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
mbedalvaro 40:3ba2b0ea9f33 610 }
mbedalvaro 40:3ba2b0ea9f33 611 // NOTE: now that the grid is set, we don't need to do it again when changing the pose: JUST SEND THE POSE MATRIX and things will be re-rendered!
mbedalvaro 40:3ba2b0ea9f33 612
mbedalvaro 40:3ba2b0ea9f33 613 // This means we received "2D" vertex data (z assumed 0) to create A SINGLE SCENE out of these points (we will create a single object)
mbedalvaro 40:3ba2b0ea9f33 614 // using orthographic projection.
mbedalvaro 40:3ba2b0ea9f33 615 // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
mbedalvaro 40:3ba2b0ea9f33 616 // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
mbedalvaro 40:3ba2b0ea9f33 617 // NOTE: the POSE is reset to ID, but we could use the current pose (i.e., send it first by the computer). But for the time being, and in order to modify the
mbedalvaro 40:3ba2b0ea9f33 618 // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
mbedalvaro 40:3ba2b0ea9f33 619 else if (!strcmp(address[0], "arbitraryGridScene")) {
mbedalvaro 40:3ba2b0ea9f33 620 // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y). The argument to this message is the IDENTIFIER of the object:
mbedalvaro 40:3ba2b0ea9f33 621 // The present value of auxDataBuffer_index contains the number of points x 2
mbedalvaro 40:3ba2b0ea9f33 622 clearScene();
mbedalvaro 40:3ba2b0ea9f33 623 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 624 //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
mbedalvaro 40:3ba2b0ea9f33 625 lsr.setColor(0x03); // blue+green
mbedalvaro 40:3ba2b0ea9f33 626 IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 40:3ba2b0ea9f33 627
mbedalvaro 40:3ba2b0ea9f33 628 // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
mbedalvaro 40:3ba2b0ea9f33 629 for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
mbedalvaro 40:3ba2b0ea9f33 630 begin(i);
mbedalvaro 40:3ba2b0ea9f33 631 for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
mbedalvaro 40:3ba2b0ea9f33 632 vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
mbedalvaro 40:3ba2b0ea9f33 633 end();
mbedalvaro 40:3ba2b0ea9f33 634 }
mbedalvaro 40:3ba2b0ea9f33 635 updateScene();
mbedalvaro 40:3ba2b0ea9f33 636 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 40:3ba2b0ea9f33 637 // properly called lsr.pushPoseMatrix())
mbedalvaro 40:3ba2b0ea9f33 638 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 639 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 640 drawScene();
mbedalvaro 40:3ba2b0ea9f33 641 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 642 }
mbedalvaro 40:3ba2b0ea9f33 643
mbedalvaro 40:3ba2b0ea9f33 644 // This means we received 2D vertex array to add an object assuming it's planar (we will create an object with all points having z=0 in current pose):
mbedalvaro 40:3ba2b0ea9f33 645 else if (!strcmp(address[0], "createObject2d")) {
mbedalvaro 40:3ba2b0ea9f33 646 int IDObject=data[1];
mbedalvaro 40:3ba2b0ea9f33 647 if (IDObject!=-1) {
mbedalvaro 40:3ba2b0ea9f33 648 // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
mbedalvaro 40:3ba2b0ea9f33 649 // The present value of auxDataBuffer_index contains the number of points x 3
mbedalvaro 40:3ba2b0ea9f33 650 //lsr.pushPoseMatrix(); // we will render on the current pose
mbedalvaro 40:3ba2b0ea9f33 651 //clearScene();
mbedalvaro 40:3ba2b0ea9f33 652 //lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 653 //lsr.setColor(0x02); // we will render in current color
mbedalvaro 40:3ba2b0ea9f33 654 begin(IDObject);
mbedalvaro 40:3ba2b0ea9f33 655 for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
mbedalvaro 40:3ba2b0ea9f33 656 vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],0); // z=0 for 2d objects (in current pose)
mbedalvaro 40:3ba2b0ea9f33 657 }
mbedalvaro 40:3ba2b0ea9f33 658 end();
mbedalvaro 40:3ba2b0ea9f33 659 updateScene();
mbedalvaro 40:3ba2b0ea9f33 660 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 40:3ba2b0ea9f33 661 // properly called lsr.pushPoseMatrix())
mbedalvaro 40:3ba2b0ea9f33 662 //lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 663 //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 40:3ba2b0ea9f33 664 //lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 665 drawScene();
mbedalvaro 40:3ba2b0ea9f33 666 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 667 }
mbedalvaro 40:3ba2b0ea9f33 668 }
mbedalvaro 40:3ba2b0ea9f33 669
mbedalvaro 40:3ba2b0ea9f33 670 // This means we received 3D vertex data to create an object.
mbedalvaro 40:3ba2b0ea9f33 671 // It will be rendered in the CURRENT pose.
mbedalvaro 40:3ba2b0ea9f33 672 else if (!strcmp(address[0], "createObject3d")) {
mbedalvaro 40:3ba2b0ea9f33 673 int IDObject=data[1];
mbedalvaro 40:3ba2b0ea9f33 674 if (IDObject!=-1) {
mbedalvaro 40:3ba2b0ea9f33 675 // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
mbedalvaro 40:3ba2b0ea9f33 676 // The present value of auxDataBuffer_index contains the number of points x 3
mbedalvaro 40:3ba2b0ea9f33 677 //lsr.pushPoseMatrix(); // we will render on the current pose
mbedalvaro 40:3ba2b0ea9f33 678 //clearScene();
mbedalvaro 40:3ba2b0ea9f33 679 //lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 680 //lsr.setColor(0x02); // we will render in current color
mbedalvaro 40:3ba2b0ea9f33 681 begin(IDObject);
mbedalvaro 40:3ba2b0ea9f33 682 for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
mbedalvaro 40:3ba2b0ea9f33 683 vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],
mbedalvaro 40:3ba2b0ea9f33 684 auxDataBuffer[3 * i + 2]);
mbedalvaro 40:3ba2b0ea9f33 685 }
mbedalvaro 40:3ba2b0ea9f33 686 end();
mbedalvaro 40:3ba2b0ea9f33 687 updateScene();
mbedalvaro 40:3ba2b0ea9f33 688 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 40:3ba2b0ea9f33 689 // properly called lsr.pushPoseMatrix())
mbedalvaro 40:3ba2b0ea9f33 690 //lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 691 //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 40:3ba2b0ea9f33 692 //lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 693 drawScene();
mbedalvaro 40:3ba2b0ea9f33 694 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 695 }
mbedalvaro 40:3ba2b0ea9f33 696 }
mbedalvaro 40:3ba2b0ea9f33 697
mbedalvaro 40:3ba2b0ea9f33 698 // Create text:
mbedalvaro 40:3ba2b0ea9f33 699 else if (!strcmp(address[0], "createTextObject")) {
mbedalvaro 40:3ba2b0ea9f33 700 //NOTE: the actual text will be in the second address, and the argument will be the ID of this new text object:
mbedalvaro 40:3ba2b0ea9f33 701 textToDisplay = address[1];
mbedalvaro 40:3ba2b0ea9f33 702 int IDObject=data[1];
mbedalvaro 40:3ba2b0ea9f33 703 if (IDObject!=-1) {
mbedalvaro 40:3ba2b0ea9f33 704 begin(IDObject);
mbedalvaro 40:3ba2b0ea9f33 705 string3d(textToDisplay, fontWidth*textToDisplay.length(), fontHeight);
mbedalvaro 40:3ba2b0ea9f33 706 end();
mbedalvaro 40:3ba2b0ea9f33 707 updateScene();
mbedalvaro 40:3ba2b0ea9f33 708 drawScene();
mbedalvaro 40:3ba2b0ea9f33 709 }
mbedalvaro 40:3ba2b0ea9f33 710 }
mbedalvaro 40:3ba2b0ea9f33 711
mbedalvaro 40:3ba2b0ea9f33 712
mbedalvaro 40:3ba2b0ea9f33 713 // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
mbedalvaro 40:3ba2b0ea9f33 714 else if (!strcmp(address[0], "poseMatrix")) { // when receiving this, it means that the WHOLE matrix data (4x4 values) have been sent in row/column format
mbedalvaro 40:3ba2b0ea9f33 715 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer'
mbedalvaro 40:3ba2b0ea9f33 716 // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
mbedalvaro 40:3ba2b0ea9f33 717 lsr.setIdentityPose(); // RT=ID
mbedalvaro 40:3ba2b0ea9f33 718 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 40:3ba2b0ea9f33 719 lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates
mbedalvaro 40:3ba2b0ea9f33 720
mbedalvaro 40:3ba2b0ea9f33 721 drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry nor update the renderer (updateScene), just project)
mbedalvaro 40:3ba2b0ea9f33 722 }
mbedalvaro 40:3ba2b0ea9f33 723
mbedalvaro 40:3ba2b0ea9f33 724 // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
mbedalvaro 40:3ba2b0ea9f33 725 else if (!strcmp(address[0], "projectionMatrix")) {// when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
mbedalvaro 40:3ba2b0ea9f33 726 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 727 // store in projection matrix:
mbedalvaro 40:3ba2b0ea9f33 728 lsr.loadProjMatrix(auxDataBuffer, 1.0);
mbedalvaro 40:3ba2b0ea9f33 729
mbedalvaro 40:3ba2b0ea9f33 730 drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
mbedalvaro 40:3ba2b0ea9f33 731 }
mbedalvaro 40:3ba2b0ea9f33 732
mbedalvaro 40:3ba2b0ea9f33 733 //(e) TERMINATOR indicating the data was for the extrinsic matrix:
mbedalvaro 40:3ba2b0ea9f33 734 else if (!strcmp(address[0], "extrinsicMatrix")) { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
mbedalvaro 40:3ba2b0ea9f33 735 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 736 // store in projection matrix:
mbedalvaro 40:3ba2b0ea9f33 737 lsr.loadExtrinsicsMatrix(auxDataBuffer);
mbedalvaro 40:3ba2b0ea9f33 738
mbedalvaro 40:3ba2b0ea9f33 739 drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
mbedalvaro 40:3ba2b0ea9f33 740 }
mbedalvaro 40:3ba2b0ea9f33 741 }
mbedalvaro 40:3ba2b0ea9f33 742
mbedalvaro 40:3ba2b0ea9f33 743 //============= RECEIVE SERIAL COMMANDS =========================
mbedalvaro 40:3ba2b0ea9f33 744 // String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC):
mbedalvaro 40:3ba2b0ea9f33 745 // TO DO: use string objects!
mbedalvaro 40:3ba2b0ea9f33 746 char receivedStringData[24]; // note: an integer is two bytes long, represented with a maximum of 5 digits, but we may send floats or unsigned int...
mbedalvaro 40:3ba2b0ea9f33 747 int indexStringData = 0; //position of the byte in the string
mbedalvaro 40:3ba2b0ea9f33 748
mbedalvaro 40:3ba2b0ea9f33 749 // String to store COMMAND WORDS (not used yet):
mbedalvaro 40:3ba2b0ea9f33 750 char stringCommand[24];
mbedalvaro 40:3ba2b0ea9f33 751 int indexStringCommand = 0;
mbedalvaro 40:3ba2b0ea9f33 752 bool commandReady = false; // will become true when receiving the byte 0 (i.e. the '/0' string terminator)
mbedalvaro 40:3ba2b0ea9f33 753
mbedalvaro 40:3ba2b0ea9f33 754 void processSerial()
mbedalvaro 40:3ba2b0ea9f33 755 {
mbedalvaro 40:3ba2b0ea9f33 756 while (pc.readable() > 0) {
mbedalvaro 40:3ba2b0ea9f33 757 char incomingByte = pc.getc();
mbedalvaro 40:3ba2b0ea9f33 758
mbedalvaro 40:3ba2b0ea9f33 759 // For test:
mbedalvaro 40:3ba2b0ea9f33 760 // pc.putc(incomingByte);
mbedalvaro 40:3ba2b0ea9f33 761
mbedalvaro 40:3ba2b0ea9f33 762 // (a) TEXT to display or NUMBERS (in ASCII) to convert to numerical value (rem: limited set of characters for the time being):
mbedalvaro 40:3ba2b0ea9f33 763 // Note: numbers are float, so we need also the "." and "-"
mbedalvaro 40:3ba2b0ea9f33 764 if ( (incomingByte >= '0' && incomingByte <= '9') // numbers
mbedalvaro 40:3ba2b0ea9f33 765 || (incomingByte >= 'A' && incomingByte <= 'Z') // capital letters
mbedalvaro 40:3ba2b0ea9f33 766 || (incomingByte >= 'a' && incomingByte <= 'z') // small letters (in fact, will be used for other symbols)
mbedalvaro 40:3ba2b0ea9f33 767 || (incomingByte == ' ') // space
mbedalvaro 40:3ba2b0ea9f33 768 || (incomingByte == '-') // minus sign
mbedalvaro 40:3ba2b0ea9f33 769 || (incomingByte == '.') // decimal point
mbedalvaro 40:3ba2b0ea9f33 770 ) {
mbedalvaro 40:3ba2b0ea9f33 771 receivedStringData[indexStringData] = incomingByte;
mbedalvaro 40:3ba2b0ea9f33 772 indexStringData++;
mbedalvaro 40:3ba2b0ea9f33 773 }
mbedalvaro 40:3ba2b0ea9f33 774
mbedalvaro 40:3ba2b0ea9f33 775 // Enable/disable projection:
mbedalvaro 40:3ba2b0ea9f33 776 else if (incomingByte == '}') {
mbedalvaro 40:3ba2b0ea9f33 777 stopDisplay();
mbedalvaro 40:3ba2b0ea9f33 778 } else if (incomingByte == '{') {
mbedalvaro 40:3ba2b0ea9f33 779 resumeDisplay();
mbedalvaro 40:3ba2b0ea9f33 780 }
mbedalvaro 40:3ba2b0ea9f33 781
mbedalvaro 40:3ba2b0ea9f33 782 // Show maximum excursion of mirrors:
mbedalvaro 40:3ba2b0ea9f33 783 else if (incomingByte == '*') {
mbedalvaro 40:3ba2b0ea9f33 784 showLimitsMirrors(50, 120);
mbedalvaro 40:3ba2b0ea9f33 785 }
mbedalvaro 40:3ba2b0ea9f33 786
mbedalvaro 40:3ba2b0ea9f33 787
mbedalvaro 40:3ba2b0ea9f33 788 else if (incomingByte == '+') {
mbedalvaro 40:3ba2b0ea9f33 789 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 790 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 791 switch(atoi(receivedStringData)) {
mbedalvaro 40:3ba2b0ea9f33 792 case 0:
mbedalvaro 40:3ba2b0ea9f33 793 currentSensingMode=NO_SENSING;
mbedalvaro 40:3ba2b0ea9f33 794 break;
mbedalvaro 40:3ba2b0ea9f33 795 case 1:
mbedalvaro 40:3ba2b0ea9f33 796 currentSensingMode=SENSING_WHOLE_SCENE;
mbedalvaro 40:3ba2b0ea9f33 797 break;
mbedalvaro 40:3ba2b0ea9f33 798 case 2:
mbedalvaro 40:3ba2b0ea9f33 799 currentSensingMode=SENSING_PER_OBJECT;
mbedalvaro 40:3ba2b0ea9f33 800 break;
mbedalvaro 40:3ba2b0ea9f33 801 default:
mbedalvaro 40:3ba2b0ea9f33 802 break;
mbedalvaro 40:3ba2b0ea9f33 803 }
mbedalvaro 40:3ba2b0ea9f33 804 }
mbedalvaro 40:3ba2b0ea9f33 805
mbedalvaro 40:3ba2b0ea9f33 806 // Performs a scan of the surface and send all the data serially (N>, with N the resolution of the scan)
mbedalvaro 40:3ba2b0ea9f33 807 else if (incomingByte == '>') {
mbedalvaro 40:3ba2b0ea9f33 808 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 809 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 810 //pc.printf("scan command issued¥n");
mbedalvaro 40:3ba2b0ea9f33 811 scanSerial(atoi(receivedStringData));
mbedalvaro 40:3ba2b0ea9f33 812 }
mbedalvaro 40:3ba2b0ea9f33 813
mbedalvaro 40:3ba2b0ea9f33 814 // Recompute the LUT table
mbedalvaro 40:3ba2b0ea9f33 815 else if (incomingByte == '<') {
mbedalvaro 40:3ba2b0ea9f33 816 recomputeLookUpTable();
mbedalvaro 40:3ba2b0ea9f33 817 }
mbedalvaro 40:3ba2b0ea9f33 818
mbedalvaro 40:3ba2b0ea9f33 819 // Change state machine color (objects created from this point on will have the state machine color)
mbedalvaro 40:3ba2b0ea9f33 820 else if (incomingByte == '(') { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
mbedalvaro 40:3ba2b0ea9f33 821 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 822 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 823 // convert to integer and set the state machine color:
mbedalvaro 40:3ba2b0ea9f33 824 lsr.setColor(atoi(receivedStringData));
mbedalvaro 40:3ba2b0ea9f33 825 }
mbedalvaro 40:3ba2b0ea9f33 826
mbedalvaro 40:3ba2b0ea9f33 827 // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
mbedalvaro 40:3ba2b0ea9f33 828 else if (incomingByte == ')') { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
mbedalvaro 40:3ba2b0ea9f33 829 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 830 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 831 // convert to integer and set the color:
mbedalvaro 40:3ba2b0ea9f33 832 changeColorScene(atoi(receivedStringData));
mbedalvaro 40:3ba2b0ea9f33 833 }
mbedalvaro 40:3ba2b0ea9f33 834
mbedalvaro 40:3ba2b0ea9f33 835
mbedalvaro 40:3ba2b0ea9f33 836 // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
mbedalvaro 40:3ba2b0ea9f33 837 // been translated previously - this may be optional, we could first send the pose, then the points...)
mbedalvaro 40:3ba2b0ea9f33 838 // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
mbedalvaro 40:3ba2b0ea9f33 839 // setting Z to 1, or by setting the projection matrix to a unit diagonal but the last term in the diagonal is set to 0.
mbedalvaro 40:3ba2b0ea9f33 840 // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
mbedalvaro 40:3ba2b0ea9f33 841 // NOTE: the following string of ASCII characters is assumed to have been input:
mbedalvaro 40:3ba2b0ea9f33 842 // posX#posY#sizeX#sizeY#nx#ny#:
mbedalvaro 40:3ba2b0ea9f33 843 // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
mbedalvaro 40:3ba2b0ea9f33 844 // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
mbedalvaro 40:3ba2b0ea9f33 845 else if (incomingByte == ':') {
mbedalvaro 40:3ba2b0ea9f33 846 // Data is now ordered in the auxDataBuffer array:
mbedalvaro 40:3ba2b0ea9f33 847 float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
mbedalvaro 40:3ba2b0ea9f33 848 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 849 clearScene();
mbedalvaro 40:3ba2b0ea9f33 850 // set color:
mbedalvaro 40:3ba2b0ea9f33 851 lsr.setColor(0x02); // green
mbedalvaro 40:3ba2b0ea9f33 852 //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 40:3ba2b0ea9f33 853
mbedalvaro 40:3ba2b0ea9f33 854 // set local pose matrix to identity:
mbedalvaro 40:3ba2b0ea9f33 855 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 856 // translate to the required position:
mbedalvaro 40:3ba2b0ea9f33 857 lsr.translate(posX, posY, 0); // since the projection and pose are going to be set
mbedalvaro 40:3ba2b0ea9f33 858 // to Identity (projection identity will automatically make the projection orthographic), this means that these
mbedalvaro 40:3ba2b0ea9f33 859 // values are in "laser projector pixels" (in fact angles).
mbedalvaro 40:3ba2b0ea9f33 860 // Create the collection of dot objects:
mbedalvaro 40:3ba2b0ea9f33 861 // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
mbedalvaro 40:3ba2b0ea9f33 862 grid(sizeX, sizeY, nx, ny, 1);
mbedalvaro 40:3ba2b0ea9f33 863 updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
mbedalvaro 40:3ba2b0ea9f33 864 // rendering is done in the drawScene() function.
mbedalvaro 40:3ba2b0ea9f33 865
mbedalvaro 40:3ba2b0ea9f33 866 // Now we can render the scene once:
mbedalvaro 40:3ba2b0ea9f33 867 // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
mbedalvaro 40:3ba2b0ea9f33 868 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 869 lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 40:3ba2b0ea9f33 870 drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
mbedalvaro 40:3ba2b0ea9f33 871 }
mbedalvaro 40:3ba2b0ea9f33 872 // NOTE: now that the grid is set, we don't need to do it again when changing the pose: JUST SEND THE POSE MATRIX and things will be re-rendered!
mbedalvaro 40:3ba2b0ea9f33 873
mbedalvaro 40:3ba2b0ea9f33 874 // This means we received "2D" vertex data (z assumed 0) to create a single scene out of these points (we will create a single object)
mbedalvaro 40:3ba2b0ea9f33 875 // using orthographic projection.
mbedalvaro 40:3ba2b0ea9f33 876 // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
mbedalvaro 40:3ba2b0ea9f33 877 // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
mbedalvaro 40:3ba2b0ea9f33 878 // NOTE: the POSE is reset to ID, but we could use the current pose (i.e., send it first by the computer). But for the time being, and in order to modify the
mbedalvaro 40:3ba2b0ea9f33 879 // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
mbedalvaro 40:3ba2b0ea9f33 880 else if (incomingByte == '=') {
mbedalvaro 40:3ba2b0ea9f33 881 // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y)
mbedalvaro 40:3ba2b0ea9f33 882 // The present value of auxDataBuffer_index contains the number of points x 2
mbedalvaro 40:3ba2b0ea9f33 883 clearScene();
mbedalvaro 40:3ba2b0ea9f33 884 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 885 //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
mbedalvaro 40:3ba2b0ea9f33 886 lsr.setColor(0x03); // blue+green
mbedalvaro 40:3ba2b0ea9f33 887 IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
mbedalvaro 40:3ba2b0ea9f33 888
mbedalvaro 40:3ba2b0ea9f33 889 // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
mbedalvaro 40:3ba2b0ea9f33 890 for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
mbedalvaro 40:3ba2b0ea9f33 891 begin(i);
mbedalvaro 40:3ba2b0ea9f33 892 for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
mbedalvaro 40:3ba2b0ea9f33 893 vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
mbedalvaro 40:3ba2b0ea9f33 894 end();
mbedalvaro 40:3ba2b0ea9f33 895 }
mbedalvaro 40:3ba2b0ea9f33 896 updateScene();
mbedalvaro 40:3ba2b0ea9f33 897 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 40:3ba2b0ea9f33 898 // properly called lsr.pushPoseMatrix())
mbedalvaro 40:3ba2b0ea9f33 899 lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 900 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 901 drawScene();
mbedalvaro 40:3ba2b0ea9f33 902 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 903 }
mbedalvaro 40:3ba2b0ea9f33 904
mbedalvaro 40:3ba2b0ea9f33 905 // This means we received 3D vertex data to create a single scene out of these points (we will create a single object).
mbedalvaro 40:3ba2b0ea9f33 906 // It will be rendered in the CURRENT pose.
mbedalvaro 40:3ba2b0ea9f33 907 else if (incomingByte == ';') {
mbedalvaro 40:3ba2b0ea9f33 908 // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
mbedalvaro 40:3ba2b0ea9f33 909 // The present value of auxDataBuffer_index contains the number of points x 3
mbedalvaro 40:3ba2b0ea9f33 910 lsr.pushPoseMatrix(); // we will render on the current pose...
mbedalvaro 40:3ba2b0ea9f33 911 clearScene();
mbedalvaro 40:3ba2b0ea9f33 912 lsr.setIdentityPose();
mbedalvaro 40:3ba2b0ea9f33 913 //lsr.setColor(0x02);
mbedalvaro 40:3ba2b0ea9f33 914 begin(1);
mbedalvaro 40:3ba2b0ea9f33 915 for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
mbedalvaro 40:3ba2b0ea9f33 916 vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],
mbedalvaro 40:3ba2b0ea9f33 917 auxDataBuffer[3 * i + 2]);
mbedalvaro 40:3ba2b0ea9f33 918 }
mbedalvaro 40:3ba2b0ea9f33 919 end();
mbedalvaro 40:3ba2b0ea9f33 920 updateScene();
mbedalvaro 40:3ba2b0ea9f33 921 // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
mbedalvaro 40:3ba2b0ea9f33 922 // properly called lsr.pushPoseMatrix())
mbedalvaro 40:3ba2b0ea9f33 923 //lsr.setOrthoProjection();
mbedalvaro 40:3ba2b0ea9f33 924 //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
mbedalvaro 40:3ba2b0ea9f33 925 lsr.popPoseMatrix();
mbedalvaro 40:3ba2b0ea9f33 926 drawScene();
mbedalvaro 40:3ba2b0ea9f33 927 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 928 }
mbedalvaro 40:3ba2b0ea9f33 929
mbedalvaro 40:3ba2b0ea9f33 930 // (b) a NUMBER to convert to float and to add to the auxiliary data buffer:
mbedalvaro 40:3ba2b0ea9f33 931 else if (incomingByte == '#') { // this means that we received a whole number to convert to float (but we don't know yet if it's for
mbedalvaro 40:3ba2b0ea9f33 932 // the modelview, the projection matrix or some other data):
mbedalvaro 40:3ba2b0ea9f33 933 receivedStringData[indexStringData] = 0;
mbedalvaro 40:3ba2b0ea9f33 934 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 935 // Serial.println(receivedStringData); // for tests
mbedalvaro 40:3ba2b0ea9f33 936 // convert to float and store in auxiliary "matrix" array:
mbedalvaro 40:3ba2b0ea9f33 937 auxDataBuffer[auxDataBuffer_index] = atof(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 938 //Serial.println( auxDataBuffer[auxDataBuffer_index]); // for tests
mbedalvaro 40:3ba2b0ea9f33 939 auxDataBuffer_index++;
mbedalvaro 40:3ba2b0ea9f33 940 }
mbedalvaro 40:3ba2b0ea9f33 941
mbedalvaro 40:3ba2b0ea9f33 942 /*
mbedalvaro 40:3ba2b0ea9f33 943 else if (incomingByte=='!') { // sets the width of the letter
mbedalvaro 40:3ba2b0ea9f33 944 receivedStringData[indexStringData]=0;
mbedalvaro 40:3ba2b0ea9f33 945 indexStringData=0;
mbedalvaro 40:3ba2b0ea9f33 946 fontWidth=atof(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 947 createTextScene(); // needed because we changed the geometry
mbedalvaro 40:3ba2b0ea9f33 948 }
mbedalvaro 40:3ba2b0ea9f33 949
mbedalvaro 40:3ba2b0ea9f33 950 else if (incomingByte=='"') { // sets the height of the letter
mbedalvaro 40:3ba2b0ea9f33 951 receivedStringData[indexStringData]=0;
mbedalvaro 40:3ba2b0ea9f33 952 indexStringData=0;
mbedalvaro 40:3ba2b0ea9f33 953 fontHeight=atof(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 954 createTextScene(); // needed because we changed the geometry
mbedalvaro 40:3ba2b0ea9f33 955 }
mbedalvaro 40:3ba2b0ea9f33 956 */
mbedalvaro 40:3ba2b0ea9f33 957
mbedalvaro 40:3ba2b0ea9f33 958 // (c) TERMINATOR indicating the data was text to display:
mbedalvaro 40:3ba2b0ea9f33 959 else if (incomingByte == '"') { // this means that the previous data was TEXT to display
mbedalvaro 40:3ba2b0ea9f33 960 receivedStringData[indexStringData] = 0; // termination for the string
mbedalvaro 40:3ba2b0ea9f33 961 indexStringData = 0;
mbedalvaro 40:3ba2b0ea9f33 962 // Serial.println(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 963 textToDisplay = string(receivedStringData);
mbedalvaro 40:3ba2b0ea9f33 964 createTextScene();
mbedalvaro 40:3ba2b0ea9f33 965 drawScene(); // needed because we changed the geometry, and we need to compute the projection once for display (of course, this will
mbedalvaro 40:3ba2b0ea9f33 966 // happens anyway when the computer send the pose matrix).
mbedalvaro 40:3ba2b0ea9f33 967
mbedalvaro 40:3ba2b0ea9f33 968 }
mbedalvaro 40:3ba2b0ea9f33 969
mbedalvaro 40:3ba2b0ea9f33 970 // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
mbedalvaro 40:3ba2b0ea9f33 971 else if (incomingByte == '$') { // when receiving this character, it means that the WHOLE matrix data (4x4 values) have been sent (with '#' separator), in row/column format
mbedalvaro 40:3ba2b0ea9f33 972 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 973 // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
mbedalvaro 40:3ba2b0ea9f33 974 lsr.setIdentityPose(); // RT=ID
mbedalvaro 40:3ba2b0ea9f33 975 lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
mbedalvaro 40:3ba2b0ea9f33 976 lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates
mbedalvaro 40:3ba2b0ea9f33 977
mbedalvaro 40:3ba2b0ea9f33 978 drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry, just project)
mbedalvaro 40:3ba2b0ea9f33 979
mbedalvaro 40:3ba2b0ea9f33 980 // Handshake:
mbedalvaro 40:3ba2b0ea9f33 981 pc.putc(13);
mbedalvaro 40:3ba2b0ea9f33 982 }
mbedalvaro 40:3ba2b0ea9f33 983
mbedalvaro 40:3ba2b0ea9f33 984 // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
mbedalvaro 40:3ba2b0ea9f33 985 else if (incomingByte == '%') { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
mbedalvaro 40:3ba2b0ea9f33 986 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 987 // store in projection matrix:
mbedalvaro 40:3ba2b0ea9f33 988 lsr.loadProjMatrix(auxDataBuffer, 1.0);
mbedalvaro 40:3ba2b0ea9f33 989
mbedalvaro 40:3ba2b0ea9f33 990 drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
mbedalvaro 40:3ba2b0ea9f33 991 }
mbedalvaro 40:3ba2b0ea9f33 992
mbedalvaro 40:3ba2b0ea9f33 993 //(e) TERMINATOR indicating the data was for the extrinsic matrix:
mbedalvaro 40:3ba2b0ea9f33 994 else if (incomingByte == '&') { // when receiving this character, it means that the WHOLE matrix data (3x3 values) have been sent (with '#' separator), in row/column format
mbedalvaro 40:3ba2b0ea9f33 995 auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
mbedalvaro 40:3ba2b0ea9f33 996 // store in projection matrix:
mbedalvaro 40:3ba2b0ea9f33 997 lsr.loadExtrinsicsMatrix(auxDataBuffer);
mbedalvaro 40:3ba2b0ea9f33 998
mbedalvaro 40:3ba2b0ea9f33 999 drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
mbedalvaro 40:3ba2b0ea9f33 1000 }
mbedalvaro 40:3ba2b0ea9f33 1001
mbedalvaro 40:3ba2b0ea9f33 1002 }
mbedalvaro 40:3ba2b0ea9f33 1003 }