/ Mbed 2 deprecated 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.

Dependencies:   Servo mbed

Fork of Nucleo_ServoKnob by Jose Rios

main.cpp@1:f66f012c59de, 2016-05-29 (annotated)

Committer:
PasKalou
Date:
Sun May 29 15:41:08 2016 +0000
Revision:
1:f66f012c59de
Parent:
0:3a3bfe92df7c
Child:
2:204230a9177d
je change tout !!!

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jose_23991 0:3a3bfe92df7c 1 #include "mbed.h"
jose_23991 0:3a3bfe92df7c 2 #include "Servo.h"
PasKalou 1:f66f012c59de 3 #include <math.h>
PasKalou 1:f66f012c59de 4
PasKalou 1:f66f012c59de 5 #define PI 3.14159265
PasKalou 1:f66f012c59de 6
PasKalou 1:f66f012c59de 7
PasKalou 1:f66f012c59de 8 //------------------------------------
PasKalou 1:f66f012c59de 9 // Hyperterminal configuration
PasKalou 1:f66f012c59de 10 // 9600 bauds, 8-bit data, no parity
PasKalou 1:f66f012c59de 11 //------------------------------------
PasKalou 1:f66f012c59de 12
PasKalou 1:f66f012c59de 13 Serial pc(SERIAL_TX, SERIAL_RX);
PasKalou 1:f66f012c59de 14
PasKalou 1:f66f012c59de 15 // configuration
PasKalou 1:f66f012c59de 16 Servo myservo0(D3); // Create the servo object
PasKalou 1:f66f012c59de 17 Servo myservo1(D5); // Create the servo object
PasKalou 1:f66f012c59de 18 Servo myservo2(D6); // Create the servo object
PasKalou 1:f66f012c59de 19 Servo myservo3(D9); // Create the servo object
PasKalou 1:f66f012c59de 20 AnalogIn knob0(A0); // Create the analog input object
PasKalou 1:f66f012c59de 21 AnalogIn knob1(A1); // Create the analog input object
PasKalou 1:f66f012c59de 22 AnalogIn knob2(A2); // Create the analog input object
PasKalou 1:f66f012c59de 23 AnalogIn knob3(A3); // Create the analog input object
PasKalou 1:f66f012c59de 24
PasKalou 1:f66f012c59de 25
PasKalou 1:f66f012c59de 26 DigitalIn monBouton(PC_13);
PasKalou 1:f66f012c59de 27 // fonction mapper
PasKalou 1:f66f012c59de 28 //
PasKalou 1:f66f012c59de 29 // quand i parcours l'intervalle [a ; b] alors j parcours l'intervalle [0 ; PI]
PasKalou 1:f66f012c59de 30 //
PasKalou 1:f66f012c59de 31 float mapper(float i, float a, float b, float c, float d)
PasKalou 1:f66f012c59de 32 {
PasKalou 1:f66f012c59de 33 float j;
PasKalou 1:f66f012c59de 34 j = ( ( d * (i - a) - c * (i - b) ) / (b - a) ) ;
PasKalou 1:f66f012c59de 35 return j;
PasKalou 1:f66f012c59de 36 }
PasKalou 1:f66f012c59de 37
PasKalou 1:f66f012c59de 38 // fonction ralentir
PasKalou 1:f66f012c59de 39 //
PasKalou 1:f66f012c59de 40 // retourne un coefficient compris entre 1 et 0
PasKalou 1:f66f012c59de 41 // avec une progression sinusoidale lorsque i parcours
PasKalou 1:f66f012c59de 42 // l'intervalle [a ; b] avec une progression linéaire
PasKalou 1:f66f012c59de 43 //
PasKalou 1:f66f012c59de 44 float accelerer(float i , float a , float b)
PasKalou 1:f66f012c59de 45 {
PasKalou 1:f66f012c59de 46 float j;
PasKalou 1:f66f012c59de 47 float angle;
PasKalou 1:f66f012c59de 48 if (i < a) {
PasKalou 1:f66f012c59de 49 j = 1;
PasKalou 1:f66f012c59de 50 } else if (i > b) {
PasKalou 1:f66f012c59de 51 j = 0;
PasKalou 1:f66f012c59de 52 } else {
PasKalou 1:f66f012c59de 53 angle = mapper (i , a , b , (-1 * PI) , 0 );
PasKalou 1:f66f012c59de 54 j = ((1 + cos(angle))/2);
PasKalou 1:f66f012c59de 55 }
PasKalou 1:f66f012c59de 56 return j;
PasKalou 1:f66f012c59de 57 }
PasKalou 1:f66f012c59de 58 float ralentir(float i , float a , float b)
PasKalou 1:f66f012c59de 59 {
PasKalou 1:f66f012c59de 60 float j;
PasKalou 1:f66f012c59de 61 float angle;
PasKalou 1:f66f012c59de 62 if (i < a) {
PasKalou 1:f66f012c59de 63 j = 1;
PasKalou 1:f66f012c59de 64 } else if (i > b) {
PasKalou 1:f66f012c59de 65 j = 0;
PasKalou 1:f66f012c59de 66 } else {
PasKalou 1:f66f012c59de 67 angle = mapper (i , a , b , 0 , PI );
PasKalou 1:f66f012c59de 68 j = ((1 + cos(angle))/2);
PasKalou 1:f66f012c59de 69 }
PasKalou 1:f66f012c59de 70 return j;
PasKalou 1:f66f012c59de 71 }
PasKalou 1:f66f012c59de 72
PasKalou 1:f66f012c59de 73 // fonction test
PasKalou 1:f66f012c59de 74 //
PasKalou 1:f66f012c59de 75 //
PasKalou 1:f66f012c59de 76 void test()
PasKalou 1:f66f012c59de 77 {
PasKalou 1:f66f012c59de 78 float j;
PasKalou 1:f66f012c59de 79 float a = 1 ;
PasKalou 1:f66f012c59de 80 float b = 0.5 ;
PasKalou 1:f66f012c59de 81 float nb = 100;
PasKalou 1:f66f012c59de 82 float coef ;
PasKalou 1:f66f012c59de 83 for (float i = 0 ; i< abs( (b-a) * nb ) ; i++) {
PasKalou 1:f66f012c59de 84 j = (i/nb);
PasKalou 1:f66f012c59de 85 coef = ralentir ( j , 0 , abs(b-a) );
PasKalou 1:f66f012c59de 86 myservo1.write (b - coef * (b-a));
PasKalou 1:f66f012c59de 87 wait_ms(10);
PasKalou 1:f66f012c59de 88 }
PasKalou 1:f66f012c59de 89
PasKalou 1:f66f012c59de 90 }
PasKalou 1:f66f012c59de 91
PasKalou 1:f66f012c59de 92 void aller(float fin0, float fin1)
PasKalou 1:f66f012c59de 93 {
PasKalou 1:f66f012c59de 94 float debut0 = myservo0.read();
PasKalou 1:f66f012c59de 95 // float fin0=0.5;
PasKalou 1:f66f012c59de 96 float debut1 = myservo1.read();
PasKalou 1:f66f012c59de 97 // float fin1=1;
PasKalou 1:f66f012c59de 98 float debut2 = debut0;
PasKalou 1:f66f012c59de 99 float fin2 = fin0;
PasKalou 1:f66f012c59de 100 int i;
PasKalou 1:f66f012c59de 101
PasKalou 1:f66f012c59de 102 for(i=0; i<100; i++) {
PasKalou 1:f66f012c59de 103 // remplacement du coefficient i/100 par un sinus
PasKalou 1:f66f012c59de 104 // et donc mouvement plus fluide
PasKalou 1:f66f012c59de 105 pc.printf("toto");
PasKalou 1:f66f012c59de 106 myservo1.write (debut1 + sin(i*PI/2.0/100.0) * (fin1-debut1));
PasKalou 1:f66f012c59de 107 wait_ms(2);
PasKalou 1:f66f012c59de 108 myservo0.write (debut0 + sin(i*PI/2.0/100.0) * (fin0-debut0));
PasKalou 1:f66f012c59de 109 wait_ms(2);
PasKalou 1:f66f012c59de 110 myservo2.write (debut2 + sin(i*PI/2.0/100.0) * (fin2-debut2));
PasKalou 1:f66f012c59de 111 wait_ms(2);
PasKalou 1:f66f012c59de 112 wait_ms(10);
PasKalou 1:f66f012c59de 113
PasKalou 1:f66f012c59de 114 }
PasKalou 1:f66f012c59de 115
PasKalou 1:f66f012c59de 116 myservo0.write(fin0);
PasKalou 1:f66f012c59de 117 wait_ms(15);
PasKalou 1:f66f012c59de 118 myservo1.write(fin1);
PasKalou 1:f66f012c59de 119 wait_ms(15);
PasKalou 1:f66f012c59de 120 myservo2.write(fin2);
PasKalou 1:f66f012c59de 121 wait_ms(15);
PasKalou 1:f66f012c59de 122 }
jose_23991 0:3a3bfe92df7c 123
jose_23991 0:3a3bfe92df7c 124 int main()
jose_23991 0:3a3bfe92df7c 125 {
PasKalou 1:f66f012c59de 126
PasKalou 1:f66f012c59de 127 //calibration
PasKalou 1:f66f012c59de 128 myservo0.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
PasKalou 1:f66f012c59de 129 myservo1.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
PasKalou 1:f66f012c59de 130 myservo2.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
PasKalou 1:f66f012c59de 131 myservo3.calibrate(0.00105, 150.0); // Calibrate the servo HDKJ D3015
PasKalou 1:f66f012c59de 132 //
PasKalou 1:f66f012c59de 133 // pour le servo tout merdique arduino :
PasKalou 1:f66f012c59de 134 // myservo1.calibrate(0.00075, 70.0); // Calibrate the servo GROOVE tout petit tout mini
PasKalou 1:f66f012c59de 135 //
PasKalou 1:f66f012c59de 136
PasKalou 1:f66f012c59de 137
PasKalou 1:f66f012c59de 138 // réinitialisation
PasKalou 1:f66f012c59de 139
PasKalou 1:f66f012c59de 140
PasKalou 1:f66f012c59de 141 aller(0.15,0.6);
PasKalou 1:f66f012c59de 142 wait_ms(3000);
PasKalou 1:f66f012c59de 143 aller(0.2,0.2);
PasKalou 1:f66f012c59de 144 aller(0.90,0.15);
PasKalou 1:f66f012c59de 145 aller(0.5,1);
PasKalou 1:f66f012c59de 146 wait_ms(3000);
PasKalou 1:f66f012c59de 147
PasKalou 1:f66f012c59de 148
PasKalou 1:f66f012c59de 149
PasKalou 1:f66f012c59de 150 //boucle
PasKalou 1:f66f012c59de 151 while(1) {
PasKalou 1:f66f012c59de 152
PasKalou 1:f66f012c59de 153 if (monBouton==0) {
PasKalou 1:f66f012c59de 154 pc.printf("\ntoto\n");
PasKalou 1:f66f012c59de 155 test();
PasKalou 1:f66f012c59de 156 }
PasKalou 1:f66f012c59de 157
PasKalou 1:f66f012c59de 158 /*
PasKalou 1:f66f012c59de 159 aller(0.5,1);
PasKalou 1:f66f012c59de 160 aller(0.4,0.9);
PasKalou 1:f66f012c59de 161 aller(0.6,0.8);
PasKalou 1:f66f012c59de 162 aller(0.5,0.5);
PasKalou 1:f66f012c59de 163 wait_ms(3000);
PasKalou 1:f66f012c59de 164 */
PasKalou 1:f66f012c59de 165
PasKalou 1:f66f012c59de 166 /*
PasKalou 1:f66f012c59de 167 for(int i=0; i<100; i++) {
PasKalou 1:f66f012c59de 168 myservo0.write(i/100.0);
PasKalou 1:f66f012c59de 169 wait_ms(10);
PasKalou 1:f66f012c59de 170 myservo1 = i/100.0;
PasKalou 1:f66f012c59de 171 wait_ms(10);
PasKalou 1:f66f012c59de 172 }
PasKalou 1:f66f012c59de 173 for(int i=100; i>0; i--) {
PasKalou 1:f66f012c59de 174 myservo0 = i/100.0;
PasKalou 1:f66f012c59de 175 wait_ms(10);
PasKalou 1:f66f012c59de 176 myservo1 = i/100.0;
PasKalou 1:f66f012c59de 177 wait_ms(10);
PasKalou 1:f66f012c59de 178 }
PasKalou 1:f66f012c59de 179
PasKalou 1:f66f012c59de 180 val = knob0.read(); // Reads the value of the potentiometer (value between 0 and 1)
PasKalou 1:f66f012c59de 181 pc.printf("val = %f\n",val);
PasKalou 1:f66f012c59de 182 myservo0.write(val); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 183 wait_ms(15); // Waits for the servo to get there
PasKalou 1:f66f012c59de 184
PasKalou 1:f66f012c59de 185 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)
PasKalou 1:f66f012c59de 186 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 187 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)
PasKalou 1:f66f012c59de 188 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 189 myservo2.position(0); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 190 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 191
PasKalou 1:f66f012c59de 192 //
PasKalou 1:f66f012c59de 193 wait_ms(3000); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 194 //
PasKalou 1:f66f012c59de 195 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)
PasKalou 1:f66f012c59de 196 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 197 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)
PasKalou 1:f66f012c59de 198 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 199 myservo2.position(-80); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 200 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 201 // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 202 //
PasKalou 1:f66f012c59de 203 wait_ms(3000); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 204 //
PasKalou 1:f66f012c59de 205 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)
PasKalou 1:f66f012c59de 206 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 207 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)
PasKalou 1:f66f012c59de 208 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 209 myservo2.position(0); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 210 wait_ms(3000); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 211
PasKalou 1:f66f012c59de 212
PasKalou 1:f66f012c59de 213
PasKalou 1:f66f012c59de 214
PasKalou 1:f66f012c59de 215 // myservo0.position(-45.0); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 216 // wait_ms(3500); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 217 val = knob0.read(); // Reads the value of the potentiometer (value between 0 and 1)
PasKalou 1:f66f012c59de 218 myservo0.write(1); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 219 myservo0.write(1); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 220 wait_ms(15); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 221 myservo1.write(1); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 222 wait_ms(3000);
PasKalou 1:f66f012c59de 223 // Waits for the servo to get there
PasKalou 1:f66f012c59de 224 val = knob2.read(); // Reads the value of the potentiometer (value between 0 and 1)
PasKalou 1:f66f012c59de 225 myservo2.write(val); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 226 wait_ms(15); // Waits for the servo to get there
PasKalou 1:f66f012c59de 227 val = knob3.read(); // Reads the value of the potentiometer (value between 0 and 1)
PasKalou 1:f66f012c59de 228 myservo3.write(val); // Sets the servo position according to the scaled value (0-1)
PasKalou 1:f66f012c59de 229 wait_ms(15); // Waits for the servo to get there
PasKalou 1:f66f012c59de 230 */
jose_23991 0:3a3bfe92df7c 231 }
PasKalou 1:f66f012c59de 232
PasKalou 1:f66f012c59de 233
PasKalou 1:f66f012c59de 234
jose_23991 0:3a3bfe92df7c 235 }