Alvaro Cassinelli
save loops
Diff: main.cpp
- Revision:
- 0:df6fdd9b99f0
- Child:
- 1:3be7b7d050f4
diff -r 000000000000 -r df6fdd9b99f0 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Dec 02 04:39:15 2014 +0000
@@ -0,0 +1,1103 @@
+#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], "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());
+}
+
+*/