Alvaro Cassinelli
Laser Sensing Display for UI interfaces in the real world
Fork of
skinGames_forktest
by 
Alvaro Cassinelli
Diff: main.cpp
- Revision:
- 40:3ba2b0ea9f33
- Parent:
- 37:fa6b1f15819f
- Child:
- 41:74e24a0e6e50
--- a/main.cpp Wed Oct 16 15:54:39 2013 +0000
+++ b/main.cpp Wed Oct 16 16:14:27 2013 +0000
@@ -1,904 +1,983 @@
-#include "mbed.h"
-#include "hardwareIO.h"
-#include "mbedOSC.h"
-#include "blobConfig.h"
-#include "simpleLaserRenderer.h"
-
-extern "C" void mbed_reset();
-
-blobConfig blobconf;
-simpleLaserSensingRenderer lsr;
-
-// For tests:
-DigitalOut myled(LED1);
-DigitalOut myled2(LED2);
-DigitalOut myled3(LED3);
-
-
-// To test the time it takes for executing one loop in the main program:
-//#define LOOPTIMECOMPUTE
-
-// To get serial commands (for debug, or other things using a Terminal - for instance, a laser scan)
-#define SERIAL_COMMANDS
-
-//---------- ETHERNET / OSC related (in the future, put somewhere else...): -------------------------------------------
-// mbed IP address (server):
-#ifdef DHCP
-EthernetNetIf eth;
-#else
-EthernetNetIf eth(
- IpAddr(10,0,0,2), //IP Address of the mbed
- IpAddr(255,255,255,0), //Network Mask
- IpAddr(10,0,0,1), //Gateway
- IpAddr(10,0,0,1) //DNS
-);
-#endif
-
-//uint8_t serverMac[] = { 0xDE, 0xAD, 0xBE, 0xEF, 0xFE, 0xED };
-uint8_t serverIp[] = { 10, 0, 0, 2 }; // not needed perhaps!
-int serverPort = 10000;
-
-//uint8_t destIp[] = {10, 0, 0, 3};
-uint8_t destIp[] = {255, 255, 255, 255}; // broadcast, so we can use several computers for sound, etc
-int destPort = 12000;
-
-char *topAddress="/mbed";
-char *subAddress[3]={ "/test1" , "/test2" , "/test3" };
-
-OSCMessage recMes;
-OSCMessage sendMes;
-
-OSCClass osc;
-//OSCClass osc(&recMes); // instantiate OSC communication object, and set the receiver container from the OSC packets
-
-void processOSC(UDPSocketEvent e);
-// ----------------------------------------------------------------------------------------------------------------------
-
-// Tickers:
-Ticker timerForRendering;
-//Ticker timerForSendingData; // better use a timer, so as not to interrupt the exact laser display ticker
-
-// Timers:
-Timer measureLoopPeriod;
-//Timer measureUpdatePeriod;
-
-
-// to get serial commands (not necessary perhaps)
-void processSerial();
-
-int main() {
-
- // Initialize the hardware (laser powers, positions...):
- IO.init();
-
- // -------------------------------
- // Set the Ethernet port:
- printf("Setting up...\r\n");
- EthernetErr ethErr = eth.setup();
- if (ethErr) {
- printf("Error %d in setup.\r\n", ethErr);
- return -1;
- }
- printf("Setup OK\r\n");
-
- //(1) Sending message:
- // Set IP and Port:
- sendMes.setIp( destIp );
- sendMes.setPort( destPort );
- // Set data:
- // sendMes.setTopAddress(topAddress);
-
- //setting osc functionnality:
- //(2) Receiving:
- // recMes.setIp( serverIp ); // not needed?
- 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)
- osc.begin(serverPort, &processOSC); // binds the upd (osc) messages to an arbitrary listening port ("server" port), and callback function
- // -------------------------------
-
- /* // sending seems not to work right after setting the osc object??
- wait(1);
- sendMes.setTopAddress("starting");
- sendMes.setSubAddress("");
- osc.sendOsc( &sendMes );
- */
-
- // initialize with the desired blob configuration:
-
- // Tested modes:
- blobconf.clearConfig();
-// blobconf.addOneElasticLoopContractCentral();
-// blobconf.addOneElasticContourFollowing();
-
-// blobconf.addOneRigidLoopBouncing();
-// blobconf.addOneRigidLoopBouncing();
-//blobconf.addOneRigidLoopFollowing();
- // blobconf.addOneRigidLoopFollowing();
- //blobconf.addOneElasticLoopContractCentralFast();
- //blobconf.addOneRigidLoopTest();
-
-// START WITH TWO FOLLOWING SPOTS (exhibition Tokyo Design Week):
- blobconf.initConfig(FOLLOWING_SPOTS, 2); // value is the nb of spots instantiated
-// Make them of different color:
- blobconf.blobArray[0]->setBlueColor(1);
- blobconf.blobArray[1]->setGreenColor(1);
- // start in no stand by mode:
- blobconf.allResume();
-
- // Important: first, set the initial position for all the blobs, this will be useful because
- // when changing modes we can use the previous central position...
- // blobconf.setInitialPos(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y);
-
-// draw the config once before activating the laser buffer:
- blobconf.draw();
- lsr.startFullDisplay();
-
- // RENRERER (attn: setConfigToRender must be called when the blobconf is set - i.e., the number of blobs and number of points/blob is fixed)
- lsr.setConfigToRender(&blobconf);
-
- // Timer on the rendering function of the oneLoop object:
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL); // the address of the object, member function, and interval (in seconds)
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL); // the address of the object, member function, and interval (in seconds)
-
- // Timer for sending OSC data:
- // timerForSendingData.attach(&blobconf, &blobConfig::sendConfData, 0.025); // time in seconds (25ms -> 40Hz)
-
- //========================================== INFINITE LOOP (in USER PROGRAM CONTEXT) ===================================================================
-#ifdef LOOPTIMECOMPUTE
- int timeCounterNum=1000;
-#endif
-
- //measureUpdatePeriod.start();
-
- while (true) {
-
- if (lsr.endedFullDisplay()) {
-
- // measureUpdatePeriod.stop();
- // measureUpdatePeriod.reset();
-
- // __disable_irq();
-
- // update config dynamics (this also could be threaded?):
- blobconf.update();
-
-
- // draw the config (note: each kind of blob renders differently)
- blobconf.draw();
-
-
- // (b)Sending Data: // PUT THIS IN AN INTERRUPT OR USE A TIMER!!! it may be TOO FAST...
- // NOTE: better use a timer, so the only ISR "ticker" is the laser rendering (otherwise the laser rendering will be interrupted by the sending of data - the other way is ok):
- // NOTE: timer for sending is now not COMMON to all blobs, but depends on the blob (it could depend on the CONFIG only, but this can be simulated by using
- // per blob timer counter.
- blobconf.sendConfData();
-
-
- lsr.startFullDisplay(); // this start the point-display counter (wherever the actual laser is). Before update and draw, the counter needs to be reset.
-
-
- // __enable_irq();
-
- // measureUpdatePeriod.start();
-
- }
-
-
- // COMMUNICATION:
- // (a) Reading commands:
- // Ethernet:
- Net::poll(); // this will take care of calling processOSC(UDPSocketEvent e) when a new packet arrives.
-
- // Serial:
-#ifdef SERIAL_COMMANDS
- if (pc.readable()>0) processSerial();
-#endif
-
- // Potentiometer, switches, etc:
- //(1) Check for change of threshold mode button (switch one):
- //!!! ATTENTION: this does not work very well to say the truth: bouncing+adc settings problems...
- bool stateswitch;
- if (IO.switchOneCheck(stateswitch)) {
- if (stateswitch) pc.printf("Setting AUTO threshold mode\n"); else pc.printf("Setting FIXED threshold mode\n");
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setThresholdMode((stateswitch? 1 : 0));
- }
- //(2) in case the the second switch was pressed, check the current pot value and update the fixed threshold (regardless of the threshold mode)
- if (IO.switchTwoCheck(stateswitch)) {
- IO.updatePotValue();
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setFixedThreshold(IO.potValue);
- pc.printf("Got :%d\n", IO.potValue);
- }
-
-
- // text:
- /*
- sendMes.setTopAddress("/hello");
- sendMes.setSubAddress("/daito"); // ATTENTION: the host computer needs to know in advance how many points are in the loop (I did not implement "bundle" messages yet...)
- int x=(long)10;
- sendMes.setArgs( "i", &x);
- osc.sendOsc( &sendMes );
- */
-
-#ifdef LOOPTIMECOMPUTE
- if (timeCounterNum>50) myled = 0;
- // if (timeCounterNum%10==0) blobconf.sendConfData();
- if (timeCounterNum>100) {
- myled = 1;
- measureLoopPeriod.stop();
- sendMes.setTopAddress("/timeloop");
- sendMes.setSubAddress("/");
- // long x=(long)(int(measureLoopPeriod.read_us()/100.0));
- // long x=(long)(blobconf.blobArray[0]->displaySensingBuffer.lsdTrajectory.size());
- // long x=(long)(blobconf.blobArray[0]->normRecenteringVector);
- long x=(long)(1000*blobconf.blobArray[0]->displaySensingBuffer.lsdTrajectory[0].intensity);
- sendMes.setArgs( "i", &x);
- osc.sendOsc( &sendMes );
- timeCounterNum=0;
- measureLoopPeriod.reset();
- measureLoopPeriod.start();
- } else timeCounterNum++;
-#endif
-
- }
-}
-
-// ================= INTERPRET COMMAND =========================
-// NOTE: the following arrays are GLOBAL (used in processOSC and processSerial, as well as in interpretCommand function):
-// max of two addresses (top and sub), of a max length of 24 characters:
-char address[2][24];
-//long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs)
-int data[2]; // this is to have -1 as NO DATA, to detect errors.
-
-//interpretCommand(const char& address[2][], const int& data[2]) {
-void interpretCommand() {
- // (I) =========================================== SPECIAL FUNCTIONS (reset, rescan LUT, etc) ====================================================
- if ( !strcmp(address[0], "takeSnapshot" ) ) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
-
- // for test:
- for (int i=0; i<2 ; i++) {
- myled = 1;
- wait(0.1);
- myled = 0;
- wait(0.1);
- }
-
- // First, we need to disable the threaded display for the loop:
- timerForRendering.detach();
-
- // Then, do the scan (sending values on SERIAL port):
- IO.scan_serial(value);
-
- // Finally, start again threaded display:
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
-
- }
- }
-
- else if ( !strcmp(address[0], "mbedReset" ) ) mbed_reset();
-
- else if (!strcmp(address[0], "showMirrorLimits")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- timerForRendering.detach();
- IO.showLimitsMirrors(value);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- }
- }
-
- else if ( !strcmp(address[0], "calibrate" ) ) {
- // First, we need to disable the threaded display for the loop:
- timerForRendering.detach();
- // RESCAN (and save LUT table):
- IO.scanLUT();
- // Finally, start again threaded display:
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
-
- // (II) ========================================= GLOBAL CONFIG and HARDWARE COMMANDS ===========================================
-
- else if ( !strcmp(address[0], "setAllColor" ) ) {
- int value=data[0]; // this is the color (RGB bits)
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- blobconf.allSetColor(value);
- }
- }
-
- else if ( !strcmp(address[0], "setAllRandomColor" ) ) {
- blobconf.randomizeAllColors();
- }
-
- else if ( !strcmp(address[0], "setAllGreenColor" ) ) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- if (value==0)
- blobconf.allSetGreen(1);
- else
- blobconf.allSetGreen(0);
- }
- }
-
- else if ( !strcmp(address[0], "setAllBlueColor" ) ) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- if (value==0)
- blobconf.allSetBlue(1);
- else
- blobconf.allSetBlue(0);
- }
- }
-
- // NOTE: RED either afect the lock in, or some other red laser... not yet done.
-
- else if ( !strcmp(address[0], "testPower" ) ) {
- // First, we need to disable the threaded display for the loop:
- timerForRendering.detach();
-
- // Note: arguments is first 3 bits to set the laser powers (3 LSB bits to set each color)
- // and then the second argument is the number of seconds to wait for the measurment
- int value1=data[0], value2=data[1];
- if ((value1!=-1)&&(value2!=-1)) { // otherwise do nothing, this is a reception error (there was no data)
-
- // Set position of mirrors:
- IO.writeOutX(CENTER_AD_MIRROR_X);
- IO.writeOutY(CENTER_AD_MIRROR_Y);
-
- for (int i=0; i<3 ; i++) {
- myled3 = 1;
- wait(0.2);
- myled3 = 0;
- wait(0.2);
- }
-
- // Set laser power:
- IO.setRGBPower((unsigned char)value1);
-
- // Wait...
- wait(value2);// in seconds
- //Timer t;
- //t.start();
- //while(t.read_ms()<value2*1000);
- //t.stop();
- }
-
- // Finally, start again threaded display:
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
-
- }
-
-
-
- // SIMPLE BEHAVIOUR MODES (to be read from an XML file in the future):
- else if (!strcmp(address[0], "elastic_following")) { //
- timerForRendering.detach();
-
- blobconf.initConfig(ONE_ELASTIC_FOLLOWING);
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
-
- } else if (!strcmp(address[0], "elastic_mouth")) { //
- timerForRendering.detach();
-
- blobconf.initConfig(ONE_ELASTIC_MOUTH);
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
-
- } else if (!strcmp(address[0], "elastic_mouth_small")) { //
- timerForRendering.detach();
-
- blobconf.initConfig(ONE_ELASTIC_MOUTH_SMALL);
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- else if (!strcmp(address[0], "spot_bouncing")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- timerForRendering.detach();
-
- blobconf.initConfig(BOUNCING_SPOTS, value); // value is the nb of spots instantiated
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- }
-
-else if (!strcmp(address[0], "spot_lorentz")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- timerForRendering.detach();
-
- blobconf.initConfig(LORENTZ_SPOTS, value); // value is the nb of spots instantiated
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- }
-
- else if (!strcmp(address[0], "air_hockey")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- timerForRendering.detach();
-
- blobconf.initConfig(AIR_HOCKEY_GAME, value); // value is the nb of spots instantiated
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- }
-
- else if (!strcmp(address[0], "rain_mode")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- timerForRendering.detach();
-
- blobconf.initConfig(RAIN_MODE, value); // value is the nb of spots instantiated
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- }
-
-
- else if (!strcmp(address[0], "spot_following")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
-
- timerForRendering.detach();
-
- blobconf.initConfig(FOLLOWING_SPOTS, value); // value is the nb of spots instantiated
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- }
-
- else if (!strcmp(address[0], "circular_pong")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
-
- timerForRendering.detach();
-
- blobconf.initConfig(CIRCULAR_PONG_GAME, value); // value is the nb of spots instantiated
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- }
-
- else if (!strcmp(address[0], "fish_net")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
-
- timerForRendering.detach();
-
- blobconf.initConfig(FISH_NET_GAME, value); // value is the nb of spots instantiated
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- }
-
- else if (!strcmp(address[0], "vertical_pinball")) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
-
- timerForRendering.detach();
-
- blobconf.initConfig(VERTICAL_PINBALL_GAME, value);
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
- }
-
- else if (!strcmp(address[0], "pac_man")) {
- timerForRendering.detach();
-
- blobconf.initConfig(PAC_MAN_GAME);
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
-
- else if (!strcmp(address[0], "spot_tracking")) {
- timerForRendering.detach();
-
- blobconf.initConfig(ONE_TRACKING_SPOT); // value is the nb of spots instantiated
-
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
-
- else if (!strcmp(address[0], "spot_test")) {
- timerForRendering.detach();
- // blobconf.computeBoundingBox();
- blobconf.clearConfig();
- blobconf.addOneRigidLoopTest();
- lsr.setConfigToRender(&blobconf);
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
-
- // other:
-
- else if ( !strcmp(address[0], "standby" ) ) { // will put ALL the blobs in stand by mode (no update function)
- blobconf.allStandBy(); // will avoid the update function
- }
- else if ( !strcmp(address[0], "resume" ) ) {
- blobconf.allResume(); // Update function is called for all the blobs
- }
-
- // (III) ========================================= Loop control (parameters, etc) ===========================================
-
- else if (!strcmp( address[0], "setSpeed" ) ) {
- int value=data[0]; // value 1 means a speed of 0.1, value 10, a speed of 1, etc.
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) {
- //if ((!strcmp(blobconf.blobArray[i].spotName, "rigid_following"))||(!strcmp(blobconf.blobArray[i].spotName, "rigid_following"))) {
- blobconf.blobArray[i]->setSpeed((float)(0.1*value));
- }
- }
- }
- else if (!strcmp( address[0], "speedFactor" ) ) {
- int value=data[0]; // value 100 means no change of speed. 200 is twice as fast, 50 is half as fast.
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) {
- //if ((!strcmp(blobconf.blobArray[i].spotName, "rigid_following"))||(!strcmp(blobconf.blobArray[i].spotName, "rigid_following"))) {
- blobconf.blobArray[i]->speedFactor((float)(1.0*value/100.0));
- }
- }
- }
-
- else if (!strcmp( address[0], "setSize" ) ) {
- int value=data[0]; // this is the direct size of the scafold
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->setSize((float)(1.0*value));
- }
- }
- else if (!strcmp( address[0], "sizeFactor" ) ) {
- int value=data[0]; // value 100 means no change of sice. 200 is twice as big, 50 is half as big.
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->sizeFactor((float)(1.0*value/100.0));
- }
- }
-
- // ADJUST MIRROR ANGLE CORRECTION:
- else if (!strcmp( address[0], "adjustPlusAngle" ) ) {
- int value=data[0]; // this is not a factor, but an additive quantity to the current delay
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.addDelayMirrors(value);
- }
- }
- else if (!strcmp( address[0], "adjustMinusAngle" ) ) {
- int value=data[0]; // this is not a factor, but an substractive quantity to the current delay
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.addDelayMirrors(-value);
- }
- }
-
- else if (!strcmp( address[0], "adjustPlusAngle" ) ) {
- int value=data[0]; // value 100 means no change of speed. 200 is twice as fast, 50 is half as fast.
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.addDelayMirrors(value);
- }
- }
-
-
- // THRESHOLD MODE, and PARAMETERS:
- //(1) Set the threshold mode:
- else if (!strcmp( address[0], "autoThreshold" ) ) {
- int value=data[0]; // 1 (auto) or 0 (fixed)
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setThresholdMode(value);
- }
- // set led and switch state (global value of threshold):
- IO.setSwitchOneState(value>0);
- }
-
- // (a) AUTO THRESHOLD:
- // MINIMUM CONTRAST RATIO:
- // (1) using multiplicative factor:
- else if (!strcmp( address[0], "adjustMultContrast" ) ) {
- int value=data[0]; // value 100 means no change of the current contrast value. 200 means a min contrast
- // that is twice as large and 50 is half as large as before.
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.multMinContrastRatio((float)(1.0*value/100.0));
- }
- }
-// (2) directly:
- else if (!strcmp( address[0], "adjustContrast" ) ) {
- int value=data[0]; // value is in PERCENT (100=1, 50 = 0.5...)
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setMinContrastRatio((float)(1.0*value/100.0));
- }
- }
- // THRESHOLD FACTOR:
- //(1) using multiplicative factor:
- else if (!strcmp( address[0], "adjustMultThreshold" ) ) {
- int value=data[0]; // value 100 means no change of the current contrast value. 200 means a min contrast
- // that is twice as large and 50 is half as large as before.
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.multThresholdFactor((float)(1.0*value/100.0));
- }
- }
- //(2) directly:
- else if (!strcmp( address[0], "adjustThresholdFactor" ) ) {
- int value=data[0]; // value is in PERCENT (100=1, 50 = 0.5...)
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setThresholdFactor((float)(1.0*value/100.0));
- }
- }
- // MINIMUM ACCEPTABLE INTENSITY:
- // Adjust minimum acceptable intensity:
- else if (!strcmp( address[0], "adjustMinAcceptableIntensity" ) ) {
- int value=data[0]; // value is DIRECT value (0 to 255)
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setMinAcceptableIntensity(value);
- }
- }
-
- // (b) FIXED THRESHOLD:
- // Adjust fixedThreshold (directly):
- else if (!strcmp( address[0], "adjustFixedThreshold" ) ) {
- int value=data[0]; // value is DIRECT value (0 to 255)
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setFixedThreshold(value);
- }
- }
-
-
-
- // ===================== SEND DATA MODES =======================
-
- else if (!strcmp( address[0], "sendOSC" ) ) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) {
- blobconf.blobArray[i]->sendOSC=(value>0);
- }
- }
- }
-
- else if (!strcmp( address[0], "sendArea" ) ) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) {
- blobconf.blobArray[i]->sendingBlobArea=(value>0);
- }
- }
- }
-
- else if (!strcmp( address[0], "sendPos" ) ) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) {
- blobconf.blobArray[i]->sendingLoopPositions=(value>0);
- }
- }
- }
-
- else if (!strcmp( address[0], "sendRegions" ) ) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) {
- blobconf.blobArray[i]->sendingLoopRegions=(value>0);
- }
- }
- }
-
- else if (!strcmp( address[0], "sendTouched" ) ) {
- int value=data[0];
- if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
- for (int i=0; i< blobconf.numBlobs; i++) {
- blobconf.blobArray[i]->sendingTouched=(value>0);
- }
- }
- }
-
-
-
-}
-
-//============= RECEIVE OSC COMMANDS =========================
-// This is the callback function called when there are packets on the listening socket. It is not nice to have it
-// here, but for the time being having a "wrapping global" is the simplest solution (we cannot pass a member-function pointer
-// as handler to the upd object).
-void processOSC(UDPSocketEvent e) {
- osc.onUDPSocketEvent(e);
-
- if (osc.newMessage) {
- // in fact, there is no need to check this if using the method of a global callback function - it is clear this is a new packet... however, it may be
- // interesting to use a timer, and process data (answers, etc) only after a certain amount of time, so as to avoid blocking the program in IRQ context...
-
- // Acquire the addresses and arguments and put them in the GLOBAL variables:
- strcpy(address[0],"");
- strcpy(address[1],"");
- 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
- // Acquire data:
- data[0]=-1;
- data[1]=-1;
- for (int i=0; i<recMes.getArgNum(); i++) data[i]=(int)recMes.getArgInt(i);
-
- // Finally, interpret the command:
- interpretCommand();//address, data);
-
- }
-}
-
-//============= RECEIVE SERIAL COMMANDS =========================
-//
-// NOTE: - NUMERIC PARAMETERS have to be send BEFORE the command word. They must be sent as ASCII DEC, without end character.
-// - Commands words SHOULD NOT have numbers in it. They should be C compliant STRINGS (ended with character '0')
-// - order is irrelevant: we can send 10 RADIUS or RADIUS 10.
-
-// String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC) sent wirelessly:
-char stringData[24]; // note: an integer is two bytes long, represented with a maximum of 5 digits, but we may send floats or unsigned int...
-int indexStringData=0;//position of the byte in the string
-
-// String to store COMMAND WORDS:
-char stringCommand[24]; // note: an integer is two bytes long, represented with a maximum of 5 digits, but we may send floats or unsigned int...
-int indexStringCommand=0;
-bool commandReady=false; // will become true when receiving the byte 0 (i.e. the '/0' string terminator)
-
-void processSerial() {
-
- while (pc.readable()>0) {
-
-
- char val =pc.getc();
- // pc.printf("Got :%d\n", incomingByte);
- //pc.putc(incomingByte);
-
- // Save ASCII numeric characters (ASCII 0 - 9) on stringData:
- if ((val >= '0') && (val <= '9')) { // this is 45 to 57 (included)
- stringData[indexStringData] = val;
- indexStringData++;
- }
-
- // Save ASCII letters in stringCommand:
- if ((val >= 'A') && (val <= 'z')) { // this is 65 to 122 (included)
- stringCommand[indexStringCommand] = val;
- indexStringCommand++;
- }
- // is command ready?
- if (val=='/') {
- commandReady=true;
- stringCommand[indexStringCommand] = 0; // string termination.
- indexStringCommand=0; // reset index string for acquiring next command
- //Serial.println(stringCommand);
- }
-
- // COMMANDS (with or without numeric parameters):
- if (commandReady==true) { // it means we can interpret the command string:
- commandReady=false;
-
- stringData[indexStringData] = 0 ;// string termination for numeric values;
- indexStringData=0;
-
- // PARSE DATA: (TO DO!!!!!!!!!!!!!!):
-
- // (a) Parse command (get address[0] and address[1]):
- //ex: "/1/standBy" -- > address[0]="1" and address[1]="standBy"
- // address[2]
-
- // Serial.println(stringCommand);
- // Serial.println(stringData);
-
- // (b) Parse data:
-
- // char address[2][24];
- //long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs)
- //int data[2]; // this is to have -1 as NO DATA, to detect errors.
-
- // FOR THE TIME BEING there is no parsing for serial commands:
-
- // SCANNING:
- if (!strcmp(stringCommand , "takeSnapshot")) {
- // First, we need to disable the threaded display for the loop:
- timerForRendering.detach();
-
- // Then, do the scan (sending values on serial port):
- IO.scan_serial(atoi(stringData));
-
- // Finally, start again threaded display:
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- } else if (!strcmp(stringCommand , "REDON")) IO.setRedPower(1); // pc.printf("%d\n",incomingByte);
-
- else if (!strcmp(stringCommand , "REDOFF")) IO.setRedPower(0);
-
- else if (!strcmp(stringCommand , "READVALUE")) pc.printf("Value read: %f", lockin.getSmoothValue());//lockin.getLastValue());/
-
- else if (!strcmp(stringCommand , "mbedReset")) mbed_reset();
-
- else if (!strcmp(stringCommand , "calibrate")) {
- // First, we need to disable the threaded display for the loop:
- timerForRendering.detach();
- // RESCAN (and save LUT table):
- IO.scanLUT();
- // Finally, start again threaded display:
- timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL);
- // timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThreadONEBLOBONLY, RENDER_INTERVAL);
- }
-
- // FINALLY, interpret commands (but only after parsing):
- // interpretCommand();//address, data);
-
- }
- }
-}
-
-
-
-// ================ MISCELANEA
-
-/* EXAMPLE SEND/RECEIVE on PROCESSING:
-
-// oscP5sendreceive by andreas schlegel
-// example shows how to send and receive osc messages.
-// oscP5 website at http://www.sojamo.de/oscP5
-
-import oscP5.*;
-import netP5.*;
-
-OscP5 oscP5;
-NetAddress myRemoteLocation;
-
-void setup() {
- size(400,400);
- frameRate(25);
- // start oscP5, listening for incoming messages at port 12000
- oscP5 = new OscP5(this,12000);
-
- // myRemoteLocation is a NetAddress. a NetAddress takes 2 parameters,
- // an ip address and a port number. myRemoteLocation is used as parameter in
- // oscP5.send() when sending osc packets to another computer, device,
- // application. usage see below. for testing purposes the listening port
- // and the port of the remote location address are the same, hence you will
- // send messages back to this sketch.
- myRemoteLocation = new NetAddress("10.0.0.2",10000);
-}
-
-
-void draw() {
- background(0);
-}
-
-void mousePressed() {
- // in the following different ways of creating osc messages are shown by example
- OscMessage myMessage = new OscMessage("/mbed/test1");
-
- myMessage.add(123); // add an int to the osc message
-
- // send the message
- oscP5.send(myMessage, myRemoteLocation);
-}
-
-
-// incoming osc message are forwarded to the oscEvent method.
-void oscEvent(OscMessage theOscMessage) {
- // print the address pattern and the typetag of the received OscMessage
- print("### received an osc message.");
- print(" addrpattern: "+theOscMessage.addrPattern());
- println(" typetag: "+theOscMessage.typetag());
-}
-
-*/
+/*
+
+- NOTE: I could instantiate at least 960 points! (as 30 objects containing each 32 points)
+ ... 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
+ the trajectory and 3d vector array... but where?
+*/
+
+#include "mbed.h"
+#include "mbedOSC.h"
+
+// Ethernet can be created with *either* an address assigned by DHCP or a static IP address. Uncomment the define line for DHCP
+//#define DHCP
+#ifdef DHCP
+EthernetNetIf eth;
+#else
+EthernetNetIf eth(
+ IpAddr(10,0,0,2), //IP Address
+ IpAddr(255,255,255,0), //Network Mask
+ IpAddr(10,0,0,1), //Gateway
+ IpAddr(10,0,0,1) //DNS
+);
+#endif
+
+//// OSC
+// The message objects to send and receive with
+OSCMessage recMes;
+OSCMessage sendMes;
+
+// Setting - The port we're listening to on the mbed for OSC messages
+unsigned short serverPort = 10000;
+
+// Setting - The address and port we're going to send to, from the mbed
+uint8_t destIp[] = { 10, 0, 0, 1};
+int destPort = 12000;
+
+// The object to do the work of sending and receiving
+OSCClass osc;
+void processOSC(UDPSocketEvent e);
+
+
+#include "WrapperFunctions.h"
+
+// 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()...)
+#include "LaserRenderer.h"
+extern LaserRenderer lsr;
+
+// for tests:
+extern Scene scene;
+extern laserSensingDisplay lsd;
+
+// =======================================================
+
+// Timers:
+Timer t; // we can read in ms or us
+
+// Tickers:
+//Ticker timerForRendering; // now in the WrapperFunctions.cpp file
+//Ticker timerForSendingData; // better use a timer, so as not to interrupt the exact laser display ticker
+unsigned long lastTimeCreated;
+unsigned long lastTimeSensed;
+
+// ======================================================================================================================
+// DEFAULT CALIBRATION MATRICES (Extrinsics and Intrinsics).
+// -> would be better to load them from a system file!! (TO DO)
+// Note: last row is completed automatically in the loadExtrinsicMatrix and loadProjMatrix methods
+// Extrinsics of usb camera to projector (note: calibration was in cm, so the translation vector is in cm)
+float E1[12] = {9.8946330287356954e-01, -5.0979372852926171e-03, -1.4469410251272136e-01, 5.8356271792647112e+00,
+ -4.7536907255693065e-03, 9.9769720674821438e-01, -6.7658599389111715e-02, -6.0821695799283804e+00,
+ 1.4470582120637832e-01, 6.7633532232509466e-02, 9.8716053943963022e-01, -8.2545447546057940e+00
+ };
+
+// 9.9540692993883650e-01, -8.5578652572728450e-03, -9.5350966287597705e-02, 2.2256570914518332e+00,
+ // 1.4872282274740087e-02, 9.9772789680840523e-01, 6.5710418886328711e-02, -6.9310510316834923e+00,
+ // 9.4571978141945845e-02, -6.6826692814433777e-02, 9.9327253766416224e-01, 1.0699927478132011e+01
+//};
+
+
+// INTRINSICS of laser projector:
+float K1[6] = { 5.9957148864529627e+03, 0.0000000e+000, 2.8869455045867635e+03,
+ 0.0000000e+000, 5.9647451141521778e+03, 2.9005238051336592e+03
+ };
+
+// 7.0776317995422105e+03, 0., 2.4203185896583996e+03,
+// 0., 7.2889265345161484e+03, 1.7718110988625751e+03};
+
+// SCALE FACTOR for the SCAN:
+float scaleFactorProjector1 = 1.0; //scale factor for the projector image (the scan is, say, 600x600, but the resolution is 4096x4096)
+
+// ======================================================================================================================
+// LASER TERMINAL:
+// We assume that the "terminal" is operating on an a4 page. Units are in cm.
+V2 cursorPosition(0,0);
+
+void createTextScene();
+float angle;
+string textToDisplay = "HELLO";
+float fontWidth = 2.5, fontHeight = 2.5; // if calibration is in cm, then this is in cm.
+
+
+// ==============================================================================================================
+// Some global functions and variables (find a better way to integrate this!)
+extern "C" void mbed_reset();
+
+void createSceneTest();
+
+void interpretData();
+void processOSC();
+void processSerial();
+
+int touchedTimes = 0;
+enum sensingModes {NO_SENSING, SENSING_WHOLE_SCENE, SENSING_PER_OBJECT};
+sensingModes currentSensingMode=SENSING_PER_OBJECT;//NO_SENSING;
+
+// ==============================================================================================================
+int main()
+{
+
+ t.start();
+ lastTimeCreated = t.read_ms();
+ lastTimeSensed = t.read_ms();
+ angle = 0;
+
+ // Setup:
+ // (1) Hardware init (laser powers, positions...):
+ IO.init();
+
+ // OSC initialization:
+ // =====================================================
+ //// TASK: Set up the Ethernet port
+ printf("Setting up ethernet...\r\n");
+ EthernetErr ethErr = eth.setup();
+ if (ethErr) {
+ printf("Ethernet Failed to setup. Error: %d\r\n", ethErr);
+ return -1;
+ }
+ printf("Ethernet OK\r\n");
+
+ // SET OSC ===================================================================
+ //(1) Sending message:
+ // Set IP and Port:
+ sendMes.setIp( destIp );
+ sendMes.setPort( destPort );
+ // Set data:
+ // sendMes.setTopAddress(topAddress);
+
+ //setting osc functionnality:
+ //(2) Receiving:
+ // recMes.setIp( serverIp ); // not needed?
+ 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)
+ osc.begin(serverPort, &processOSC); // binds the upd (osc) messages to an arbitrary listening port ("server" port), and callback function
+ //===================================================================
+
+ /*
+ //EXAMPLE 1: simple geometry ======================================================
+ // (1) Calibration matrices (Projection, and extrinsics - if needed):
+ // Typically, the projection matrix is set from the PC side, or loaded by default (from file system).
+ lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being.
+
+ // (2) Create the scene in "local" coordinates:
+ createSceneTest(); // Create a default scene (for tests)
+
+ // (3) Set a default GLOBAL POSE and PROJECTION MATRIX (if we changed it) for rendering and start displaying:
+ // - Using the current K (that correct for the center of the camera too!):
+ //lsr.setIdentityPose();
+ //lsr.translate(0, 0, 2000);
+ // - or doing orthoprojection:
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose();
+ lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0);
+ lsr.rotateZ(45);
+ drawScene();
+
+ // *** 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.
+ lsr.startDisplay();
+
+ // (4) Main loop:
+ // Change the POSE when necessary (also called "viewing transformation" or "global modelview" in OpenGL jargon), render and display:
+ while (1) {
+
+ if (pc.readable())
+ processSerial(); // here, the pose can be changed by a pc command
+ hardwareKnobs();
+
+ if (0) { //((t.read_ms()-lastTimeCreated)>10) {
+ // for tests:
+ myLed2 = !myLed2;
+
+ angle += 1;
+ if (angle > 360)
+ angle = 0;
+
+ // Set the current global pose:
+ // lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
+ // lsr.translate(0, 0, 1000);
+ // lsr.rotateZ(angle);
+ // lsr.rotateX(2.5 * angle);
+
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose();
+ lsr.translate(CENTER_AD_MIRROR_X, CENTER_AD_MIRROR_Y, 0);
+ lsr.rotateZ(angle);
+ // lsr.rotateY( angle);
+
+ // 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)
+ drawScene();
+
+ lastTimeCreated = t.read_ms();
+ } // end auto-change pose loop
+
+ if ((t.read_ms() - lastTimeSensed) > 50) { // check data sensing every xx ms
+ // Check objects and change colors or other properties as a function of the touch:
+ // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h
+ // (note: we assume we are drawing the text).
+ // 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.
+ for (int i = 0; i < scene.totalObjects(); i++) {
+ BaseObject* ptrObj = scene.objectArray[i];
+ if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData()
+ ptrObj->setColor(0x02);
+
+ // char str[15];
+ // sprintf(str, "%d", touchedTimes);
+ // textToDisplay=string(str);//string(itoa(touchedTimes));
+ // touchedTimes++;
+ // createTextScene();
+
+ } else
+ ptrObj->setColor(0x01);
+ }
+ lastTimeSensed = t.read_ms();
+ } // end sensing loop
+
+ } // "end" infite loop
+ // =================================
+ */
+
+ // EXAMPLE 2: 3d text ======================================================
+
+ // (1) Calibration matrices:
+ // Typically, the projection and extrinsic matrix is set from the PC side, or loaded by default (from file system).
+ lsr.loadProjMatrix(K1, scaleFactorProjector1); // we could use a wrapper, but I won't for the time being.
+ lsr.loadExtrinsicsMatrix(E1);
+
+ // (2) Create the scene in "local" coordinates:
+ createTextScene(); // Create a default scene (for tests)
+
+ // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
+ lsr.loadProjMatrix(K1, scaleFactorProjector1);
+ lsr.setIdentityPose();
+ lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
+
+ // DEFAULT position in CAMERA coordinates (calibration was in cm):
+ lsr.translate(0, 0, 130);
+
+ drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
+
+ // *** 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.
+ startDisplay();
+
+ // (4) Main loop:
+ while(true) {
+
+ // =========== Query hardware events (knobs, ethernet, serial port) ============
+ Net::poll(); // This polls the network connection for new activity, without keeping on calling this you won't receive any OSC!
+ if (pc.readable()) processSerial(); // here, the pose can be changed by a pc command
+ hardwareKnobs();
+ // =============================================================================
+
+ // Automatic change of pose for tests
+ if (0) {//(t.read_ms()-lastTimeCreated)>20) {
+ angle+=.2;
+ if (angle>360) angle=0;
+
+ lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
+ lsr.flipY();
+ lsr.translate(0,0,1000);
+ lsr.rotateZ(angle);
+ //lsr.rotateX(angle);
+
+ drawScene();
+
+ lastTimeCreated=t.read_ms();
+ }
+
+ if ((currentSensingMode!=NO_SENSING)&&((t.read_ms()-lastTimeSensed)>80)) { // check data sensing every xx ms
+ // Check objects and change colors or other properties as a function of the touch:
+ // NOTE: the testing #define debugDelayMirrors should not be defined in the laserSensingDisplay.h
+ // (note: we assume we are drawing the text).
+ // 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.
+ switch(currentSensingMode) {
+ case SENSING_WHOLE_SCENE: // Sensing whole scene ----------
+ if (scene.sense()) {
+
+ // COUNTING:
+ // char str[15];
+ // sprintf(str, "%d", touchedTimes);
+ // textToDisplay=string(str);//string(itoa(touchedTimes));
+ // touchedTimes++;
+ // createTextScene();
+ // (3) Set a default PERSPECTIVE and GLOBAL POSE for start displaying:
+ // lsr.loadProjMatrix(K1, scaleFactorProjector1);
+ // lsr.setIdentityPose();
+ // lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
+ // This means that the current coordinate frame is the CAMERA, and calibration was in cm:
+ // lsr.translate(0, 0, 100);
+ ////lsr.flipY();
+ // drawScene(); // this will be called when we change the pose matrix - but needs to be called at least once to start displaying
+ // }
+ }
+ break;
+ case SENSING_PER_OBJECT: // Per-object sensing and action:
+ for (int i=0; i<textToDisplay.length(); i++) {
+ BaseObject* ptrObj=scene.objectArray[i];
+ if (ptrObj->sense()) { // this calls to ptrObj->displaySensingBuffer.processSensedData()
+ ptrObj->setColor(0x02); // make it green
+
+ // for fun: let's rotate THIS letter:
+ //lsr.pushPoseMatrix();
+ //lsr.setIdentityPose();
+ //lsr.rotateX(180);
+ //transformObject(ptrObj);
+ //lsr.popPoseMatrix();
+ // we need to redraw the scene (ie, reproject), or at least this object:
+ //lsr.renderObject(ptrObj, lsr.RT);
+
+ // or "highlight" the object by putting "cartouche" around it object (note: do this only if it did NOT have
+ //addCartoucheObject(ptrObj);
+
+ //pc.putc(i+48);
+
+ } else
+ ptrObj->setColor(0x01); // make it blue
+ }
+ break;
+ default:
+ break;
+ }
+ lastTimeSensed=t.read_ms();
+ } // end sensing
+
+ } // infinite while loop
+ // =================================
+}
+
+void createSceneTest()
+{
+ lsr.pushPoseMatrix();
+
+ // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
+ clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)
+
+ // (2) Set displaying attributes (colors and local pose):
+ // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
+ lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
+ // (b) set color:
+ lsr.setColor(0x02);
+
+ // Add a first object: using openGL-like syntaxis (here, a closed square):
+ // begin(1); // start creating object with identifier = 1
+ // vertex(10,10,1);
+ // vertex(10,100,1);
+ // vertex(100,100,1);
+ // vertex(10,100,1);
+ // vertex(10,10,1); //
+ // end();
+
+ // Add another objects using "object primitives":
+ //begin(2);
+ //lsr.translate(-100,-100,0); // recenter the square
+ //square(200, 10); // this square has a corner in (0,0), and by default, z=0
+ //end();
+
+ // A "cube" (with one corner at the origin):
+ //begin(3);
+ //lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
+ //lsr.translate(100,-100,-100); // recenter the cube (no need to push pop because we are seting the identity above)
+ //lsr.rotateY(30);
+ //cube(200,10); // cube(200, 20);
+ //end();
+
+ // A grid:
+ // IMPORTANT: a grid is a multi-object built, and DOES NOT NEED to be between a begin and end
+ lsr.setIdentityPose(); // we set the identity - then we don't need to do push pop... we start from "scratch" here. Not incremental rotation...
+ lsr.translate(-500, -500, 0);
+ grid(1000, 1000, 5, 5, 1); //grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
+ //gridCircles(300, 250, 6, 5, 10, 6); // gridCircles(float sizeX, float sizeY, int nx, int ny, float radius, int nbpointsCircle)
+
+ // A circle:
+ //begin(4);
+ //circle(200, 100);
+ //end();
+
+ updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
+ // rendering is done in the drawScene() function.
+
+ // pc.printf("Number of objects in the scene: %d\n", scene.totalObjects());
+ // pc.printf("Number of points in the scene: %d\n", scene.totalPoints());
+
+ lsr.popPoseMatrix();
+}
+
+void createTextScene()
+{
+ lsr.pushPoseMatrix();
+ //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).
+ // The first option is simpler now, we just use the wrapper function string3d(string _text, float totalWidth, float height):
+
+ // (1) Clear the scene (this stops displaying, thus creating a "laser hot spot", but in principle it only happens in the setup)
+ clearScene(); // NOTE: in the future (and if we have enough RAM, we can have several indexed "scenes", instead of a unique global)
+
+ // (2) Set displaying attributes (colors and pose):
+ // (a) Set the "local pose" (i.e., the MODELING TRANSFORMATION in OpenGL jargon):
+ lsr.setIdentityPose(); // we could use a wrapper, but I won't for the time being.
+ //lsr.flipY();
+ //lsr.translate(2,6,0);
+
+ lsr.flipY();
+ lsr.flipX();
+ lsr.translate(-15,-10,0);
+
+ // (b) set current color:
+ lsr.setColor(0x04); // three LSB bits for RGB - so 0x02 is GREEN ON (note that the sensing laser, now red, is always on...).
+
+ // A string as a UNIQUE object:
+ // begin(1); // start creating an object with index 1
+ // no retouching of the modelview, meaning the text is in (0,0,0) in "local" coordinates
+ // string3d(textToDisplay, textToDisplay.size()*fontWidth, fontHeight);
+ //end();
+
+ // A string as a *collection* of object-letters:
+ //lsr.translate(-textToDisplay.length()*fontWidth/2,-fontHeight/2,0);
+ for (unsigned short i = 0; i < textToDisplay.length(); i++) {
+ char ch = textToDisplay.at(i);
+ if (ch != ' ') {
+ begin(i);
+ letter3d(ch, fontWidth, fontHeight);
+ end();
+ }
+ lsr.translate(1.1*fontWidth, 0, 0);
+ }
+
+ updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
+ // rendering is done in the drawScene() function.
+
+ lsr.popPoseMatrix();
+
+}
+
+// ================= INTERPRET COMMAND =========================
+// NOTE: the following arrays are GLOBAL (used in processOSC and processSerial, as well as in interpretData function):
+// max of two addresses (top and sub), of a max length of 24 characters:
+char address[2][24];
+//long auxdata[2]; // to store a max of two arguments (note: we will only use LONGs)
+int data[2]; // this is to have -1 as NO DATA, to detect errors.
+int numArgs;
+
+// An auxiliary buffer to store parameters data, or matrices, or points to load as vertices.
+// 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
+// we can send 3d points, this means 3 floats per point; let's limit the number of points per object to 128 (a lot)
+float auxDataBuffer[128];
+int auxDataBuffer_index=0;
+
+//================== OSC messageReceivedCallback function =====================
+//============= RECEIVE OSC COMMANDS =========================
+// This is the callback function called when there are packets on the listening socket. It is not nice to have it
+// here, but for the time being having a "wrapping global" is the simplest solution (we cannot pass a member-function pointer
+// as handler to the upd object).
+void processOSC(UDPSocketEvent e) {
+ osc.onUDPSocketEvent(e);
+
+ if (osc.newMessage) {
+ // Acquire the addresses and arguments and put them in the GLOBAL variables to be processed by the interpretData function (common to serial and OSC):
+ strcpy(address[0],"");
+ strcpy(address[1],"");
+ 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
+ // Acquire data:
+ numArgs=recMes.getArgNum();
+ data[0]=-1;
+ data[1]=-1;
+ for (int i=0; i<numArgs; i++) data[i]=(int)recMes.getArgInt(i);
+
+ // Finally, interpret the command or data:
+ interpretData();
+ }
+}
+
+
+
+//interpretData(const char& address[2][], const int& data[2]) {
+void interpretData() {
+
+ // ========== DATA (all this is a hack because my OSC class does not accept BUNDLES...) =============
+ // Add data to auxiliary data buffer:
+ if (!strcmp(address[0], "data")) {
+ // numArgs is the number of arguments (in my OSC version this is max 2):
+ for (int i=0; i<numArgs; i++) auxDataBuffer[auxDataBuffer_index] = data[i];
+ auxDataBuffer_index+=numArgs;
+ // note: the index will be reset to 0 or whatever needed when a command USING data from the "d" message is received.
+ }
+
+ // ========== COMMANDS (using data on auxDataBuffer or directly as arguments =========================
+
+ else if ( !strcmp(address[0], "mbedReset" ) ) mbed_reset();
+
+ // Enable/disable projection:
+ else if ( !strcmp(address[0], "stopDisplay" )) stopDisplay();
+ else if (!strcmp(address[0], "resumeDisplay")) resumeDisplay();
+ else if (!strcmp(address[0], "showLimits")) {
+ int value=data[0]; // argument is the number of seconds for display
+ if (value!=-1) {
+ showLimitsMirrors(50, value);
+ }
+ }
+ else if (!strcmp(address[0], "setSensingMode")) {
+ int value=data[0];
+ if (value!=-1) { // otherwise do nothing, this is a reception error (there was no data)
+ switch(value) {
+ case 0:
+ currentSensingMode=NO_SENSING;
+ break;
+ case 1:
+ currentSensingMode=SENSING_WHOLE_SCENE;
+ break;
+ case 2:
+ currentSensingMode=SENSING_PER_OBJECT;
+ break;
+ default:
+ break;
+ }
+ }
+ }
+
+ // Change state machine color (objects created from this point on will have the state machine color)
+ 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.
+ int value=data[0];
+ if (value!=-1) lsr.setColor(value);
+ }
+
+ // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
+ 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.
+ int value=data[0];
+ if (value!=-1) changeColorScene(value);
+ }
+
+
+// =================== CREATION and DESTRUCTION of objects =========================================================
+ else if (!strcmp(address[0], "clearScene")) {
+ clearScene();
+ }
+
+ else if (!strcmp(address[0], "deleteObject")) { // second address is object identifier (a string that will be converted
+ int idobject=atoi(address[1]);
+ deleteObject(idobject);
+ }
+
+ // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
+ // been translated previously - this may be optional, we could first send the pose, then the points...)
+ // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
+ // 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.
+ // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
+ // NOTE: the following string of ASCII characters is assumed to have been input:
+ // posX#posY#sizeX#sizeY#nx#ny#:
+ // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
+ // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
+ else if (!strcmp(address[0], "gridScene")) {
+ // Data is now ordered in the auxDataBuffer array:
+ float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
+ auxDataBuffer_index=0; // to restart loading the auxiliary buffer 'auxDataBuffer'
+ clearScene();
+ // set color:
+ lsr.setColor(0x02); // green
+ //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
+
+ // set local pose matrix to identity:
+ lsr.setIdentityPose();
+ // translate to the required position:
+ lsr.translate(posX, posY, 0); // since the projection and pose are going to be set
+ // to Identity (projection identity will automatically make the projection orthographic), this means that these
+ // values are in "laser projector pixels" (in fact angles).
+ // Create the collection of dot objects:
+ // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
+ grid(sizeX, sizeY, nx, ny, 1);
+ updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
+ // rendering is done in the drawScene() function.
+
+ // Now we can render the scene once:
+ // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
+ drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
+ }
+ // 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!
+
+ // 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)
+ // using orthographic projection.
+ // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
+ // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
+ // 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
+ // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
+ else if (!strcmp(address[0], "arbitraryGridScene")) {
+ // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y). The argument to this message is the IDENTIFIER of the object:
+ // The present value of auxDataBuffer_index contains the number of points x 2
+ clearScene();
+ lsr.setIdentityPose();
+ //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
+ lsr.setColor(0x03); // blue+green
+ IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
+
+ // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
+ for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
+ begin(i);
+ for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
+ vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
+ end();
+ }
+ updateScene();
+ // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
+ // properly called lsr.pushPoseMatrix())
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose();
+ drawScene();
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ }
+
+ // 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):
+ else if (!strcmp(address[0], "createObject2d")) {
+ int IDObject=data[1];
+ if (IDObject!=-1) {
+ // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
+ // The present value of auxDataBuffer_index contains the number of points x 3
+ //lsr.pushPoseMatrix(); // we will render on the current pose
+ //clearScene();
+ //lsr.setIdentityPose();
+ //lsr.setColor(0x02); // we will render in current color
+ begin(IDObject);
+ for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
+ vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],0); // z=0 for 2d objects (in current pose)
+ }
+ end();
+ updateScene();
+ // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
+ // properly called lsr.pushPoseMatrix())
+ //lsr.setOrthoProjection();
+ //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
+ //lsr.popPoseMatrix();
+ drawScene();
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ }
+ }
+
+ // This means we received 3D vertex data to create an object.
+ // It will be rendered in the CURRENT pose.
+ else if (!strcmp(address[0], "createObject3d")) {
+ int IDObject=data[1];
+ if (IDObject!=-1) {
+ // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
+ // The present value of auxDataBuffer_index contains the number of points x 3
+ //lsr.pushPoseMatrix(); // we will render on the current pose
+ //clearScene();
+ //lsr.setIdentityPose();
+ //lsr.setColor(0x02); // we will render in current color
+ begin(IDObject);
+ for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
+ vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],
+ auxDataBuffer[3 * i + 2]);
+ }
+ end();
+ updateScene();
+ // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
+ // properly called lsr.pushPoseMatrix())
+ //lsr.setOrthoProjection();
+ //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
+ //lsr.popPoseMatrix();
+ drawScene();
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ }
+ }
+
+ // Create text:
+ else if (!strcmp(address[0], "createTextObject")) {
+ //NOTE: the actual text will be in the second address, and the argument will be the ID of this new text object:
+ textToDisplay = address[1];
+ int IDObject=data[1];
+ if (IDObject!=-1) {
+ begin(IDObject);
+ string3d(textToDisplay, fontWidth*textToDisplay.length(), fontHeight);
+ end();
+ updateScene();
+ drawScene();
+ }
+ }
+
+
+ // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
+ 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
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer'
+ // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
+ lsr.setIdentityPose(); // RT=ID
+ lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
+ lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates
+
+ drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry nor update the renderer (updateScene), just project)
+ }
+
+ // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
+ 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
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ // store in projection matrix:
+ lsr.loadProjMatrix(auxDataBuffer, 1.0);
+
+ drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
+ }
+
+ //(e) TERMINATOR indicating the data was for the extrinsic matrix:
+ 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
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ // store in projection matrix:
+ lsr.loadExtrinsicsMatrix(auxDataBuffer);
+
+ drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
+ }
+}
+
+//============= RECEIVE SERIAL COMMANDS =========================
+// String to store ALPHANUMERIC DATA (i.e., integers, floating point numbers, unsigned ints, etc represented as DEC):
+// TO DO: use string objects!
+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...
+int indexStringData = 0; //position of the byte in the string
+
+// String to store COMMAND WORDS (not used yet):
+char stringCommand[24];
+int indexStringCommand = 0;
+bool commandReady = false; // will become true when receiving the byte 0 (i.e. the '/0' string terminator)
+
+void processSerial()
+{
+ while (pc.readable() > 0) {
+ char incomingByte = pc.getc();
+
+ // For test:
+ // pc.putc(incomingByte);
+
+ // (a) TEXT to display or NUMBERS (in ASCII) to convert to numerical value (rem: limited set of characters for the time being):
+ // Note: numbers are float, so we need also the "." and "-"
+ if ( (incomingByte >= '0' && incomingByte <= '9') // numbers
+ || (incomingByte >= 'A' && incomingByte <= 'Z') // capital letters
+ || (incomingByte >= 'a' && incomingByte <= 'z') // small letters (in fact, will be used for other symbols)
+ || (incomingByte == ' ') // space
+ || (incomingByte == '-') // minus sign
+ || (incomingByte == '.') // decimal point
+ ) {
+ receivedStringData[indexStringData] = incomingByte;
+ indexStringData++;
+ }
+
+ // Enable/disable projection:
+ else if (incomingByte == '}') {
+ stopDisplay();
+ } else if (incomingByte == '{') {
+ resumeDisplay();
+ }
+
+ // Show maximum excursion of mirrors:
+ else if (incomingByte == '*') {
+ showLimitsMirrors(50, 120);
+ }
+
+
+ else if (incomingByte == '+') {
+ receivedStringData[indexStringData] = 0;
+ indexStringData = 0;
+ switch(atoi(receivedStringData)) {
+ case 0:
+ currentSensingMode=NO_SENSING;
+ break;
+ case 1:
+ currentSensingMode=SENSING_WHOLE_SCENE;
+ break;
+ case 2:
+ currentSensingMode=SENSING_PER_OBJECT;
+ break;
+ default:
+ break;
+ }
+ }
+
+ // Performs a scan of the surface and send all the data serially (N>, with N the resolution of the scan)
+ else if (incomingByte == '>') {
+ receivedStringData[indexStringData] = 0;
+ indexStringData = 0;
+ //pc.printf("scan command issued¥n");
+ scanSerial(atoi(receivedStringData));
+ }
+
+ // Recompute the LUT table
+ else if (incomingByte == '<') {
+ recomputeLookUpTable();
+ }
+
+ // Change state machine color (objects created from this point on will have the state machine color)
+ else if (incomingByte == '(') { // example: 0( (all OFF), 1( blue ON, 2( green ON, 4( red ON, 3( blue and green ON; etc.
+ receivedStringData[indexStringData] = 0;
+ indexStringData = 0;
+ // convert to integer and set the state machine color:
+ lsr.setColor(atoi(receivedStringData));
+ }
+
+ // Real time change of color of ALL the objects in the CURRENT scene (state machine color not changed)
+ else if (incomingByte == ')') { // example: 0) (all OFF), 1) blue ON, 2) green ON, 4) red ON, 3) blue and green ON; etc.
+ receivedStringData[indexStringData] = 0;
+ indexStringData = 0;
+ // convert to integer and set the color:
+ changeColorScene(atoi(receivedStringData));
+ }
+
+
+ // Produce a grid of points. The projection is ORTHOGRAPHIC. Also, POSE is reset to identity (i.e., the sent points should have
+ // been translated previously - this may be optional, we could first send the pose, then the points...)
+ // Note: orthographic projection can be achieved by simply setting the projection matrix to ID, while
+ // 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.
+ // Finally, if one wants to avoid an unnecessary matrix product, the laserRendering class provides a renderingMode that can be set to RAW.
+ // NOTE: the following string of ASCII characters is assumed to have been input:
+ // posX#posY#sizeX#sizeY#nx#ny#:
+ // where the names (ex: posX) is in fact an ASCII string representing the number in decimal base. This means that when we arrive here,
+ // the auxiliary array auxDataBuffer contains all that data (as floats), ex: auxDataBuffer[0] is just posX, and auxDataBuffer[5] is ny
+ else if (incomingByte == ':') {
+ // Data is now ordered in the auxDataBuffer array:
+ float posX = auxDataBuffer[0], posY = auxDataBuffer[1], sizeX = auxDataBuffer[2], sizeY = auxDataBuffer[3], nx = auxDataBuffer[4], ny = auxDataBuffer[5];
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ clearScene();
+ // set color:
+ lsr.setColor(0x02); // green
+ //IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
+
+ // set local pose matrix to identity:
+ lsr.setIdentityPose();
+ // translate to the required position:
+ lsr.translate(posX, posY, 0); // since the projection and pose are going to be set
+ // to Identity (projection identity will automatically make the projection orthographic), this means that these
+ // values are in "laser projector pixels" (in fact angles).
+ // Create the collection of dot objects:
+ // void grid(float sizeX, float sizeY, int nx, int ny, int repeatpoint);
+ grid(sizeX, sizeY, nx, ny, 1);
+ updateScene(); // this is important: this will compute the number of objects, points, etc, to prepare the displaying engine (but rendering is NOT yet done,
+ // rendering is done in the drawScene() function.
+
+ // Now we can render the scene once:
+ // Set a default GLOBAL POSE and orthographic projection for rendering and start displaying:
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
+ drawScene(); // this will be called when we change the pose matrix - but needs to be called at least one to start displaying
+ }
+ // 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!
+
+ // 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)
+ // using orthographic projection.
+ // This is in particular useful to create "structured light" grids - such the grid of planar points for calibration.
+ // In the future, we can have functions to add objects, etc. Basically, every wrapped function should be call-able by the PC.
+ // 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
+ // cameraProjectorCalibration Xcode project as little as possible, I will perform the transformations on the computer.
+ else if (incomingByte == '=') {
+ // Data is now ordered in the auxDataBuffer array as PAIRS (X,Y)
+ // The present value of auxDataBuffer_index contains the number of points x 2
+ clearScene();
+ lsr.setIdentityPose();
+ //lsr.setColor(0x02); // fixed color, or use the current color (in which case, we need to call lsr.pushColor() and then pop it again).
+ lsr.setColor(0x03); // blue+green
+ IO.setLaserLockinPower(0); // DISABLING SENSING LASER! (here the red). This is FOR CALIBRATION laser-camera
+
+ // begin(1) // we can create a single object, or one object per position (with repetition of the point if needed)
+ for (unsigned char i = 0; i < auxDataBuffer_index / 2; i++) {
+ begin(i);
+ for (unsigned char k=0; k<1; k++) // fixed repetition of points - we can send this as a parameter if we want.
+ vertex(auxDataBuffer[2 * i], auxDataBuffer[2 * i + 1], 0);
+ end();
+ }
+ updateScene();
+ // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
+ // properly called lsr.pushPoseMatrix())
+ lsr.setOrthoProjection();
+ lsr.setIdentityPose();
+ drawScene();
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ }
+
+ // This means we received 3D vertex data to create a single scene out of these points (we will create a single object).
+ // It will be rendered in the CURRENT pose.
+ else if (incomingByte == ';') {
+ // Data is now ordered in the auxDataBuffer array in triplets (X,Y,Z).
+ // The present value of auxDataBuffer_index contains the number of points x 3
+ lsr.pushPoseMatrix(); // we will render on the current pose...
+ clearScene();
+ lsr.setIdentityPose();
+ //lsr.setColor(0x02);
+ begin(1);
+ for (unsigned char i = 0; i < auxDataBuffer_index / 3; i++) {
+ vertex(auxDataBuffer[3 * i], auxDataBuffer[3 * i + 1],
+ auxDataBuffer[3 * i + 2]);
+ }
+ end();
+ updateScene();
+ // Set a default GLOBAL POSE and ORTHO projection matrix for start displaying, or use the CURRENT ONE (for that, we should have
+ // properly called lsr.pushPoseMatrix())
+ //lsr.setOrthoProjection();
+ //lsr.setIdentityPose(); // we could have done the translation here instead of in "local coordinates", but it's the same.
+ lsr.popPoseMatrix();
+ drawScene();
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ }
+
+ // (b) a NUMBER to convert to float and to add to the auxiliary data buffer:
+ 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
+ // the modelview, the projection matrix or some other data):
+ receivedStringData[indexStringData] = 0;
+ indexStringData = 0;
+ // Serial.println(receivedStringData); // for tests
+ // convert to float and store in auxiliary "matrix" array:
+ auxDataBuffer[auxDataBuffer_index] = atof(receivedStringData);
+ //Serial.println( auxDataBuffer[auxDataBuffer_index]); // for tests
+ auxDataBuffer_index++;
+ }
+
+ /*
+ else if (incomingByte=='!') { // sets the width of the letter
+ receivedStringData[indexStringData]=0;
+ indexStringData=0;
+ fontWidth=atof(receivedStringData);
+ createTextScene(); // needed because we changed the geometry
+ }
+
+ else if (incomingByte=='"') { // sets the height of the letter
+ receivedStringData[indexStringData]=0;
+ indexStringData=0;
+ fontHeight=atof(receivedStringData);
+ createTextScene(); // needed because we changed the geometry
+ }
+ */
+
+ // (c) TERMINATOR indicating the data was text to display:
+ else if (incomingByte == '"') { // this means that the previous data was TEXT to display
+ receivedStringData[indexStringData] = 0; // termination for the string
+ indexStringData = 0;
+ // Serial.println(receivedStringData);
+ textToDisplay = string(receivedStringData);
+ createTextScene();
+ drawScene(); // needed because we changed the geometry, and we need to compute the projection once for display (of course, this will
+ // happens anyway when the computer send the pose matrix).
+
+ }
+
+ // (d) TERMINATOR indicating data was for the POSE MATRIX of the object (Mp) (with respect to the CAMERA):
+ 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
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ // Now, auxDataBuffer is a buffer with 12 values (4x3), corresponding to the pose of the object in CAMERA coordinartes (RT')
+ lsr.setIdentityPose(); // RT=ID
+ lsr.multPoseMatrix(lsr.EXTRINSICS); // RT=E // we can simply do: lsr.setExtrinsicsPose()
+ lsr.multPoseMatrix(auxDataBuffer); // RT=ExRT' : this sets RT as the current object pose in PROJECTOR coordinates
+
+ drawScene(); // needed because we changed the POSE (but there is no need to re-create the geometry, just project)
+
+ // Handshake:
+ pc.putc(13);
+ }
+
+ // (d) TERMINATOR indicating data was for the projection matrix (will probably be saved in "hard" in the microcontroller code):
+ 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
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ // store in projection matrix:
+ lsr.loadProjMatrix(auxDataBuffer, 1.0);
+
+ drawScene(); // needed because we changed the PROJECTION matrix (but there is no need to re-create all the geometry, just project)
+ }
+
+ //(e) TERMINATOR indicating the data was for the extrinsic matrix:
+ 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
+ auxDataBuffer_index = 0; // to restart loading the auxiliary buffer 'auxDataBuffer' through serial port
+ // store in projection matrix:
+ lsr.loadExtrinsicsMatrix(auxDataBuffer);
+
+ drawScene(); // needed because we changed the EXTRINSICS (hence the pose), but there is no need to re-create the geometry, just project.
+ }
+
+ }
+}