From 3fa09f41be901092763f66435469413532554bd5 Mon Sep 17 00:00:00 2001
From: xctd692 <xctd692@445490SS231.dct.adt.local>
Date: Fri, 4 Mar 2016 11:48:18 +0100
Subject: [PATCH] Ajout d'une fonction pour mesurer la vitesse du moteur

---
 .../ServoStrapPortManipulationLD293D_1_2.ino           | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino
index 66da4d2..c6c53b4 100755
--- a/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino	
+++ b/Moteur DC/ServoStrap/ServoStrapPortManipulationLD293D_1_2/ServoStrapPortManipulationLD293D_1_2.ino	
@@ -57,6 +57,10 @@ int lastError = 0;
 int sumError = 0;
 int StartRoutine=0;
 
+float OldTime = 0;
+float Oldencoder0Pos = 0;
+float MySpeed = 0;
+
 //Integral term min/max (random value and not yet tested/verified)
 int iMax = 100;
 int iMin = 0;
@@ -158,6 +162,12 @@ void Fstart(){
 
 }
 
+void Speed(){
+MySpeed = (Oldencoder0Pos - encoder0Pos) / ( OldTime - millis());
+OldTime = millis();
+Oldencoder0Pos = encoder0Pos;
+}
+
 void docalc() {
   
   if (millis() - previousMillis > interval)