Initial commit

1 files changed, 795 insertions, 0 deletions

diff --git a/OpenModelica-Arduino-MDD-Windows/tools/arduino-firmware/arduino-firmware.ino b/OpenModelica-Arduino-MDD-Windows/tools/arduino-firmware/arduino-firmware.ino
new file mode 100644
index 0000000..8d10650
--- /dev/null
+++ b/OpenModelica-Arduino-MDD-Windows/tools/arduino-firmware/arduino-firmware.ino
@@ -0,0 +1,795 @@
+
+  /* This file is meant to be used with the SCILAB arduino  
+   toolbox, however, it can be used from the IDE environment
+   (or any other serial terminal) by typing commands like:
+   
+   Conversion ascii -> number
+   48->'0' ... 57->'9' 58->':' 59->';' 60->'<' 61->'=' 62->'>' 63->'?' 64->'@' 
+   65->'A' ... 90->'Z' 91->'[' 92->'\' 93->']' 94->'^' 95->'_' 96->'`' 
+   97->'a' ... 122->'z'
+   
+   Dan0 or Dan1 : attach digital pin n (ascii from 2 to b) to input (0) or output (1)
+   Drn : read digital value (0 or 1) on pin n (ascii from 2 to b)
+   Dwn0 or Dwn1 : write 1 or 0 on pin n
+   An    : reads analog pin n (ascii from 0 to 19)
+   Wnm  : write analog value m (ascii from 0 to 255) on pin n (ascii from 0 to 19)
+   Sa1 or Sa2 : Attach servo 1 (digital pin 9) or 2 (digital pin 10)
+   Sw1n or Sw2n : moves servo 1 or servo 2 to position n (from ascii(0) to ascii(180))
+   Sd1 or Sd2 : Detach servo 1 or 2
+   
+   Generic DC_Motor
+   Cijkl : setup for generic DCmotor number i (1 to 4), PW1 on pin number j, PWM2 or direction on pin number k, mode=l
+           l=0 for L293 (2 PWM) and l=1 for L298 (1PWM + 1 bit for direction)
+   Mijk  : sets speed for generic DCmotor number i, j=0/1 for direction, k=ascii(0) .. ascii(255)
+   Mir   : releases motor i (r=release)
+
+   Generic Interrupt counter
+   Iai   : activate counter on INT number i (i=ascii(2 or 3 or 18 or 19 or 20 or 21)
+   Iri   : release counter on INT number i
+   Ipi   : read counter on INT number i
+   Izi   : reset counter on INT number i
+
+   Generic Encoder
+   Eajkl: activate encoder on channelA on INT number j (j=ascii(2 or 3 or 18 or 19 or 20 or 21) et channelB on pin k or INT number k (k=ascii(0)..ascii(53))
+           and l=1 or 2 or 4 for 1x mode (count every rising of chA) or 2x mode (count every change statement of chA)
+           or  4x mode (every change statement of chA et chB)
+   Eri   : release encoder on INTi
+   Epi   : read position of encoder on INTi
+   Ezi   : reset value of encoder on INTi position
+   
+   R0    : sets analog reference to DEFAULT
+   R1    : sets analog reference to INTERNAL
+   R2    : sets analog reference to EXTERNAL
+
+   */
+
+#include <Servo.h>
+
+/* define internal for the MEGA as 1.1V (as as for the 328)  */
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#define INTERNAL INTERNAL1V1
+#endif
+
+/* create and initialize servos                              */
+Servo servo1;
+Servo servo2;
+
+/* Generic motors */
+int dcm1_pin1,dcm1_pin2,dcm1_mode;
+int dcm2_pin1,dcm2_pin2,dcm2_mode;
+int dcm3_pin1,dcm3_pin2,dcm3_mode;
+int dcm4_pin1,dcm4_pin2,dcm4_mode;
+
+// Generic encoder 
+/* Encoders initialisation */
+// volatile declare as those variables will change in interrupts
+volatile long int encoder_0_position = 0,encoder_1_position = 0, encoder_2_position = 0, encoder_3_position = 0, encoder_4_position = 0, encoder_5_position = 0;
+int encoder_0_int2 ;          // Pin used for encoder0 chanel B : define from scilab
+int encoder_1_int2 ;          // Pin used for encoder1 chanel B : define from scilab
+int encoder_2_int2 ;          // Pin used for encoder2 chanel B : define from scilab
+int encoder_3_int2 ;          // Pin used for encoder3 chanel B : define from scilab
+int encoder_4_int2 ;          // Pin used for encoder4 chanel B : define from scilab
+int encoder_5_int2 ;          // Pin used for encoder5 chanel B : define from scilab
+int encoder_num, encoder_int2;
+int corresp[6]={2,3,21,20,19,18}; //Correspondance beetween interrupt number and pin number
+
+//Generic counter
+volatile long int counter_0=0,counter_1=0,counter_2=0,counter_3=0,counter_4=0,counter_5=0;
+
+int initiat=1;
+
+void setup() {
+  /* initialize serial                                       */
+  Serial.begin(115200);
+  
+}
+void loop() {
+  
+  
+   
+  /* variables declaration and initialization                */
+  
+  static int  s   = -1;    /* state                          */
+  static int  pin = 13;    /* generic pin number             */
+  static int  dcm =  4;    /* generic dc motor number        */
+
+  int  val =  0;           /* generic value read from serial */
+  int  agv =  0;           /* generic analog value           */
+  int  dgv =  0;           /* generic digital value          */
+  static int  enc   = 1;    /* encoder number 1 (or 2 for Arduino mega)     */
+  
+  while (Serial.available()==0) {}; // Waiting char
+  val = Serial.read(); 
+  
+  //Checking for availability of firmware
+  if(val==118)
+  {
+    delay(10);
+    Serial.write("ok"); 
+  }
+  
+//  if (val==0){// version
+//    Serial.print('v3');
+//    val=-1;
+//  }
+  //case A -> Analog
+  else if (val==65){//A -> Analog read
+    while (Serial.available()==0) {}; // Waiting char
+//    val=Serial.read();
+//    if (val==114){ //'r'-> read pin
+//       while (Serial.available()==0) {}; // Waiting char
+       val=Serial.read();
+       if (val>47 && val<67) { //from pin 0, to pin 19
+          pin=val-48; //number of the pin
+          agv=analogRead(pin);
+          //Serial.println(agv);
+          Serial.write((uint8_t*)&agv,2); /* send binary value via serial  */   
+       }
+       val=-1;
+  }
+  else if (val==87){//W -> Analog write
+      while (Serial.available()==0) {}; // Waiting char
+      val=Serial.read();
+         if (val>47 && val<67) { //from pin 0 to pin 19
+            pin=val-48; //number of the pin
+            while (Serial.available()==0) {}; // Waiting char
+            val=Serial.read();
+            analogWrite(pin,val);
+         }
+          val=-1;
+      }
+  //}
+
+  //case D -> Digital
+  else if (val==68){//D -> Digital pins
+    while (Serial.available()==0) {}; // Waiting char
+    val=Serial.read();
+    if (val==97){ //'a'-> declare pin
+       while (Serial.available()==0) {}; // Waiting char
+       val=Serial.read();
+       if (val>49 && val<102) {
+         pin=val-48;
+         while (Serial.available()==0) {}; // Waiting char
+         val=Serial.read();
+         if (val==48 || val==49) { 
+            if (val==48){//'0' -> input
+               pinMode(pin,INPUT);
+            }
+            else if (val==49){//'1' -> output
+               pinMode(pin,OUTPUT);
+            }
+         }
+       }
+    }
+    if (val==114){ //'r'-> read pin
+       while (Serial.available()==0) {}; // Waiting char
+       val=Serial.read();
+       if (val>49 && val<102) { 
+          pin=val-48; //number of the digital pin
+          dgv=digitalRead(pin);      
+//          Serial.println(dgv);        
+          Serial.print(dgv);         
+       }
+    }
+    if (val==119){ //'w'-> write pin
+       while (Serial.available()==0) {}; // Waiting char
+       val=Serial.read();
+       if (val>49 && val<102) { 
+          pin=val-48; //number of the digital pin
+          while (Serial.available()==0) {}; // Waiting char
+          val=Serial.read();
+          if (val==48 || val==49) { // 0 or 1
+            dgv=val-48;
+            digitalWrite(pin,dgv);
+//            Serial.println(dgv);    
+          }
+       }
+    }
+   val=-1;
+
+  }
+  //case S -> servomotor
+  else if (val==83){
+    while (Serial.available()==0) {}; // Waiting char
+    val=Serial.read();
+    if (val==97){ //'a'-> declare servo
+       while (Serial.available()==0) {}; // Waiting char
+       val=Serial.read();
+       if (val==49 || val==50) { //servo 1 or 2
+          pin=val-48; //number of the servo
+          if (pin==1) {
+            servo1.attach(9); 
+            servo1.write(0);           
+//            agv=servo1.read();
+//            Serial.println(agv);
+          }
+          if (pin==2) {
+            servo2.attach(10); 
+            servo2.write(0);           
+//            agv=servo2.read();
+//            Serial.println(agv);
+          }
+
+       }
+    }
+    if (val==100){ //'d'-> detach servo
+       while (Serial.available()==0) {}; // Waiting char
+       val=Serial.read();
+       if (val==49 || val==50) { //servo 1 or 2
+          pin=val-48; //number of the servo
+          if (pin==1) {servo1.detach(); }
+          if (pin==2) {servo2.detach(); }
+       }
+    }
+    if (val==119){ //'w'-> write pin
+       while (Serial.available()==0) {}; // Waiting char
+       val=Serial.read();
+       if (val==49 || val==50) { //servo 1 or 2
+          pin=val-48; //number of the servo
+          while (Serial.available()==0) {}; // Waiting char
+          val=Serial.read();
+          if (val>=0 && val<=180){
+              if (pin==1) {
+                servo1.write(val);
+//                agv=servo1.read();
+//                Serial.println(agv);
+              }
+              if (pin==2) {
+                servo2.write(val);
+//                agv=servo2.read();
+//                Serial.println(agv);
+              }
+          }
+       }
+    }
+   val=-1;
+
+  }
+  
+  //case I -> Interrupt
+  else if (val==73){
+      /* ASKING ACTIVATION OF AN COUNTER     */
+      while (Serial.available()==0) {}; // Waiting char
+      val=Serial.read();                                
+      if (val==97) { //a = activation
+         while (Serial.available()==0) {};                 // Waiting char
+         val=Serial.read();                                // Read int_number (must be 0 or 1 on UNO / 1 to 5 on MEGA) : int_number set to encoder number
+         pinMode(corresp[val],INPUT);              // set interrupt pin as input
+         if       (val == 0) {attachInterrupt(val, counter_0_change, RISING);counter_0=0;}  //counter INT0
+         else if  (val == 1) {attachInterrupt(val, counter_1_change, RISING);counter_1=0;}  //counter INT1
+         else if  (val == 2) {attachInterrupt(val, counter_2_change, RISING);counter_2=0;}  //counter INT2
+         else if  (val == 3) {attachInterrupt(val, counter_3_change, RISING);counter_3=0;}  //counter INT3
+         else if  (val == 4) {attachInterrupt(val, counter_4_change, RISING);counter_4=0;}  //counter INT4
+         else if  (val == 5) {attachInterrupt(val, counter_5_change, RISING);counter_5=0;}  //counter INT5
+      }
+      /* ASKING POSITION OF A COUNTER     */ 
+      if (val==112) { //p = sending counting value
+         while (Serial.available()==0) {};   // Waiting char
+         val = Serial.read()   ;             //reading next value = counter number
+         if      (val==0){ Serial.write((uint8_t*)&counter_0,4); }// asking counter 0 
+         else if (val==1){ Serial.write((uint8_t*)&counter_1,4); }// asking counter 1 
+         else if (val==2){ Serial.write((uint8_t*)&counter_2,4); }// asking counter 2 
+         else if (val==3){ Serial.write((uint8_t*)&counter_3,4); }// asking counter 3 
+         else if (val==4){ Serial.write((uint8_t*)&counter_4,4); }// asking counter 4 
+         else if (val==5){ Serial.write((uint8_t*)&counter_5,4); }// asking counter 5 
+     }
+      /* ASKING RELEASE OF AN INTERRUPT   */ 
+      if (val==114) { //r = release counter 
+         while (Serial.available()==0) {};        // Waiting char
+         val = Serial.read();                     //reading next value = counter number
+         detachInterrupt(val);                    // Detach interrupt on chanel A of counter num=val
+         if (val==0)       { counter_0=0;}        // Reset counter
+         else if (val==1)  { counter_1=0;}        // Reset counter
+         else if (val==2)  { counter_2=0;}        // Reset counter
+         else if (val==3)  { counter_3=0;}        // Reset counter
+         else if (val==4)  { counter_4=0;}        // Reset counter
+         else if (val==5)  { counter_5=0;}        // Reset counter
+      }
+      /* ASKING RESET VALUE OF AN COUNTER     */ 
+      if (val==122) { //z set to zero
+         while (Serial.available()==0) {};        // Waiting char
+         val = Serial.read();                     //reading next value = counter number
+         if (val==0)       { counter_0=0;}        // Reset counter
+         else if (val==1)  { counter_1=0;}        // Reset counter
+         else if (val==2)  { counter_2=0;}        // Reset counter
+         else if (val==3)  { counter_3=0;}        // Reset counter
+         else if (val==4)  { counter_4=0;}        // Reset counter
+         else if (val==5)  { counter_5=0;}        // Reset counter
+       }
+    val=-1;
+
+  } 
+  
+  //case E -> Encoder
+  else if (val==69){    
+      /*Generic encoder functions */
+      while (Serial.available()==0) {};
+      val=Serial.read();  
+      /* ASKING ACTIVATION OF AN ENCODER     */
+      if (val==97) {                                       //activation
+         while (Serial.available()==0) {};                 // Waiting char
+         encoder_num=Serial.read();                        // Read int_number (must be 0 or 1 on UNO / 1 to 5 on MEGA) : int_number set to encoer number
+         pinMode(corresp[encoder_num],INPUT);              // set interrupt pin as input
+         while (Serial.available()==0) {};                 // Waiting char
+         encoder_int2=Serial.read();                       // Read int2 (must be a digital PIN with interrupt or not : depends on mode)
+                                                           // no declaration for the moment : wait for encoder mode
+         while (Serial.available()==0) {};                 // Waiting char
+         int mode = Serial.read()-48;                      // Read mode 1 ou 2 (1 counting only rising of chA, 2 counting rising and falling)
+         if (mode == 4) {                                  // mode 4x : 2 cases : chA=pin2 / chB=pin3 or chA=pin3/chB=pin2 [Uno retriction]
+            pinMode(corresp[encoder_int2],INPUT);          // set interrupt number as input
+         } else {
+            pinMode(encoder_int2,INPUT);                   // set pin as input
+         }
+         
+         if (encoder_num == 0) {                             //encoder INT0
+           encoder_0_position=0;                             // Reset position
+           if (mode==4) {
+             encoder_0_int2=corresp[encoder_int2];           // Save pin of second interruption
+             attachInterrupt(encoder_num , encoder_change_m4_A0, CHANGE); // Attach interrupt on chanel A change
+             attachInterrupt(encoder_int2, encoder_change_m4_B0, CHANGE); // Attach interrupt on chanel B change
+           } else if (mode==2) {
+             encoder_0_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_0_change_m2, CHANGE); // Attach interrupt on chanel A change
+           } else if (mode==1) {
+             encoder_0_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_0_change_m1, RISING); // Attach interrupt on chanel A rising
+           }
+         } else if (encoder_num == 1) {                      //encoder INT1
+           encoder_1_position=0;                             // Reset position
+           if (mode==4) {
+             encoder_1_int2=corresp[encoder_int2];           // Save pin of second interruption
+             attachInterrupt(encoder_num , encoder_change_m4_A1, CHANGE); // Attach interrupt on chanel A change
+             attachInterrupt(encoder_int2, encoder_change_m4_B1, CHANGE); // Attach interrupt on chanel B change
+           } else if (mode==2) {
+             encoder_1_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_1_change_m2, CHANGE); // Attach interrupt on chanel A change
+           } else if (mode==1) {
+             encoder_1_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_1_change_m1, RISING); // Attach interrupt on chanel A rising
+           }
+         } else if (encoder_num == 2) {                      //encoder INT2
+           encoder_2_position=0;                             // Reset position
+           if (mode==4) {
+             encoder_2_int2=corresp[encoder_int2];           // Save pin of second interruption
+             attachInterrupt(encoder_num , encoder_change_m4_A2, CHANGE); // Attach interrupt on chanel A change
+             attachInterrupt(encoder_int2, encoder_change_m4_B2, CHANGE); // Attach interrupt on chanel B change
+           } else if (mode==2) {
+             encoder_2_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_2_change_m2, CHANGE); // Attach interrupt on chanel A change
+           } else if (mode==1) {
+             encoder_2_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_2_change_m1, RISING); // Attach interrupt on chanel A rising
+           }
+         } else if (encoder_num == 3) {                      //encoder INT3
+           encoder_3_position=0;                             // Reset position
+           if (mode==4) {
+             encoder_3_int2=corresp[encoder_int2];           // Save pin of second interruption
+             attachInterrupt(encoder_num , encoder_change_m4_A3, CHANGE); // Attach interrupt on chanel A change
+             attachInterrupt(encoder_int2, encoder_change_m4_B3, CHANGE); // Attach interrupt on chanel B change
+           } else if (mode==2) {
+             encoder_3_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_3_change_m2, CHANGE); // Attach interrupt on chanel A change
+           } else if (mode==1) {
+             encoder_3_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_3_change_m1, RISING); // Attach interrupt on chanel A rising
+           }
+         } else if (encoder_num == 4) {                      //encoder INT4
+           encoder_4_position=0;                             // Reset position
+           if (mode==4) {
+             encoder_4_int2=corresp[encoder_int2];           // Save pin of second interruption
+             attachInterrupt(encoder_num , encoder_change_m4_A4, CHANGE); // Attach interrupt on chanel A change
+             attachInterrupt(encoder_int2, encoder_change_m4_B4, CHANGE); // Attach interrupt on chanel B change
+           } else if (mode==2) {
+             encoder_4_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_4_change_m2, CHANGE); // Attach interrupt on chanel A change
+           } else if (mode==1) {
+             encoder_4_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_4_change_m1, RISING); // Attach interrupt on chanel A rising
+           }
+         } else if (encoder_num == 5) {                      //encoder INT5
+           encoder_5_position=0;                             // Reset position
+           if (mode==4) {
+             encoder_5_int2=corresp[encoder_int2];           // Save pin of second interruption
+             attachInterrupt(encoder_num , encoder_change_m4_A5, CHANGE); // Attach interrupt on chanel A change
+             attachInterrupt(encoder_int2, encoder_change_m4_B5, CHANGE); // Attach interrupt on chanel B change
+           } else if (mode==2) {
+             encoder_5_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_5_change_m2, CHANGE); // Attach interrupt on chanel A change
+           } else if (mode==1) {
+             encoder_5_int2=encoder_int2;
+             attachInterrupt(encoder_num, encoder_5_change_m1, RISING); // Attach interrupt on chanel A rising
+           }
+         }
+      }
+      /* ASKING POSITION OF AN ENCODER     */ 
+      if (val==112) { //p = sending encoder position
+         while (Serial.available()==0) {};   // Waiting char
+         val = Serial.read()   ;             //reading next value = encoder number
+         if      (val==0){ Serial.write((uint8_t*)&encoder_0_position,4); }// asking encoder 0 position
+         else if (val==1){ Serial.write((uint8_t*)&encoder_1_position,4); }// asking encoder 1 position
+         else if (val==2){ Serial.write((uint8_t*)&encoder_2_position,4); }// asking encoder 2 position
+         else if (val==3){ Serial.write((uint8_t*)&encoder_3_position,4); }// asking encoder 3 position
+         else if (val==4){ Serial.write((uint8_t*)&encoder_4_position,4); }// asking encoder 4 position
+         else if (val==5){ Serial.write((uint8_t*)&encoder_5_position,4); }// asking encoder 5 position
+     }
+      /* ASKING RELEASE OF AN ENCODER     */ 
+      if (val==114) { //r = release encoder 
+         while (Serial.available()==0) {};                 // Waiting char
+         val = Serial.read();                              //reading next value = encoder number
+         detachInterrupt(val);                             // Detach interrupt on chanel A of encoder num=val
+         if (val==0)       { encoder_0_position=0;encoder_0_int2=-1;}        // Reset position
+         else if (val==1)  { encoder_1_position=0;encoder_1_int2=-1;}        // Reset position
+         else if (val==2)  { encoder_2_position=0;encoder_2_int2=-1;}        // Reset position
+         else if (val==3)  { encoder_3_position=0;encoder_3_int2=-1;}        // Reset position
+         else if (val==4)  { encoder_4_position=0;encoder_4_int2=-1;}        // Reset position
+         else if (val==5)  { encoder_5_position=0;encoder_5_int2=-1;}        // Reset position
+         while (Serial.available()==0) {};                 // Waiting char
+         val = Serial.read();                              // reading next value = encoder number
+         detachInterrupt(val);                             // Detach interrupt on chanel B of encoder num=val (may be the same if mode=1 or 2)
+      }
+      /* ASKING RESET POSITION OF AN ENCODER     */ 
+      if (val==122) {  // z = encoder position to zero
+         while (Serial.available()==0) {};                 // Waiting char
+         val = Serial.read();                              //reading next value = encoder number
+         if (val==0)       { encoder_0_position=0;}        // Reset position
+         else if (val==1)  { encoder_1_position=0;}        // Reset position
+         else if (val==2)  { encoder_2_position=0;}        // Reset position
+         else if (val==3)  { encoder_3_position=0;}        // Reset position
+         else if (val==4)  { encoder_4_position=0;}        // Reset position
+         else if (val==5)  { encoder_5_position=0;}        // Reset position
+      }
+     val=-1;
+
+  }
+  
+  //case C -> DCmotor init
+  else if(val==67){
+    while (Serial.available()==0) {};                
+    val = Serial.read();
+    /* 2nd char = motor number */
+    if (val>48 && val<53) {
+        dcm=val-48;                
+        while (Serial.available()==0) {};                
+        val = Serial.read();      
+        /* the third received value indicates the pin1 number from ascii(2)=50 to ascii(e)=101  */
+        if (val>49 && val<102) {
+          if (dcm==1) dcm1_pin1=val-48;/* calculate motor pin1 */
+          if (dcm==2) dcm2_pin1=val-48;/* calculate motor pin1 */
+          if (dcm==3) dcm3_pin1=val-48;/* calculate motor pin1 */
+          if (dcm==4) dcm4_pin1=val-48;/* calculate motor pin1 */
+          pinMode(val-48, OUTPUT); //set pin as output
+          analogWrite(val-48,0);   /* DUTY CYCLE */
+          while (Serial.available()==0) {};                
+          val = Serial.read();
+          /* the fourth received value indicates the pin2 number from ascii(2)=50 to ascii(e)=101  */
+          if (val>49 && val<102) {
+            if (dcm==1) dcm1_pin2=val-48;/* calculate motor pin2 */
+            if (dcm==2) dcm2_pin2=val-48;/* calculate motor pin2 */
+            if (dcm==3) dcm3_pin2=val-48;/* calculate motor pin2 */
+            if (dcm==4) dcm4_pin2=val-48;/* calculate motor pin2 */
+            pinMode(val-48, OUTPUT); //set pin as output
+            while (Serial.available()==0) {};                
+            val = Serial.read();
+            /* the fifth received value indicates the pin2 number from ascii(2)=50 to ascii(e)=101 */
+            if (val>47 && val<50) {
+              int mode = val-48;
+              if (dcm==1) dcm1_mode=mode;/* calculate motor mode */
+              if (dcm==2) dcm2_mode=mode;/* calculate motor mode */
+              if (dcm==3) dcm3_mode=mode;/* calculate motor mode */
+              if (dcm==4) dcm4_mode=mode;/* calculate motor mode */
+              //initialization of port
+              if(mode==0){//L293
+                if (dcm==1) analogWrite(dcm1_pin2,0);   /* DUTY CYCLE */
+                if (dcm==2) analogWrite(dcm2_pin2,0);   /* DUTY CYCLE */
+                if (dcm==3) analogWrite(dcm3_pin2,0);   /* DUTY CYCLE */
+                if (dcm==4) analogWrite(dcm4_pin2,0);   /* DUTY CYCLE */
+              } else if (mode==1) {//L297
+                if (dcm==1) digitalWrite(dcm1_pin2, LOW);  /* DIRECTION */
+                if (dcm==2) digitalWrite(dcm2_pin2, LOW);  /* DIRECTION */
+                if (dcm==3) digitalWrite(dcm3_pin2, LOW);  /* DIRECTION */
+                if (dcm==4) digitalWrite(dcm4_pin2, LOW);  /* DIRECTION */      
+              }
+              Serial.print("OK"); // tell Scilab that motor s initialization finished
+                                  // Cette commande sert à rien dans la toolbox de base,
+                                  // sauf si on prévoit d'ajouter des actions à l'init des moteurs
+                                  // par exemple chercher la position d'origine !
+            }
+          }
+        }
+    }
+    val=-1;
+
+  }
+
+  //case M -> DC motor
+  else if(val==77){
+      while (Serial.available()==0) {};                
+      val = Serial.read();
+      /* the second received value indicates the motor number
+         from abs('1')=49, motor1, to abs('4')=52, motor4    */
+      if (val>48 && val<53) {
+        dcm=val-48;                /* calculate motor number */
+        while (Serial.available()==0) {};       // Waiting char
+        val = Serial.read();  
+        /* the third received value indicates the sens direction or release*/
+        if (val==48 || val ==49){
+            int direction=val-48;
+            while (Serial.available()==0) {};       // Waiting char
+            val = Serial.read();                    //reading next value = 0..255 
+            if (dcm==1){
+             if(dcm1_mode==0){//L293 
+              if(direction==1){
+                analogWrite(dcm1_pin1,val);
+                analogWrite(dcm1_pin2,0);
+              } else {
+                analogWrite(dcm1_pin2,val);
+                analogWrite(dcm1_pin1,0);
+              }
+             } else {//L298
+              if (direction==0) digitalWrite(dcm1_pin2,LOW);
+              if (direction==1) digitalWrite(dcm1_pin2,HIGH);
+              analogWrite(dcm1_pin1,val);
+             }
+            }
+            if (dcm==2){
+             if(dcm2_mode==0){//L293 
+              if(direction==1){
+                analogWrite(dcm2_pin1,val);
+                analogWrite(dcm2_pin2,0);
+              } else {
+                analogWrite(dcm2_pin2,val);
+                analogWrite(dcm2_pin1,0);
+              }
+             } else {//L298
+              if (direction==0) digitalWrite(dcm2_pin2,LOW);
+              if (direction==1) digitalWrite(dcm2_pin2,HIGH);
+              analogWrite(dcm2_pin1,val);
+             }
+            }
+            if (dcm==3){
+             if(dcm3_mode==0){//L293 
+              if(direction==1){
+                analogWrite(dcm3_pin1,val);
+                analogWrite(dcm3_pin2,0);
+              } else {
+                analogWrite(dcm3_pin2,val);
+                analogWrite(dcm3_pin1,0);
+              }
+             } else {//L298
+              if (direction==0) digitalWrite(dcm3_pin2,LOW);
+              if (direction==1) digitalWrite(dcm3_pin2,HIGH);
+              analogWrite(dcm3_pin1,val);
+             }
+            }
+            if (dcm==4){
+             if(dcm4_mode==0){//L293 
+              if(direction==1){
+                analogWrite(dcm4_pin1,val);
+                analogWrite(dcm4_pin2,0);
+              } else {
+                analogWrite(dcm4_pin2,val);
+                analogWrite(dcm4_pin1,0);
+              }
+             } else {//L298
+              if (direction==0) digitalWrite(dcm4_pin2,LOW);
+              if (direction==1) digitalWrite(dcm4_pin2,HIGH);
+              analogWrite(dcm4_pin1,val);
+             }
+            }
+         }
+         if (val==114){//release motor
+           if(dcm==1) {
+           analogWrite(dcm1_pin1,0);
+           if(dcm1_mode==0)  analogWrite(dcm1_pin2,0); 
+            }
+          if(dcm==2) {
+           analogWrite(dcm2_pin1,0);
+           if(dcm2_mode==0)  analogWrite(dcm2_pin2,0); 
+          }
+          if(dcm==3) {
+           analogWrite(dcm3_pin1,0);
+           if(dcm3_mode==0)  analogWrite(dcm3_pin2,0); 
+          }
+          if(dcm==4) {
+           analogWrite(dcm4_pin1,0);
+           if(dcm4_mode==0)  analogWrite(dcm4_pin2,0); 
+          }  
+         }
+         
+      }
+   val=-1;
+
+  }
+
+
+  //case R -> Analog reference
+  if(val==82){
+    while (Serial.available()==0) {};                
+    val = Serial.read();
+    if (val==48) analogReference(DEFAULT);
+    if (val==49) analogReference(INTERNAL);
+    if (val==50) analogReference(EXTERNAL);
+    if (val==51) Serial.print("v3");
+    val=-1;
+  }
+  
+} /* end loop statement                                      */
+
+
+/**************************************/
+// Generic interrupt encoder functions//
+/**************************************/
+//Encoder on INT0
+void encoder_0_change_m1() { //encoder0 mode 1x
+  int chB=digitalRead(encoder_0_int2);
+  if (!chB) { encoder_0_position++;}
+  else { encoder_0_position--; }
+}
+void encoder_0_change_m2() { //encoder0 mode 2x
+  int chB=digitalRead(encoder_0_int2);
+  int chA=digitalRead(corresp[0]);
+  if ((chA & !chB)|(!chA & chB)) { encoder_0_position++; }
+  else { encoder_0_position--; }
+}
+void encoder_change_m4_A0(){//encoder0 mode 4x chA
+  int chA=digitalRead(corresp[0]);
+  int chB=digitalRead(encoder_0_int2);
+  if ((chA & !chB)|(!chA & chB)) { encoder_0_position++; }
+  else { encoder_0_position--; }
+}
+void encoder_change_m4_B0(){//encoder0 mode 4x chB
+  int chA=digitalRead(corresp[0]);
+  int chB=digitalRead(encoder_0_int2);
+  if ((!chA & !chB)|(chA & chB)) { encoder_0_position++; }
+  else { encoder_0_position--; }
+}
+//Encoder on INT1
+void encoder_1_change_m1() { //encoder1 mode 1x
+  int chB=digitalRead(encoder_1_int2);
+  if (!chB) { encoder_1_position++;}
+  else { encoder_1_position--; }
+}
+void encoder_1_change_m2() { //encoder1 mode 2x
+  int chB=digitalRead(encoder_1_int2);
+  int chA=digitalRead(corresp[1]);
+  if ((chA & !chB)|(!chA & chB)) { encoder_1_position++; }
+  else { encoder_1_position--; }
+}
+void encoder_change_m4_A1(){//encoder1 mode 4x chA
+  int chA=digitalRead(corresp[1]);
+  int chB=digitalRead(encoder_1_int2);
+  if ((chA & !chB)|(!chA & chB)) { encoder_1_position++; }
+  else { encoder_1_position--; }
+}
+void encoder_change_m4_B1(){//encoder1 mode 4x chB
+  int chA=digitalRead(corresp[1]);
+  int chB=digitalRead(encoder_1_int2);
+  if ((!chA & !chB)|(chA & chB)) { encoder_1_position++; }
+  else { encoder_1_position--; }
+}
+//Encoder on INT2
+void encoder_2_change_m1() { //encoder2 mode 1x
+  int chB=digitalRead(encoder_2_int2);
+  if (!chB) { encoder_2_position++;}
+  else { encoder_2_position--; }
+}
+void encoder_2_change_m2() { //encoder2 mode 2x
+  int chB=digitalRead(encoder_2_int2);
+  int chA=digitalRead(corresp[2]);
+  if ((chA & !chB)|(!chA & chB)) { encoder_2_position++; }
+  else { encoder_2_position--; }
+}
+void encoder_change_m4_A2(){//encoder2 mode 4x chA
+  int chA=digitalRead(corresp[2]);
+  int chB=digitalRead(encoder_2_int2);
+  if ((chA & !chB)|(!chA & chB)) { encoder_2_position++; }
+  else { encoder_2_position--; }
+}
+void encoder_change_m4_B2(){//encoder2 mode 4x chB
+  int chA=digitalRead(corresp[2]);
+  int chB=digitalRead(encoder_2_int2);
+  if ((!chA & !chB)|(chA & chB)) { encoder_2_position++; }
+  else { encoder_2_position--; }
+}
+//Encoder on INT3
+void encoder_3_change_m1() { //encoder3 mode 1x
+  int chB=digitalRead(encoder_3_int2);
+  if (!chB) { encoder_3_position++;}
+  else { encoder_3_position--; }
+}
+void encoder_3_change_m2() { //encoder3 mode 2x
+  int chB=digitalRead(encoder_3_int2);
+  int chA=digitalRead(corresp[3]);
+  if ((chA & !chB)|(!chA & chB)) { encoder_3_position++; }
+  else { encoder_3_position--; }
+}
+void encoder_change_m4_A3(){//encoder3 mode 4x chA
+  int chA=digitalRead(corresp[3]);
+  int chB=digitalRead(encoder_3_int2);
+  if ((chA & !chB)|(!chA & chB)) { encoder_3_position++; }
+  else { encoder_3_position--; }
+}
+void encoder_change_m4_B3(){//encoder3 mode 4x chB
+  int chA=digitalRead(corresp[3]);
+  int chB=digitalRead(encoder_3_int2);
+  if ((!chA & !chB)|(chA & chB)) { encoder_3_position++; }
+  else { encoder_3_position--; }
+}
+//Encoder on INT4
+void encoder_4_change_m1() { //encoder4 mode 1x
+  int chB=digitalRead(encoder_4_int2);
+  if (!chB) { encoder_4_position++;}
+  else { encoder_4_position--; }
+}
+void encoder_4_change_m2() { //encoder4 mode 2x
+  int chB=digitalRead(encoder_4_int2);
+  int chA=digitalRead(corresp[4]);
+  if ((chA & !chB)|(!chA & chB)) { encoder_4_position++; }
+  else { encoder_4_position--; }
+}
+void encoder_change_m4_A4(){//encoder4 mode 4x chA
+  int chA=digitalRead(corresp[4]);
+  int chB=digitalRead(encoder_4_int2);
+  if ((chA & !chB)|(!chA & chB)) { encoder_4_position++; }
+  else { encoder_4_position--; }
+}
+void encoder_change_m4_B4(){//encoder4 mode 4x chB
+  int chA=digitalRead(corresp[4]);
+  int chB=digitalRead(encoder_4_int2);
+  if ((!chA & !chB)|(chA & chB)) { encoder_4_position++; }
+  else { encoder_4_position--; }
+}
+//Encoder on INT5
+void encoder_5_change_m1() { //encoder5 mode 1x
+  int chB=digitalRead(encoder_5_int2);
+  if (!chB) { encoder_5_position++;}
+  else { encoder_5_position--; }
+}
+void encoder_5_change_m2() { //encoder5 mode 2x
+  int chB=digitalRead(encoder_5_int2);
+  int chA=digitalRead(corresp[5]);
+  if ((chA & !chB)|(!chA & chB)) { encoder_5_position++; }
+  else { encoder_5_position--; }
+}
+void encoder_change_m4_A5(){//encoder5 mode 4x chA
+  int chA=digitalRead(corresp[5]);
+  int chB=digitalRead(encoder_5_int2);
+  if ((chA & !chB)|(!chA & chB)) { encoder_5_position++; }
+  else { encoder_5_position--; }
+}
+void encoder_change_m4_B5(){//encoder5 mode 4x chB
+  int chA=digitalRead(corresp[5]);
+  int chB=digitalRead(encoder_5_int2);
+  if ((!chA & !chB)|(chA & chB)) { encoder_5_position++; }
+  else { encoder_5_position--; }
+}
+
+/**************************************/
+// Generic interrupt counter functions//
+/**************************************/
+//Counter on INT0
+void counter_0_change() { //counter 0
+  counter_0++;
+}
+//Counter on INT1
+void counter_1_change() { //counter 1
+  counter_1++;
+}
+//Counter on INT2
+void counter_2_change() { //counter 2
+  counter_2++;
+}
+//Counter on INT3
+void counter_3_change() { //counter 3
+  counter_3++;
+}
+//Counter on INT4
+void counter_4_change() { //counter 4
+  counter_4++;
+}
+//Counter on INT5
+void counter_5_change() { //counter 5
+  counter_5++;
+}
+
+