program 1 driver
Dependencies: mbed motorController2
main.cpp@4:6956b568c810, 2017-02-14 (annotated)
- Committer:
- abckcp
- Date:
- Tue Feb 14 22:50:24 2017 +0000
- Revision:
- 4:6956b568c810
- Parent:
- 3:e3b61eb0590b
- Child:
- 5:f2edd689cc38
Should add arduino - STM 32 ACK protocol
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
BalB | 0:1d2060237094 | 1 | #include "mbed.h" |
BalB | 1:3370c513ff5c | 2 | #include "math.h" |
abckcp | 4:6956b568c810 | 3 | #include "DigitalInOut.h" |
abckcp | 4:6956b568c810 | 4 | |
abckcp | 4:6956b568c810 | 5 | /* Como el movimiento de los motores lo hace el arduino y las medidas la stm32 tienen que coordinarse de alguna manera |
abckcp | 4:6956b568c810 | 6 | Este codigo tiene que hacer las medidas cuando le llegue un 1 por el puerto digital input 13 ,significa q los motores ya estan en posición |
abckcp | 4:6956b568c810 | 7 | Cuando acabe de medir, enviar un 1 por el puerto digital output 12 , le dice al arduino q ha acabado y tiene q cambiar la posición |
abckcp | 4:6956b568c810 | 8 | |
abckcp | 4:6956b568c810 | 9 | */ |
BalB | 0:1d2060237094 | 10 | AnalogIn top_left(A0); // Analog InPut |
BalB | 0:1d2060237094 | 11 | AnalogIn top_right(A1); // Analog InPut |
BalB | 0:1d2060237094 | 12 | AnalogIn bot_left(A2); // Analog InPut |
BalB | 0:1d2060237094 | 13 | AnalogIn bot_right(A3); // Analog InPut |
abckcp | 4:6956b568c810 | 14 | //Para coordinar el arduino y el stm32 utilizamos dos pines digitales de mas |
abckcp | 4:6956b568c810 | 15 | //pin 13: si 0 arduino se esta moviendo no tomar medidas, si 1 arduino en posicion empezar a tomar medidas |
abckcp | 4:6956b568c810 | 16 | //pin 12: si 0 aun se esta midiendo no mover, si 1 medidas acabadas arduino ya se puede mover |
abckcp | 4:6956b568c810 | 17 | DigitalInOut medir(D6); //si llega 1=high el arduino ya esta en posicion y ya se puede medir |
abckcp | 4:6956b568c810 | 18 | DigitalInOut mover(D5); // ya hemos medido, decimos al arduino q cambie la posicion, enviar 1=high |
abckcp | 4:6956b568c810 | 19 | DigitalInOut end(D4); //calibracion acabada |
BalB | 1:3370c513ff5c | 20 | |
BalB | 0:1d2060237094 | 21 | |
BalB | 2:50b081df251a | 22 | const int N =100; //Number of measures x position |
BalB | 2:50b081df251a | 23 | |
BalB | 2:50b081df251a | 24 | |
BalB | 0:1d2060237094 | 25 | DigitalOut myled(LED1); // Digital OutPut |
BalB | 0:1d2060237094 | 26 | Serial pc(SERIAL_TX, SERIAL_RX); // Default USART is: 8N1 NO FlowControl |
BalB | 1:3370c513ff5c | 27 | |
BalB | 1:3370c513ff5c | 28 | |
BalB | 0:1d2060237094 | 29 | // Variables -------------------------------------------------------------- |
BalB | 1:3370c513ff5c | 30 | uint16_t measTL = 0; |
BalB | 1:3370c513ff5c | 31 | uint16_t measTR = 0; |
BalB | 1:3370c513ff5c | 32 | uint16_t measBL = 0; |
BalB | 1:3370c513ff5c | 33 | uint16_t measBR = 0; |
abckcp | 4:6956b568c810 | 34 | int j=0; //iterator 1 |
BalB | 2:50b081df251a | 35 | |
BalB | 2:50b081df251a | 36 | int mitja_esquerra_adalt= 0; //Average of measures of PH |
BalB | 2:50b081df251a | 37 | int mitja_dreta_adalt=0; //Average of measures of PH |
BalB | 2:50b081df251a | 38 | int mitja_esquerra_abaix=0; //Average of measures of PH |
BalB | 2:50b081df251a | 39 | int mitja_dreta_abaix = 0; //Average of measures of PH |
BalB | 2:50b081df251a | 40 | |
BalB | 2:50b081df251a | 41 | int val_top_left [N]= {0}; //instantaneous value of measure of PH |
BalB | 2:50b081df251a | 42 | int val_top_right [N]= {0}; //instantaneous value of measure of PH |
BalB | 2:50b081df251a | 43 | int val_bot_left [N]= {0}; //instantaneous value of measure of PH |
BalB | 2:50b081df251a | 44 | int val_bot_right [N]= {0}; //instantaneous value of measure of PH |
BalB | 2:50b081df251a | 45 | |
BalB | 2:50b081df251a | 46 | double sum_top_left= 0; //sumatory of measures of PH |
BalB | 2:50b081df251a | 47 | double sum_top_right= 0; //sumatory of measures of PH |
BalB | 2:50b081df251a | 48 | double sum_bot_left= 0; //sumatory of measures of PH |
BalB | 2:50b081df251a | 49 | double sum_bot_right= 0; //sumatory of measures of PH |
BalB | 2:50b081df251a | 50 | |
BalB | 2:50b081df251a | 51 | double sum_var_top_left=0; |
BalB | 2:50b081df251a | 52 | double sum_var_top_right=0; |
BalB | 2:50b081df251a | 53 | double sum_var_bot_left=0; |
BalB | 2:50b081df251a | 54 | double sum_var_bot_right=0; |
BalB | 2:50b081df251a | 55 | |
BalB | 2:50b081df251a | 56 | int val_var_top_left=0; //Variance Value for this PH |
BalB | 2:50b081df251a | 57 | int val_var_top_right=0; //Variance Value for this PH |
BalB | 2:50b081df251a | 58 | int val_var_bot_left=0; //Variance Value for this PH |
BalB | 2:50b081df251a | 59 | int val_var_bot_right=0; //Variance Value for this PH |
BalB | 2:50b081df251a | 60 | |
BalB | 2:50b081df251a | 61 | |
BalB | 2:50b081df251a | 62 | |
BalB | 2:50b081df251a | 63 | |
BalB | 1:3370c513ff5c | 64 | |
BalB | 0:1d2060237094 | 65 | //DigitalOut led(LED1); |
BalB | 0:1d2060237094 | 66 | |
BalB | 0:1d2060237094 | 67 | |
BalB | 0:1d2060237094 | 68 | // ATTENTION |
BalB | 0:1d2060237094 | 69 | // The two value below, must be changed in according to the Vcc and ADC |
BalB | 0:1d2060237094 | 70 | // resolution |
BalB | 0:1d2060237094 | 71 | float_t Valim = 3300; // This is the supplay voltage of ADC (or MCU) |
BalB | 0:1d2060237094 | 72 | float_t ADCres = 4096; // This is the ADC resolution that in this case si 12Bit |
abckcp | 4:6956b568c810 | 73 | // All NUCLEO boards use an ADC with 12bit resolution |
BalB | 0:1d2060237094 | 74 | |
BalB | 0:1d2060237094 | 75 | int main() |
BalB | 0:1d2060237094 | 76 | { |
abckcp | 4:6956b568c810 | 77 | pc.printf("\n\r\n\rSTART program\n\r"); |
abckcp | 4:6956b568c810 | 78 | |
BalB | 0:1d2060237094 | 79 | pc.printf("\n\r\n\rSTART program\n\r"); |
abckcp | 4:6956b568c810 | 80 | pc.printf("\n mitja_esquerra_adalt , var_top_left , mitja_dreta_adalt , var_top_right , mitja_esquerra_abaix , var_bot_left , mitja_dreta_abaix , var_bot_right\n"); |
abckcp | 4:6956b568c810 | 81 | medir.write(0); |
abckcp | 4:6956b568c810 | 82 | mover.write(1); |
abckcp | 4:6956b568c810 | 83 | end.write(0); |
abckcp | 4:6956b568c810 | 84 | while(end.read()!=1) { |
abckcp | 4:6956b568c810 | 85 | |
abckcp | 4:6956b568c810 | 86 | //mientras no este en la posicion, el arduino se esta moviendo, no se puede medir, se espera |
abckcp | 4:6956b568c810 | 87 | while(medir.read()==0) { |
abckcp | 4:6956b568c810 | 88 | } |
abckcp | 4:6956b568c810 | 89 | |
abckcp | 4:6956b568c810 | 90 | //cuando esta en posicion, medir |
abckcp | 4:6956b568c810 | 91 | if(medir.read()==1) { |
abckcp | 4:6956b568c810 | 92 | mover.write(0);// decir al arduino q aun no ha medido, no se puede mover |
abckcp | 4:6956b568c810 | 93 | sum_top_left=0; |
abckcp | 4:6956b568c810 | 94 | sum_top_right=0; |
abckcp | 4:6956b568c810 | 95 | sum_bot_left=0; |
abckcp | 4:6956b568c810 | 96 | sum_bot_right=0; |
abckcp | 4:6956b568c810 | 97 | |
abckcp | 4:6956b568c810 | 98 | sum_var_top_left=0; |
abckcp | 4:6956b568c810 | 99 | sum_var_top_right=0; |
abckcp | 4:6956b568c810 | 100 | sum_var_bot_left=0; |
abckcp | 4:6956b568c810 | 101 | sum_var_bot_right=0; |
abckcp | 4:6956b568c810 | 102 | |
abckcp | 4:6956b568c810 | 103 | while(j<N) { |
abckcp | 4:6956b568c810 | 104 | val_top_left[j]=top_left.read_u16(); //Taking N measures |
abckcp | 4:6956b568c810 | 105 | val_top_right[j]=top_right.read_u16(); //Taking N measures |
abckcp | 4:6956b568c810 | 106 | val_bot_left[j]=bot_left.read_u16(); //Taking N measures |
abckcp | 4:6956b568c810 | 107 | val_bot_right[j]=bot_right.read_u16(); //Taking N measures |
abckcp | 4:6956b568c810 | 108 | |
abckcp | 4:6956b568c810 | 109 | |
BalB | 2:50b081df251a | 110 | |
abckcp | 4:6956b568c810 | 111 | sum_top_left = sum_top_left + val_top_left[j]; //Making the sum of the measure for each PH |
abckcp | 4:6956b568c810 | 112 | sum_top_right = sum_top_right + val_top_right[j]; //Making the sum of the measure for each PH |
abckcp | 4:6956b568c810 | 113 | sum_bot_left = sum_bot_left + val_bot_left[j]; //Making the sum of the measure for each PH |
abckcp | 4:6956b568c810 | 114 | sum_bot_right = sum_bot_right + val_bot_right[j]; //Making the sum of the measure for each PH |
abckcp | 4:6956b568c810 | 115 | |
abckcp | 4:6956b568c810 | 116 | j++; |
abckcp | 4:6956b568c810 | 117 | |
abckcp | 4:6956b568c810 | 118 | } |
abckcp | 4:6956b568c810 | 119 | |
abckcp | 4:6956b568c810 | 120 | mitja_esquerra_adalt=double(rint((sum_top_left/N))); //Computiong the average |
abckcp | 4:6956b568c810 | 121 | mitja_dreta_adalt=double(rint((sum_top_right/N))); //Computiong the average |
abckcp | 4:6956b568c810 | 122 | mitja_esquerra_abaix=double(rint((sum_bot_left/N))); //Computiong the average |
abckcp | 4:6956b568c810 | 123 | mitja_dreta_abaix=double(rint((sum_bot_right/N))); //Computiong the average |
abckcp | 4:6956b568c810 | 124 | |
abckcp | 4:6956b568c810 | 125 | for (j=0; j<N; j++) { |
abckcp | 4:6956b568c810 | 126 | sum_var_top_left= ((val_top_left[j]-mitja_esquerra_adalt)*(val_top_left[j]-mitja_esquerra_adalt)) + sum_var_top_left; //Variance for each measure |
abckcp | 4:6956b568c810 | 127 | sum_var_top_right= ((val_top_right[j]-mitja_dreta_adalt)*(val_top_right[j]-mitja_dreta_adalt)) + sum_var_top_right; //Variance for each measure |
abckcp | 4:6956b568c810 | 128 | sum_var_bot_left= ((val_bot_left[j]-mitja_esquerra_abaix)*(val_bot_left[j]-mitja_esquerra_abaix)) + sum_var_bot_left; //Variance for each measure |
abckcp | 4:6956b568c810 | 129 | sum_var_bot_right= ((val_bot_right[j]-mitja_dreta_abaix)*(val_bot_right[j]-mitja_dreta_abaix)) + sum_var_bot_right; //Variance for each measure |
abckcp | 4:6956b568c810 | 130 | |
abckcp | 4:6956b568c810 | 131 | } |
abckcp | 4:6956b568c810 | 132 | val_var_top_left=double(rint(sqrt(sum_var_top_left/N))); //Obtaining Variance Value for this PH |
abckcp | 4:6956b568c810 | 133 | val_var_top_right=double(rint(sqrt(sum_var_top_right/N))); //Obtaining Variance Value for this PH |
abckcp | 4:6956b568c810 | 134 | val_var_bot_left=double(rint(sqrt(sum_var_bot_left/N))); //Obtaining Variance Value for this PH |
abckcp | 4:6956b568c810 | 135 | val_var_bot_right=double(rint(sqrt(sum_var_bot_right/N))); //Obtaining Variance Value for this PH |
abckcp | 4:6956b568c810 | 136 | |
abckcp | 4:6956b568c810 | 137 | |
abckcp | 4:6956b568c810 | 138 | //PRINTEJAR RESULTATS |
abckcp | 4:6956b568c810 | 139 | pc.printf("\n%d ,", mitja_esquerra_adalt); |
abckcp | 4:6956b568c810 | 140 | pc.printf(" %d ,", val_var_top_left); |
abckcp | 4:6956b568c810 | 141 | pc.printf(" %d ,", mitja_dreta_adalt); |
abckcp | 4:6956b568c810 | 142 | pc.printf(" %d ,", val_var_top_right); |
abckcp | 4:6956b568c810 | 143 | pc.printf(" %d ,", mitja_esquerra_abaix); |
abckcp | 4:6956b568c810 | 144 | pc.printf(" %d ,", val_var_bot_left); |
abckcp | 4:6956b568c810 | 145 | pc.printf(" %d ,", mitja_dreta_abaix); |
abckcp | 4:6956b568c810 | 146 | pc.printf(" %d ,", val_var_bot_right); |
BalB | 2:50b081df251a | 147 | |
BalB | 2:50b081df251a | 148 | |
abckcp | 4:6956b568c810 | 149 | } |
abckcp | 4:6956b568c810 | 150 | |
BalB | 2:50b081df251a | 151 | |
abckcp | 4:6956b568c810 | 152 | mover.write(1); //ya ha medido, decir al arduino que cambie la posicion |
abckcp | 4:6956b568c810 | 153 | //wait_ms(1000); //esperar por si acaso |
abckcp | 4:6956b568c810 | 154 | //para asegurar que la stm se espere hasta que cambien la posicion |
abckcp | 4:6956b568c810 | 155 | medir.write(0);//decir que el arduino se esta moviendo, no tomar medidas |
BalB | 0:1d2060237094 | 156 | } |
BalB | 0:1d2060237094 | 157 | } |
abckcp | 4:6956b568c810 | 158 |