//Firmware per Esocheletro con scheda Nucleo F401RE progetto Mimic //Autore: DAM Bros robotics //Licenza: Creative Commons BY-NC-SA #include "mbed.h" #define sample 100 // Numero di campioni per le medie //Definizione degli angoli massimi generati dalla tuta #define angolo_max_palmoDx 180 #define angolo_max_palmoSx 180 #define angolo_max_polsoDx 140 #define angolo_max_polsoSx 140 #define angolo_max_gomitoDx 160 #define angolo_max_gomitoSx 160 #define angolo_max_spallaDx 110 #define angolo_max_spallaSx 110 #define angolo_max_schienaDx 100 #define angolo_max_schienaSx 100 //Definizione dei pin di ingresso analogici AnalogIn pot1(A0); //PA_0 = ADC1_IN0 = A0 - Potenziometro palmoDx AnalogIn pot2(A1); //PA_1 = ADC1_IN1 = A1 - Potenziometro palmoSx AnalogIn pot3(A2); //PA_2 = ADC1_IN2 = A2 - Potenziometro polsoDx AnalogIn pot4(A3); //PA_3 = ADC1_IN3 = A3 - Potenziometro polsoSx AnalogIn pot5(A4); //PA_4 = ADC1_IN4 = A4 - Potenziometro gomitoDx AnalogIn pot6(A5); //PA_5 = ADC1_IN5 = A5 - Potenziometro gomitoSx AnalogIn pot7(PA_6); //PA_6 = ADC1_IN6 = A6 - Potenziometro spallaDx AnalogIn pot8(PA_7); //PA_7 = ADC1_IN7 = A7 - Potenziometro spallaSx AnalogIn pot9(PC_3); //PB_0 = ADC1_IN8 = A8 - Potenziometro schienaDx AnalogIn pot10(PC_4);//PB_1 = ADC1_IN9 = A9 - Potenziometro schienaSx Serial blutooth(D8, D2);//Tx, Rx Serial pc(USBTX,USBRX);//Tx, Rx DigitalOut led(LED1); //Prototipo funzione di map float map(float x,float in_min,float in_max,float out_min,float out_max); float nullo=0; int main() { //Variabili per valorimi minimi dei potenziometri float min_pot1=0,min_pot2=0,min_pot3=0,min_pot4=0,min_pot5=0,min_pot6=0,min_pot7=0,min_pot8=0,min_pot9=0,min_pot10=0; // //Variabili per valorimi massimi dei potenziometri float max_pot1=0,max_pot2=0,max_pot3=0,max_pot4=0,max_pot5=0,max_pot6=0,max_pot7=0,max_pot8=0,max_pot9=0,max_pot10=0; // //Variabili per valorimi letti dai potenziometri float sens1,sens2,sens3,sens4,sens5,sens6, sens7,sens8,sens9,sens10; // //Variabili per angoli mappati dalle letture float angolo_palmoDx, angolo_palmoSx, angolo_polsoDx, angolo_polsoSx, angolo_gomitoDx, angolo_gomitoSx, angolo_spallaDx, angolo_spallaSx, angolo_schienaDx, angolo_schienaSx; // blutooth.baud(115200); //per HC-06_Master //----------------------------------------------------------------------------------------- //CALIBRAZIONE TUTA - CALCOLO VALORI MINIMI E MASSIMI DEI POTENZIOMETRI //----------------------------------------------------------------------------------------- pc.printf("Inizio calcolo valori minimi\r\n"); // 5 Lampeggi del LED a bordo della Nucleo per segnalare l'avvio della fase 1 di calibrazione for(int i=0; i<5; i++) { led = 1; // LED is ON wait_ms(500); led = 0; // LED is OFF wait_ms(500); } wait(5); //Tempo di attesa prima del ciclo di letture dei minimi //----------------------------------------------------------------------------------------- //Lettura e accumulo dei valori minimi potenziometri - Tuta a riposo for(int n=0; n< sample; n++) { min_pot1 += pot1.read(); //Accumulo valori palmoDx min_pot2 += pot2.read(); //Accumulo valori palmoSx min_pot3 += pot3.read(); //Accumulo valori polsoDx min_pot4 += pot4.read(); //Accumulo valori polsoSx min_pot5 += pot5.read(); //Accumulo valori gomitoDx min_pot6 += pot6.read(); //Accumulo valori gomitoSx min_pot7 += pot7.read(); //Accumulo valori spallaDx min_pot8 += pot8.read(); //Accumulo valori spallaSx min_pot9 += pot9.read(); //Accumulo valori schienaDx min_pot10 += pot10.read(); //Accumulo valori schienaSx wait_ms(10); } //Media valori minimi min_pot1 /= sample; //Media valori minimi valori palmoDx min_pot2 /= sample; //Media valori minimi valori palmoSx min_pot3 /= sample; //Media valori minimi valori polsoDx min_pot4 /= sample; //Media valori minimi valori polsoSx min_pot5 /= sample; //Media valori minimi valori gomitoDx min_pot6 /= sample; //Media valori minimi valori gomitoSx min_pot7 /= sample; //Media valori minimi valori spallaDx min_pot8 /= sample; //Media valori minimi valori spallaSx min_pot9 /= sample; //Media valori minimi valori schienaDx min_pot10 /= sample; //Media valori minimi valori schienaSx wait_ms(10); //----------------------------------------------------------------------------------------- // 5 Lampeggi del LED a bordo della Nucleo per segnalare l'avvio della fase di calibrazione for(int i=0; i<5; i++) { led = 1; // LED is ON wait_ms(500); led = 0; // LED is OFF wait_ms(500); } wait(5); //Tempo di attesa prima del ciclo di letture dei massimi //----------------------------------------------------------------------------------------- //Lettura e accumulo dei valori massimi potenziometri - Tuta a riposo for(int n=0; n< sample; n++) { max_pot1 += pot1.read(); //Accumulo valori palmoDx max_pot2 += pot2.read(); //Accumulo valori palmoSx max_pot3 += pot3.read(); //Accumulo valori polsoDx max_pot4 += pot4.read(); //Accumulo valori polsoSx max_pot5 += pot5.read(); //Accumulo valori gomitoDx max_pot6 += pot6.read(); //Accumulo valori gomitoSx max_pot7 += pot7.read(); //Accumulo valori spallaDx max_pot8 += pot8.read(); //Accumulo valori spallaSx max_pot9 += pot9.read(); //Accumulo valori schienaDx max_pot10 += pot10.read(); //Accumulo valori schienaSx wait_ms(10); } //Media valori minimi max_pot1 /= sample; //Media valori massimi valori palmoDx max_pot2 /= sample; //Media valori massimi valori palmoSx max_pot3 /= sample; //Media valori massimi valori polsoDx max_pot4 /= sample; //Media valori massimi valori polsoSx max_pot5 /= sample; //Media valori massimi valori gomitoDx max_pot6 /= sample; //Media valori massimi valori gomitoSx max_pot7 /= sample; //Media valori massimi valori spallaDx max_pot8 /= sample; //Media valori massimi valori spallaSx max_pot9 /= sample; //Media valori massimi valori schienaDx max_pot10 /= sample; //Media valori massimi valori schienaSx wait_ms(10); //----------------------------------------------------------------------------------------- // 5 Lampeggi del LED a bordo della Nucleo per segnalare l'avvio della fase di calibrazione for(int i=0; i<10; i++) { led = 1; // LED is ON wait_ms(500); led = 0; // LED is OFF wait_ms(150); } wait(5); //Tempo di attesa prima del ciclo di letture dei massimi //----------------------------------------------------------------------------------------- //LETTURA VALORI POTENZIOMETRI E CONVERSIONE IN ANGOLI (Utilizzo della funzione map) //----------------------------------------------------------------------------------------- while(1) { //Lettura potenziometro 1 - palmoDx sens1 = pot1.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_palmoDx = map(sens1,min_pot1,max_pot1,0,angolo_max_palmoDx); //Lettura potenziometro 2 - palmoSx sens2 = pot2.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_palmoSx= map(sens2,min_pot2,max_pot2,0,angolo_max_palmoSx); //Lettura potenziometro 3 - polsoDx sens3 = pot3.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_polsoDx = map(sens3,min_pot3,max_pot3,0,angolo_max_polsoDx); //Lettura potenziometro 4 - polsoSx sens4 = pot4.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_polsoSx = map(sens4,min_pot4,max_pot4,0,angolo_max_polsoSx); //Lettura potenziometro 5 - gomitoDx sens5 = pot5.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_gomitoDx = map(sens5,min_pot5,max_pot5,0,angolo_max_gomitoDx); //Lettura potenziometro 6 - gomitoSx sens6 = pot6.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_gomitoSx = map(sens6,min_pot6,max_pot6,0,angolo_max_gomitoSx); //Lettura potenziometro 7 - spallaDx sens7 = pot7.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_spallaDx = map(sens7,min_pot7,max_pot7,0,angolo_max_spallaDx); //Lettura potenziometro 8 - spallaSx sens8 = pot8.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_spallaSx = map(sens8,min_pot8,max_pot8,0,angolo_max_spallaSx); //Lettura potenziometro 9 - schienaDx sens9 = pot9.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_schienaDx = map(sens9,min_pot9,max_pot9,0,angolo_max_schienaDx); //Lettura potenziometro 10 - schienaSx sens10 = pot10.read(); // Converts and read the analog input value (value from 0.0 to 1.0) angolo_schienaSx = map(sens10,min_pot10,max_pot10,0,angolo_max_schienaSx); //Stampa valori dei potenziometri convertiti in angoli pc.printf("#%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f|\r\n",angolo_palmoDx,angolo_palmoSx,angolo_polsoDx,angolo_polsoSx,angolo_gomitoDx,angolo_gomitoSx,angolo_spallaDx,angolo_spallaSx,angolo_schienaDx,angolo_schienaSx); bluetooth.printf("#%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f;%3.0f|\r\n",angolo_palmoDx,angolo_palmoSx,angolo_polsoDx,angolo_polsoSx,angolo_gomitoDx,angolo_gomitoSx,angolo_spallaDx,angolo_spallaSx,angolo_schienaDx,angolo_schienaSx); wait_ms(10); // 10 ms } } float map(float x,float in_min,float in_max,float out_min,float out_max) { float angolo_temp; //pc.printf("x - in_min = %d\n", x - in_min); //pc.printf("out_max - out_min = %d\n", out_max - out_min); //pc.printf("in_max - in_min = %d\n", in_max - in_min); angolo_temp =((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min) ; if (angolo_temp < out_min){ return out_min; } else if (angolo_temp > out_max){ return out_max; } else { return angolo_temp;} //return ((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min) ; }