/
Nucleo_ServoKnob
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Fork of
Nucleo_ServoKnob
by 
Jose Rios
Diff: main.cpp
- Revision:
- 2:204230a9177d
- Parent:
- 1:f66f012c59de
- Child:
- 3:0de66e6f1a80
--- a/main.cpp Sun May 29 15:41:08 2016 +0000
+++ b/main.cpp Sun May 29 19:07:50 2016 +0000
@@ -24,70 +24,6 @@
DigitalIn monBouton(PC_13);
-// fonction mapper
-//
-// quand i parcours l'intervalle [a ; b] alors j parcours l'intervalle [0 ; PI]
-//
-float mapper(float i, float a, float b, float c, float d)
-{
- float j;
- j = ( ( d * (i - a) - c * (i - b) ) / (b - a) ) ;
- return j;
-}
-
-// fonction ralentir
-//
-// retourne un coefficient compris entre 1 et 0
-// avec une progression sinusoidale lorsque i parcours
-// l'intervalle [a ; b] avec une progression linéaire
-//
-float accelerer(float i , float a , float b)
-{
- float j;
- float angle;
- if (i < a) {
- j = 1;
- } else if (i > b) {
- j = 0;
- } else {
- angle = mapper (i , a , b , (-1 * PI) , 0 );
- j = ((1 + cos(angle))/2);
- }
- return j;
-}
-float ralentir(float i , float a , float b)
-{
- float j;
- float angle;
- if (i < a) {
- j = 1;
- } else if (i > b) {
- j = 0;
- } else {
- angle = mapper (i , a , b , 0 , PI );
- j = ((1 + cos(angle))/2);
- }
- return j;
-}
-
-// fonction test
-//
-//
-void test()
-{
- float j;
- float a = 1 ;
- float b = 0.5 ;
- float nb = 100;
- float coef ;
- for (float i = 0 ; i< abs( (b-a) * nb ) ; i++) {
- j = (i/nb);
- coef = ralentir ( j , 0 , abs(b-a) );
- myservo1.write (b - coef * (b-a));
- wait_ms(10);
- }
-
-}
void aller(float fin0, float fin1)
{
@@ -102,7 +38,6 @@
for(i=0; i<100; i++) {
// remplacement du coefficient i/100 par un sinus
// et donc mouvement plus fluide
- pc.printf("toto");
myservo1.write (debut1 + sin(i*PI/2.0/100.0) * (fin1-debut1));
wait_ms(2);
myservo0.write (debut0 + sin(i*PI/2.0/100.0) * (fin0-debut0));
@@ -121,6 +56,100 @@
wait_ms(15);
}
+// fonction MAP
+// pour faire de la translation d'intervalle
+// i dans [a ; b] devient j dans [c ; d]
+//
+float map (float i, float a, float b, float c, float d)
+{
+ float j;
+ j = (d * (i-a) - c * (i-b) ) / (b-a);
+ return j;
+}
+
+// fonctino BEZIER
+// pour pour passer de 3 intervalles continus
+// à un seul lissé
+// i paramètre dans [ borne_inf ; borne_sup]
+// intervalle 1 : [debut ; intermed0]
+// intervalle 2 : [intermed0 ; intermed1]
+// intervalle 3 ; [intermed1 ; fin]
+float bezier(int i , int borne_inf , int borne_sup , float debut , float intermed0 , float intermed1 , float fin)
+{
+ float pt[6];
+ pt[0] = debut;
+ pt[1] = debut + (intermed0 - debut)*0.05;
+ pt[2] = intermed0;
+ pt[3] = intermed1;
+ pt[4] = fin - (fin-intermed1)*0.05;
+ pt[5] = fin;
+ //
+ for (int j = 5 ; j>0 ; j--) {
+ for (int k = 0 ; k<j ; k++) {
+ pt[k] = map (i , borne_inf, borne_sup , pt[k] , pt[k+1]) ;
+ }
+ }
+ return pt[0];
+}
+
+// fonction ALLERPAR
+// pour le servo0 : intermed00 , intermed01 et fin 0
+// pour le servo1 : intermed10 , intermed11 et fin 1
+// un paramètre de vitesse au plus c'est grand au plus c'est lent
+//
+void allerPar(float intermed00,float intermed01, float fin0, float intermed10,float intermed11,float fin1, float vitesse)
+{
+ float debut0 = myservo0.read();
+ float debut1 = myservo1.read();
+ float fin2 = fin0;
+ int i;
+ float valeur ;
+
+ for(i=0; i<vitesse; i++) {
+ //
+ valeur = bezier ( i , 0 , vitesse , debut0 , intermed00 , intermed01 , fin0);
+ //
+ myservo0.write ( valeur );
+ wait_ms(2);
+ //
+ myservo2.write ( valeur );
+ wait_ms(2);
+ //
+ valeur = bezier ( i , 0 , vitesse , debut1 , intermed10 , intermed11 , fin1);
+ //
+ myservo1.write ( valeur );
+ wait_ms(2);
+ //
+ wait_ms(10);
+
+ }
+
+ myservo0.write(fin0);
+ wait_ms(15);
+ myservo1.write(fin1);
+ wait_ms(15);
+ myservo2.write(fin2);
+ wait_ms(15);
+}
+
+
+void test()
+{
+ // point de départ
+ aller(0.15,0.6);
+ wait_ms(3000);
+ //
+ // petite promenade pour monter en 0,5 - 1 - 0,5
+ //servo 0 et servo 2 : 0.3 -> 1.2 -> 0.5
+ //servo 1 : 0 -> -0.3 -> 1
+ //vitesse : 300
+ //
+ allerPar(0.3 ,1.2, 0.5 , 0, -0.3 , 1, 300);
+ wait_ms(25);
+}
+
+
+
int main()
{
@@ -129,107 +158,20 @@
myservo1.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
myservo2.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
myservo3.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
+ wait_ms(2000);
//
// pour le servo tout merdique arduino :
// myservo1.calibrate(0.00075, 70.0); // Calibrate the servo GROOVE tout petit tout mini
//
-// réinitialisation
-
-
- aller(0.15,0.6);
- wait_ms(3000);
- aller(0.2,0.2);
- aller(0.90,0.15);
- aller(0.5,1);
- wait_ms(3000);
-
-
-
-//boucle
while(1) {
+ // teste l'appuie sur le bouton
if (monBouton==0) {
- pc.printf("\ntoto\n");
test();
}
- /*
- aller(0.5,1);
- aller(0.4,0.9);
- aller(0.6,0.8);
- aller(0.5,0.5);
- wait_ms(3000);
- */
-
- /*
- for(int i=0; i<100; i++) {
- myservo0.write(i/100.0);
- wait_ms(10);
- myservo1 = i/100.0;
- wait_ms(10);
- }
- for(int i=100; i>0; i--) {
- myservo0 = i/100.0;
- wait_ms(10);
- myservo1 = i/100.0;
- wait_ms(10);
- }
-
- val = knob0.read(); // Reads the value of the potentiometer (value between 0 and 1)
- pc.printf("val = %f\n",val);
- myservo0.write(val); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Waits for the servo to get there
-
- myservo0.position(0.0); // Sets the servo position according to the scaled value (0-1) wait_ms(1500); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
- myservo1.position(-90.0); // Sets the servo position according to the scaled value (0-1) wait_ms(1500); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
- myservo2.position(0); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
-
- //
- wait_ms(3000); // Sets the servo position according to the scaled value (0-1)
- //
- myservo0.position(-90.0); // Sets the servo position according to the scaled value (0-1) wait_ms(1500); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
- myservo1.position(-60.0); // Sets the servo position according to the scaled value (0-1) wait_ms(1500); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
- myservo2.position(-80); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
- // Sets the servo position according to the scaled value (0-1)
- //
- wait_ms(3000); // Sets the servo position according to the scaled value (0-1)
- //
- myservo0.position(10.0); // Sets the servo position according to the scaled value (0-1) wait_ms(1500); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
- myservo1.position(90.0); // Sets the servo position according to the scaled value (0-1) wait_ms(1500); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
- myservo2.position(0); // Sets the servo position according to the scaled value (0-1)
- wait_ms(3000); // Sets the servo position according to the scaled value (0-1)
-
-
-
-
- // myservo0.position(-45.0); // Sets the servo position according to the scaled value (0-1)
- // wait_ms(3500); // Sets the servo position according to the scaled value (0-1)
- val = knob0.read(); // Reads the value of the potentiometer (value between 0 and 1)
- myservo0.write(1); // Sets the servo position according to the scaled value (0-1)
- myservo0.write(1); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Sets the servo position according to the scaled value (0-1)
- myservo1.write(1); // Sets the servo position according to the scaled value (0-1)
- wait_ms(3000);
- // Waits for the servo to get there
- val = knob2.read(); // Reads the value of the potentiometer (value between 0 and 1)
- myservo2.write(val); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Waits for the servo to get there
- val = knob3.read(); // Reads the value of the potentiometer (value between 0 and 1)
- myservo3.write(val); // Sets the servo position according to the scaled value (0-1)
- wait_ms(15); // Waits for the servo to get there
- */
}
-
-
}