File content as of revision 1:3be7b7d050f4:

#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);
 DigitalOut ledSwitchOne(LED_SWITCH_ONE); 

// 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

 int renderFraction=1;// when 1, the blob needs to be rendered completely before update; 2 means half of it, etc. 0 means render all the time. 

// NEW (1.4.2014): change modes by switch for fast demos:
int totalNumberDemos=13;
int currentDemoMode=0;
void changeMode(int mode);

//----------  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 measureReadPeriod; // timer for reading hardare or communication data (I will read in ms)
#define periodReadingData 30

//Timer measureUpdatePeriod;


// to get serial commands (not necessary perhaps)
void  processSerial();

int main() {

    // Initialize the hardware (laser powers, positions...):
    IO.init();
    ledSwitchOne=0;

    // -------------------------------
    // Set the Ethernet port:
   // pc.printf("Setting up...\r\n");
    //printf("Setting up...\r\n");
    EthernetErr ethErr = eth.setup();
    if (ethErr) {
       // pc.printf("Error %d in setup.\r\n", ethErr);
       // return -1;
    }
   // pc.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:

  //  blobconf.initConfig(ONE_ELASTIC_FOLLOWING);
       
    // 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, 1); // 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();
    
    measureReadPeriod.start();

    while (true) {

         //  Process sensing buffer and compute light forces (for all blobs here, or in the update function for each one)
        blobconf.processSensedData();
         
        if (lsr.endedFractionDisplay(renderFraction)) {// (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();

        } else {
            
            // do a delay to equilibrate the timings?
            //wait_us(6700);
            }


        // COMMUNICATION and HARDWARE CONTROL:
        
        // (a) Reading commands:
        if (measureReadPeriod.read_ms()>periodReadingData) {
            measureReadPeriod.stop();
        
        // 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... better use a TWO STATE SWITCH:
       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));
            currentDemoMode=(currentDemoMode+1)%totalNumberDemos;
           // ledSwitchOne=(stateswitch? 1 :0); // this switch has a built-in led
           ledSwitchOne=1; wait_ms(500) ; ledSwitchOne=0; 
            changeMode(currentDemoMode);
             blobconf.allResume();
       }
       if (IO.twoStateSwitchCheck(stateswitch)) { // if there is a change...
           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)
       // NOTE: using the potentiometer FAILS because I cannot properly switch back and forth between ADC modes (burst and normal)
       if (IO.switchTwoCheck(stateswitch)) { 
        timerForRendering.detach();
          IO.showLimitsMirrors(5); // in seconds
          timerForRendering.attach(&lsr, &simpleLaserSensingRenderer::laserRenderThread, RENDER_INTERVAL); 
          
       //    IO.updatePotValue(); 
       //    for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setFixedThreshold(IO.potValue);
       //    pc.printf("Got :%d\n", IO.potValue);
        }
        
        if (rotaryEncoder1.CheckNew()) {
            for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setFixedThreshold(rotaryEncoder1.Get());
            //pc.printf("Fixed Threshold :%d\n", rotaryEncoder1.Get());
            }
        
        // (3) Change additional mirror delay from rotary encoder: 
        if (rotaryEncoder2.CheckNew()) {
            for (int i=0; i< blobconf.numBlobs; i++) blobconf.blobArray[i]->displaySensingBuffer.setDelayMirrors(rotaryEncoder2.Get());
            // pc.printf("Mirror delay :%d\n", rotaryEncoder2.Get());
            }
        
        
        // Restart the timer:
        measureLoopPeriod.reset();
        measureReadPeriod.start();
        }

        // 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>1000) {
            myled = 1;
            measureLoopPeriod.stop();
            sendMes.setTopAddress("/timeloop");
            sendMes.setSubAddress("/");
            long x=(long)(int(measureLoopPeriod.read_us()/1000.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);
            
            if ( blobconf.numBlobs>1) 
            for (int i=0; i< blobconf.numBlobs; i++) {
                blobconf.blobArray[i]->displaySensingBuffer.setDelayMirrors(2);
                blobconf.blobArray[i]->angleCorrectionForceLoop=-8;// in degrees
            }
        }
    }

    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]->addAngleCorrection(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]->addAngleCorrection(-value);
    }
   
   // ADJUST MIRROR DELAY (angle or mirror delay are equivalent in case of circular blobs):
    else if (!strcmp( address[0], "adjustPlusDelay" ) ) {
      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);
      }
    }
    else if (!strcmp( address[0], "adjustMinusDelay" ) ) {
      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);
      }
    }
    
     else if (!strcmp( address[0], "adjustRenderFraction" ) ) { // for all spots...
      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)     
         renderFraction=value;
         if (renderFraction>lsr.configTotalPoints) renderFraction=0;// update ALL the time (per-point)
         if (renderFraction<1) renderFraction=1;
      }
    }
    
    else if (!strcmp( address[0], "adjustPlusRenderFraction" ) ) {
      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)     
         renderFraction+=value;
         if (renderFraction>lsr.configTotalPoints) renderFraction=0;// meaning: update ALL the time (per-point)
      }
    }
    
    else if (!strcmp( address[0], "adjustMinusRenderFraction" ) ) {
      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)     
          renderFraction=((renderFraction-value)>=1? renderFraction-value : 1);
      }
    }
    

    
 // 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);
      }
    } 
    
    else if (!strcmp( address[0], "printParameters" ) ) {  
        pc.printf("Fraction display for the config: %d\n", renderFraction);
        blobconf.printParameters();
    } 
    
 // ===================== 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], "sendAnchorPos" ) ) {
        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]->sendingAnchorPosition=(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);

        }
    }
}

void changeMode(int mode) {

    strcpy(address[0],"");
   strcpy(address[1],"");
   data[0]=-1;
   data[1]=-1;

switch(mode) {
  case 0: 
   strcpy(address[0],"spot_following");
   data[0]=1;
   interpretCommand();
 break;
 case 1:
    strcpy(address[0],"spot_following");
   data[0]=2;
   interpretCommand();
 break;
 case 2:
  strcpy(address[0],"spot_bouncing");
   data[0]=1;
   interpretCommand();
 break;
 case 3:
  strcpy(address[0],"spot_bouncing");
   data[0]=4;
   interpretCommand();
 break;
 case 4:
 strcpy(address[0],"spot_lorentz");
   data[0]=1;
   interpretCommand();
 break;
 case 5:
 strcpy(address[0],"spot_lorentz");
   data[0]=2;
   interpretCommand();
 break;
 case 6:
  strcpy(address[0],"rain_mode");
  data[0]=2;
  interpretCommand();
 break;
 case 7:
  strcpy(address[0],"rain_mode");
  data[0]=5;
  interpretCommand();
 break;
case 8:
 strcpy(address[0],"air_hockey");
   data[0]=1;
   interpretCommand();
 break;
   case 9:
  strcpy(address[0],"spot_tracking");
  data[0]=1;
  interpretCommand();
   // here, let's make the min contrast smaller, for finger tracking:
   strcpy(address[0],"adjustContrast");
  data[0]=120;
  interpretCommand();
 break;
 case 10:
strcpy(address[0],"elastic_mouth");
interpretCommand();
 break;
 case 11:
 strcpy(address[0],"elastic_following");
 interpretCommand();
 break;
 case 12:
 strcpy(address[0],"elastic_mouth_small");
 interpretCommand();
 break;
 //case 9:
 //strcpy(address[0],"pac_man");
 //interpretCommand();
 //break;
 //case 10:
 //  strcpy(address[0],"circular_pong");
 //  data[0]=3;
 //  interpretCommand();
 //break;
 // case 10:
 // strcpy(address[0],"vertical_pinball");
 //  data[0]=2;
 //  interpretCommand();
 //pbreak;
 //case 11:
 //strcpy(address[0],"fish_net");
  // data[0]=3;
  // interpretCommand();
 //break;
 
 }
 
 }
 


// ================ 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());
}

*/