Moduler le temps ecoulé avant que le moteur ne se replace

teensycontrol/com.cpp

@@ -3,7 +3,7 @@
 
 void initCom() {
   Serial.begin(115200);
-  Serial1.begin(1000000);
+  Serial1.begin(115200);
 }
 
 void printSerialCommandsAvailable() {
@@ -176,4 +176,4 @@ void printCmdValVal( String cmd, int32_t val1, int32_t val2) {
   Serial.print(" ");
   Serial.print(val2);
   Serial.print("\n");
-}
+}

teensycontrol/com.h

@@ -11,4 +11,4 @@ void parseSerialBuffer();
 void printCmdVal( String cmd, int32_t val);
 void printCmdValVal( String cmd, int32_t val1, int32_t val2);
 
-#endif
+#endif

teensycontrol/processing.cpp

@@ -1,5 +1,6 @@
 #include "processing.h"
 
+
 //   Receive with start- and end-markers combined with parsing
 const byte numChars = 200;
 char receivedChars[numChars];
@@ -9,6 +10,15 @@ char tempChars[numChars];  // temporary array for use when parsing
 char messageFromPC[numChars] = { 0 };  //or 5 doesn't change anything
 
 #define NBDATA 10
+
+int timeElasped;
+
+//int missedStepInterval;
+
+int  timeElapsedBeforeAdaptPositionFromProcessing;
+
+
+
 int integerFromPC[NBDATA] = { 0 };
 int PC[NBDATA] = { 0 };
 int PCTer[NBDATA] = { 0 };
@@ -21,7 +31,7 @@ int startStop = 0;
 float floatFromPC = 0.0;  // not used for the moment
 
 boolean newData = false;
-//==================================================================== CHECK NUMBEROF ROUND AND GOOD MOVEMENT
+//==================================================================== CHECK NUMBER OF ROUND AND GOOD MOVEMENT
 
 #define TX_SIZE 512
 uint8_t tx_buffer[TX_SIZE];
@@ -44,6 +54,9 @@ int16_t computeDeltaPosition(uint8_t n) {
   oldPositionAbsolue[n] = positionAbsolue;
   return resultat;
 }
+void doReboot() {  // reset Teensy
+  SCB_AIRCR = 0x05FA0004;
+}
 
 void initProcessing() {
   for (uint8_t i = 0; i < NBDATA; i++) {
@@ -52,16 +65,87 @@ void initProcessing() {
 
   int tourTest = 6400 * 0;
 
+  timeElapsedBeforeAdaptPositionFromProcessing=8000;
+
   for (uint8_t i = 0; i < NBMOTORS; i++) {
     PC[i] = tourTest * 1;  // premier devant moi
   }
 
   PCTer[2] = -3;  //noJoe method of Processng data conversion
+
+  PCTer[4] = 1;  // driver actived (LOW
+  PCTer[1] = 0;  // driver disable (HIGH
 }
 
 
 void processingControl() {
+
+  if (PCTer[1] > 0)
+  {
+    digitalWrite(EN[0], LOW);  
+  }
+
+  if (PCTer[1] < 1)
+  {
+    digitalWrite(EN[0], HIGH);  
+  }
+
+  if (PCTer[4] > 0)
+  {
+    digitalWrite(EN[NBMOTORS-1], LOW); //
+    digitalWrite(EN[NBMOTORS-2], LOW);
+    digitalWrite(EN[NBMOTORS-3], LOW);
+    digitalWrite(EN[NBMOTORS-4], LOW); 
+    digitalWrite(EN[NBMOTORS-5], LOW);  
+  } 
 
+  if (PCTer[4] < 1)
+  {
+    digitalWrite(EN[NBMOTORS-1], HIGH);  // 
+    digitalWrite(EN[NBMOTORS-2], HIGH);
+    digitalWrite(EN[NBMOTORS-3], HIGH);
+    digitalWrite(EN[NBMOTORS-4], HIGH); 
+    digitalWrite(EN[NBMOTORS-5], HIGH);  
+  }
+
+   timeElapsedBeforeAdaptPositionFromProcessing = PCTer[5]; 
+
+ //  timeElapsedBeforeAdaptPosition=  timeElapsedBeforeAdaptPositionFromProcessing;
+
+
+/*
+  if ( PCTer[5] != missedStepInterval ) {
+       moveMissedStep.interval(PCTer[5]);
+       missedStepInterval = PCTer[5]; 
+      }  
+ */     
+
+  //------------  Bellow method to avoiding motor go back to zero when i restart Processing. It doesn't work
+
+    // load 0 to all motor. Erase position datas
+  if (PCTer[3] >= 2) {
+     for (uint8_t i = 0; i < NBDATA; i++) {
+      PC[i]=0;
+      processingPosition[i] = PC[i];
+      positionX[i]=processingPosition[i];
+
+    }
+    doReboot();
+  }
+
+/*
+  if (PCTer[3] >= 2) {
+     for (uint8_t i = 0; i < NBDATA; i++) {
+      PC[i]=0;
+      processingPosition[i] = PC[i];
+
+
+    }
+    doReboot();
+  }
+
+   //  Maybe it is better to find something else in Processing
+ */ 
   recvWithStartEndMarkers();  // recevoir chaine de character et disperse en unité
   if (newData == true) {
     strcpy(tempChars, receivedChars);
@@ -88,6 +172,10 @@ void processingControl() {
     }
     writeTargets();
   }
+
+
+
+
 }
 
 void recvWithStartEndMarkers() {

teensycontrol/teensycontrol.ino

@@ -1,15 +1,20 @@
 #include "io.h"
 #include "com.h"
 #include "processing.h"
-#include <Metro.h>  // Include the Metro library
+#include <Metro.h> // Include the Metro library
 
-Metro displayMissedStep = Metro(100);  // 100ms
-Metro moveMissedStep = Metro(2000);    // 2s
+// int timeElapsedBeforeAdaptPositionFromProcessing;
+int timeElapsedBeforeAdaptPosition = 8000;
+int missedStepInterval;
 
-#define PROCESSING  // Uncomment this line to use proccessing, comment this line if you want to use serial demo.
-#define AUTORETURN  // Uncomment this line to use auto return feature, comment to desactivate the feature.
+Metro displayMissedStep = Metro(30);                          // 30ms
+Metro moveMissedStep = Metro(timeElapsedBeforeAdaptPosition); // 1seconde
 
-void setup() {
+#define PROCESSING // Uncomment this line to use proccessing, comment this line if you want to use serial demo.
+#define AUTORETURN // Uncomment this line to use auto return feature, comment to desactivate the feature.
+
+void setup()
+{
   initIo();
   initCom();
   initSteppers();
@@ -20,29 +25,49 @@ void setup() {
 #endif
 }
 
-void loop() {
-
-  if (displayMissedStep.check()) {
+void loop()
+{
+  if (displayMissedStep.check())
+  {
     toggleLed();
-    if (checkMissedStep(false)) {
+
+//    if (checkMissedStep(false)) { // permet d'avoir les positions tous le temps sur le port serie1
 #ifdef PROCESSING
-      displayAllCountSerial1();
+    displayAllCountSerial1();
+    checkMissedStep(false);
 #else
-      displayAllCountSerial();
+    displayAllCountSerial();
 #endif
-    }
+    //    }
   }
 
-  if (moveMissedStep.check()) {
+  if (moveMissedStep.check())
+  {
 #ifdef AUTORETURN
     checkMissedStep(true);
 #endif
   }
 
 #ifdef PROCESSING
   processingControl();
+
+  /*
+    if ( PCTer[5] != missedStepInterval ) {
+       moveMissedStep.interval(PCTer[5]);
+       missedStepInterval = PCTer[5];
+      }
+  */
+
+  // timeElapsedBeforeAdaptPosition=  timeElapsedBeforeAdaptPositionFromProcessing;
+
+  if (timeElapsedBeforeAdaptPosition != missedStepInterval)
+  {
+    moveMissedStep.interval(timeElapsedBeforeAdaptPosition);
+    missedStepInterval = timeElapsedBeforeAdaptPosition;
+  }
+
 #else
   readSerial();
 #endif
   run();
-}
+}