From 5933077fb22ae2df42dec6d5f9f6b6631874bb6d Mon Sep 17 00:00:00 2001
From: Arnaud Cadot <arnaud.cadot@telecom-bretagne.eu>
Date: Mon, 21 Mar 2016 18:14:14 +0100
Subject: [PATCH] Updated values for KJ servos

---
 stm32/include/KJ2016Tempo.h            |  4 ++--
 stm32/include/actionneurs/fishingNet.h | 14 +++++++-------
 stm32/src/KJ2016Tempo.cpp              |  9 ++++++---
 3 files changed, 15 insertions(+), 12 deletions(-)

diff --git a/stm32/include/KJ2016Tempo.h b/stm32/include/KJ2016Tempo.h
index e99be6a3..9e19a41f 100644
--- a/stm32/include/KJ2016Tempo.h
+++ b/stm32/include/KJ2016Tempo.h
@@ -8,8 +8,8 @@
  */
 class KJ2016Tempo
 {
-    static const float KJ_INTERAXIS        = 100.f; // To update
-    static const float KJ_WHEEL_DIAMETER   = 60.f;
+    static const float KJ_INTERAXIS        = 132.f; // To update
+    static const float KJ_WHEEL_DIAMETER   = 70.f;
     static const float SERVO_MAX_RPM       = 59.f;
 
     public:
diff --git a/stm32/include/actionneurs/fishingNet.h b/stm32/include/actionneurs/fishingNet.h
index fdf10041..dcda6bb1 100644
--- a/stm32/include/actionneurs/fishingNet.h
+++ b/stm32/include/actionneurs/fishingNet.h
@@ -10,21 +10,21 @@ class FishingNet
     /**
       * The ID of the innermost AX12 (i.e. for rotation)
       */
-    static const int SERVO_INT_ID = 0; // To update
+    static const int SERVO_INT_ID = 42; // To update
     /**
       * The ID of the outermost AX12 (i.e. for folding/unfolding)
       */
-    static const int SERVO_EXT_ID = 1;
+    static const int SERVO_EXT_ID = 16;
 
     /**
       * Those constantes are angles sent to the relevant servos for the relevant action (names are pretty explicit)
       */
-    static const int SERVO_EXT_CLOSED_POS   = 0x00;
-    static const int SERVO_EXT_DEPLOYED_POS = 0x00;
-    static const int SERVO_EXT_RAISED_POS   = 0x00;
+    static const int SERVO_EXT_CLOSED_POS   = 0x0339;
+    static const int SERVO_EXT_DEPLOYED_POS = 0x0200;
+    static const int SERVO_EXT_RAISED_POS   = 0x0240;
 
-    static const int SERVO_INT_RAISED_POS   = 0x00;
-    static const int SERVO_INT_LOWERED_POS  = 0x00;
+    static const int SERVO_INT_RAISED_POS   = 0x0200;
+    static const int SERVO_INT_LOWERED_POS  = 0x00C3;
 
     public:
         enum NET_STATE
diff --git a/stm32/src/KJ2016Tempo.cpp b/stm32/src/KJ2016Tempo.cpp
index ad05f487..4439dc1d 100644
--- a/stm32/src/KJ2016Tempo.cpp
+++ b/stm32/src/KJ2016Tempo.cpp
@@ -20,8 +20,8 @@ void KJ2016Tempo::run(bool isYellow)
 {
     /** ID for the driving servos **/
     /* Left and right is relative to the starting position */
-    const unsigned int SERVO_ONE_ID = 0;
-    const unsigned int SERVO_TWO_ID = 1;
+    const unsigned int SERVO_ONE_ID = 69;
+    const unsigned int SERVO_TWO_ID = 15;
 
     KJ2016Tempo KJ(isYellow?SERVO_ONE_ID:SERVO_TWO_ID, isYellow?SERVO_TWO_ID:SERVO_ONE_ID);
 
@@ -97,10 +97,13 @@ void KJ2016Tempo::move(int distance)
 
 void KJ2016Tempo::waitForArrival(unsigned int duration)
 {
-    static const unsigned int sensorCheckDelay = Clock::MS_PER_TICK * 10;
+    unsigned int sensorCheckDelay = Clock::MS_PER_TICK * 10;
 
     while(true)
     {
+        if(sensorCheckDelay > duration)
+            sensorCheckDelay = duration;
+
         unsigned int t = Clock::delay(sensorCheckDelay);
 
         if(t >= duration)
-- 
GitLab