Revision 5:a81d551cfffa, committed 2015-06-03

Comitter:

wumzi

Date:

Wed Jun 03 15:32:35 2015 +0000

Parent:

4:ca5bdb807fa1

Commit message:

Modif pour tenter de setter vitesse

Changed in this revision

--- a/MX-28_DefConstantes.h	Mon Jun 01 16:08:53 2015 +0000
+++ b/MX-28_DefConstantes.h	Wed Jun 03 15:32:35 2015 +0000
@@ -79,4 +79,4 @@
 #define MX28_ERRBIT_MASTER_CHECKSUM 0xFF
 
 #define MX28_WAIT_AFTER_WRITE       1700 // Attention point de réglage délicat !!!
-// *** END "DO NOT MODIFY THESE CONSTANTS" SECTION ***
+// *** END "DO NOT MODIFY THESE CONSTANTS" SECTION ***
\ No newline at end of file

--- a/main.cpp	Mon Jun 01 16:08:53 2015 +0000
+++ b/main.cpp	Wed Jun 03 15:32:35 2015 +0000
@@ -27,8 +27,8 @@
 #include "MX-28_DefConstantes.h"
 #include "NeoStrip.h"
 
-#define ID_SERVO_VERTICAL 0x0A		// MX-106R
-#define ID_SERVO_HORIZONTALE 0x01	// MX-28R
+#define ID_SERVO_VERTICAL 10		// MX-106R
+#define ID_SERVO_HORIZONTALE 1	// MX-28R
 
 #define N1 64 // Nombre de pixels(Led) du panneau Led 1
 #define N2 128 // Nombre de pixels(Led) du panneau Led 2
@@ -68,6 +68,9 @@
 I2C i2cLsm303D(p9, p10);   // Création objet et affectation SDA1, SCL1 pour l'accéléromètre LSM303D
 const int addrLsm303D = 0x3C; // adresse du LSM303D avec SDO/SA0=0
 
+//---------------------------------------------------------------------------------------------
+// Set Motor gain
+
 //------------------------------------------------------------------------------------------------
 // Envoi de la trame de pilotage a un servomoteur MX-28
 void write (char id, char longueurTrame, char instruction, char param1 = NULL, char param2 = NULL, char param3 = NULL, char param4 = NULL)
@@ -100,6 +103,16 @@
     dir = 0;
 }
 
+void setMotorGainSpeed(){
+	char min = 1;
+	char max = 2;
+	write(ID_SERVO_VERTICAL, 5, MX28_WRITE_DATA, MX28_MOVING_SPEED_L, min, max);
+	write(ID_SERVO_HORIZONTALE, 5, MX28_WRITE_DATA, MX28_MOVING_SPEED_L, min, max);
+		
+	serialPc.printf("gains set\n");
+}
+
+
 // Set goal position of engine
 void setPosition(char id, int goal)
 {
@@ -234,7 +247,31 @@
                          
 			//--Solution simple pour retourner la position mais sans tester "positions demandéess=atteintes"
             // test positions atteintes à faire
-            wait(2); //pour laisser le temps au moteur de se déplacer, 
+            wait(1); //pour laisser le temps au moteur de se déplacer, 
+            
+            int incr = 0;
+            
+            int valeurLue = lirePositionServo(ID_SERVO_VERTICAL);
+            int valeurAngleVertCentiemeDegres = (valeurLue * 100) * 360/4096;
+            wait(0.2);
+            valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
+            int valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;
+            
+            /*while ((abs(valeurAngleVertCentiemeDegres - anglePositionVerticale * 100) > 100
+                    || abs(valeurAngleHorizontaleCentiemeDegres - anglePositionHorizontale * 100) > 100)
+                && incr < 60)
+                {
+                    wait(1);
+		            int valeurLue = lirePositionServo(ID_SERVO_VERTICAL);
+		            valeurAngleVertCentiemeDegres = (valeurLue * 100) * 360/4096;
+		            wait(0.2);
+		            valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
+		            valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;
+                    incr++;
+                    
+                    //pc.printf(" Goal = %d ; Current = %d", goalHoriz, currentPositionHoriz);
+                }*/
+                       
             bufferRec[0] = '1'; //pour retourner la mesure de la position
             numeroOctetRecu = 2; //pour retourner la mesure de la position
         }
@@ -245,7 +282,8 @@
             wait(0.2);
             valeurLue = lirePositionServo(ID_SERVO_HORIZONTALE);
             int valeurAngleHorizontaleCentiemeDegres = (valeurLue * 100) * 360/4096;
-
+            
+            
             serialPc.printf("$OK_moteurs_%0#5d_%0#5d/\n",valeurAngleVertCentiemeDegres, valeurAngleHorizontaleCentiemeDegres);
 
             led03 = 0; //
@@ -360,6 +398,9 @@
             dataLu[1]=0x82; // Accelero ON et data rate = 100Hz
             i2cLsm303D.write(addrLsm303D, dataLu, 2);
 
+	// Gain moteurs
+	setMotorGainSpeed();
+
     while(1) {
         led01 = 0;
         wait(0.1);