Index: /branches/atorf/NXC/MotorControl2/ControllerCore.nxc
===================================================================
--- /branches/atorf/NXC/MotorControl2/ControllerCore.nxc	(revision 678)
+++ /branches/atorf/NXC/MotorControl2/ControllerCore.nxc	(revision 681)
@@ -416,4 +416,5 @@
                         // again no need to care about synced driving
                         DisableSpeedRegWhithMotorOn(port);
+                        Wait(1); // for FW to process
                     }//end if
                     //~~~~~~~~~~~~~~~~~~ same code until here
@@ -435,4 +436,5 @@
                         // again no need to care about synced driving
                         DisableSpeedRegWhithMotorOn(port);
+                        Wait(1); // for FW to process
                     }//end if
                     //~~~~~~~~~~~~~~~~~~ same code until here
Index: /branches/atorf/NXC/MotorControl2/MotorControl20.nxc
===================================================================
--- /branches/atorf/NXC/MotorControl2/MotorControl20.nxc	(revision 678)
+++ /branches/atorf/NXC/MotorControl2/MotorControl20.nxc	(revision 681)
@@ -76,17 +76,4 @@
 };
 
-
-struct RotateMotorInfo {
-    int power;
-    long angle;
-    int turnratio;
-    bool syncmode;
-    bool endbrake;
-};
-
-RotateMotorInfo motorA;
-RotateMotorInfo motorB;
-RotateMotorInfo motorC;
-RotateMotorInfo motorsSync;
 
 byte SyncPorts;
@@ -290,4 +277,5 @@
         i++;
         i = j;
+        j = MotorTachoCount(OUT_A);
     }//end while
 }//end task
@@ -299,4 +287,5 @@
         i++;
         i = j;
+        j = MotorTachoCount(OUT_B);
     }//end while
 }//end task
@@ -308,7 +297,29 @@
         i++;
         i = j;
+        j = MotorTachoCount(OUT_C);
     }//end while
 }//end task
 
+task dummyD() {
+    int i = 0;
+    int j = 0;
+    while(true){
+        i++;
+        i = j;
+        j = MotorTachoCount(OUT_A);
+    }//end while
+}//end task
+
+task dummyE() {
+    int i = 0;
+    int j = 0;
+    while(true){
+        i++;
+        i = j;
+        j = MotorTachoCount(OUT_B);
+    }//end while
+}//end task
+
+
 
 
@@ -320,5 +331,5 @@
 
 
-task TESTmain() {
+task main() {
 
      byte port = OUT_B;
@@ -334,7 +345,9 @@
 
     // launch 3 dummy tasks
-    start dummyA;
-    start dummyB;
+    //start dummyA;
+    //start dummyB;
     //start dummyC;
+    //start dummyD;
+    //start dummyE;
     Wait(20);
 
@@ -350,8 +363,31 @@
 // **** Try out new function
     //RunMotor(const byte &port, const int &power, const long &tacholimit, const bool &speedreg, const bool &holdbrake, const bool &smoothstart)
-    RunMotor(port, 100, 1000, true, false, false);
+    //RunMotor(port, 100, 1000, true, false, false);
     //RunMotor2(OUT_B, 100, 1000, false, false, true, OUT_C);
 
 
+    motorParamsB.power = 100;
+    motorParamsB.tacholimit = 1000;
+    motorParamsB.speedreg = false;
+    motorParamsB.holdbrake = false;
+    motorParamsB.smoothstart = true;
+    
+    motorParamsC.power = 100;
+    motorParamsC.tacholimit = 1000;
+    motorParamsC.speedreg = false;
+    motorParamsC.holdbrake = false;
+    motorParamsC.smoothstart = true;
+
+    start MoveB;
+    start MoveC;
+    
+    //Wait(4000);
+
+    //NumOut(0, LCD_LINE1, MotorTachoCount(OUT_B), true);
+    //NumOut(0, LCD_LINE2, MotorTachoCount(OUT_C));
+
+
+    
+
 
 }//end task
@@ -361,5 +397,5 @@
 
 
-task main(){
+task NEWmain(){
 
     // parameter initialization