![](/media/cache/group/V2.png.50x50_q85.png)
FN
Dependencies: mbed RF24Network RF24
main.cpp@10:09b236addeb5, 2019-05-01 (annotated)
- Committer:
- guillaume6544
- Date:
- Wed May 01 16:39:19 2019 +0000
- Revision:
- 10:09b236addeb5
- Parent:
- 9:ec9bf9f7645f
corrige erreur courte distance
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
akashvibhute | 0:3982c0e9eda1 | 1 | #include "mbed.h" |
akashvibhute | 0:3982c0e9eda1 | 2 | #include <RF24Network.h> |
akashvibhute | 2:926b93a68399 | 3 | #include <RF24.h> |
guillaume6544 | 6:d46e3c0c8d35 | 4 | |
akashvibhute | 0:3982c0e9eda1 | 5 | Serial pc(USBTX, USBRX); |
guillaume6544 | 6:d46e3c0c8d35 | 6 | InterruptIn button(D8); |
guillaume6544 | 6:d46e3c0c8d35 | 7 | AnalogIn LM35(A2); |
belkaous | 9:ec9bf9f7645f | 8 | void position_abs(double, double, double, double,double); |
guillaume6544 | 10:09b236addeb5 | 9 | |
wesleytiem | 4:f70f3b565af0 | 10 | RF24 radio(SPI_MOSI, SPI_MISO, SPI_SCK, D9, SPI_CS ); |
guillaume6544 | 5:bfef4ea383be | 11 | Timer timer; |
guillaume6544 | 5:bfef4ea383be | 12 | unsigned int temps; |
guillaume6544 | 5:bfef4ea383be | 13 | int a=0; |
guillaume6544 | 6:d46e3c0c8d35 | 14 | float temperature; |
guillaume6544 | 6:d46e3c0c8d35 | 15 | float vitesse; |
guillaume6544 | 10:09b236addeb5 | 16 | |
guillaume6544 | 7:1b47a2b6329a | 17 | const int n = 3; |
guillaume6544 | 7:1b47a2b6329a | 18 | int addr[n] = {01,02,03}; |
guillaume6544 | 10:09b236addeb5 | 19 | double d[n]; |
guillaume6544 | 10:09b236addeb5 | 20 | double dist; |
guillaume6544 | 6:d46e3c0c8d35 | 21 | int indice=0; |
guillaume6544 | 10:09b236addeb5 | 22 | |
guillaume6544 | 5:bfef4ea383be | 23 | void pressed() |
guillaume6544 | 5:bfef4ea383be | 24 | { |
guillaume6544 | 5:bfef4ea383be | 25 | button.disable_irq(); |
guillaume6544 | 5:bfef4ea383be | 26 | timer.stop(); |
guillaume6544 | 5:bfef4ea383be | 27 | temps = timer.read_us(); |
guillaume6544 | 10:09b236addeb5 | 28 | dist=(temps-370.0)*(vitesse/1000000.0); //calcule la distance (eqution de calibration du système, considére la reponse et vitesse) |
guillaume6544 | 10:09b236addeb5 | 29 | if(dist<0.14){ |
guillaume6544 | 10:09b236addeb5 | 30 | dist=dist+0.06; |
guillaume6544 | 10:09b236addeb5 | 31 | d[indice-1]=dist; |
guillaume6544 | 10:09b236addeb5 | 32 | } |
guillaume6544 | 10:09b236addeb5 | 33 | if(dist>0.14 && dist<0.26){ |
guillaume6544 | 10:09b236addeb5 | 34 | dist=dist+0.05; |
guillaume6544 | 10:09b236addeb5 | 35 | d[indice-1]=dist; |
guillaume6544 | 10:09b236addeb5 | 36 | } |
guillaume6544 | 10:09b236addeb5 | 37 | if(dist>0.26 && dist<0.34){ |
guillaume6544 | 10:09b236addeb5 | 38 | dist=dist+0.04; |
guillaume6544 | 10:09b236addeb5 | 39 | d[indice-1]=dist; |
guillaume6544 | 10:09b236addeb5 | 40 | } |
guillaume6544 | 10:09b236addeb5 | 41 | if(dist>0.34 && dist<0.51){ |
guillaume6544 | 10:09b236addeb5 | 42 | dist=dist+0.03; |
guillaume6544 | 10:09b236addeb5 | 43 | d[indice-1]=dist; |
guillaume6544 | 10:09b236addeb5 | 44 | } |
guillaume6544 | 10:09b236addeb5 | 45 | else{ |
guillaume6544 | 10:09b236addeb5 | 46 | d[indice-1]=dist; |
guillaume6544 | 10:09b236addeb5 | 47 | } |
guillaume6544 | 6:d46e3c0c8d35 | 48 | //printf("distance=%f\r\n",(temps-70.0)*(vitesse/1000000.0); |
guillaume6544 | 6:d46e3c0c8d35 | 49 | //wait_ms(3); |
guillaume6544 | 5:bfef4ea383be | 50 | timer.reset(); |
guillaume6544 | 6:d46e3c0c8d35 | 51 | //a=1; |
guillaume6544 | 5:bfef4ea383be | 52 | } |
guillaume6544 | 6:d46e3c0c8d35 | 53 | |
akashvibhute | 0:3982c0e9eda1 | 54 | // Network uses that radio |
akashvibhute | 0:3982c0e9eda1 | 55 | RF24Network network(radio); |
guillaume6544 | 6:d46e3c0c8d35 | 56 | |
akashvibhute | 0:3982c0e9eda1 | 57 | // Address of our node |
guillaume6544 | 7:1b47a2b6329a | 58 | const uint16_t this_node = 00; |
guillaume6544 | 6:d46e3c0c8d35 | 59 | |
akashvibhute | 0:3982c0e9eda1 | 60 | // Address of the other node |
guillaume6544 | 6:d46e3c0c8d35 | 61 | const uint16_t other_node1 = 00; |
guillaume6544 | 6:d46e3c0c8d35 | 62 | const uint16_t other_node2 = 001; |
guillaume6544 | 6:d46e3c0c8d35 | 63 | |
akashvibhute | 2:926b93a68399 | 64 | // How often to send payload packet to the other unit |
guillaume6544 | 7:1b47a2b6329a | 65 | const unsigned long interval = 1000; //ms |
guillaume6544 | 6:d46e3c0c8d35 | 66 | |
akashvibhute | 0:3982c0e9eda1 | 67 | // When did we last send? |
akashvibhute | 0:3982c0e9eda1 | 68 | unsigned long last_sent; |
akashvibhute | 1:5be48a9550c3 | 69 | Timer t; |
guillaume6544 | 6:d46e3c0c8d35 | 70 | |
akashvibhute | 0:3982c0e9eda1 | 71 | // How many have we sent already |
akashvibhute | 0:3982c0e9eda1 | 72 | unsigned long packets_sent; |
akashvibhute | 1:5be48a9550c3 | 73 | Timer t_packet; |
guillaume6544 | 6:d46e3c0c8d35 | 74 | |
akashvibhute | 0:3982c0e9eda1 | 75 | // Structure of our payload |
akashvibhute | 2:926b93a68399 | 76 | struct payload_t |
akashvibhute | 0:3982c0e9eda1 | 77 | { |
akashvibhute | 2:926b93a68399 | 78 | unsigned long ms; |
akashvibhute | 2:926b93a68399 | 79 | unsigned long counter; |
akashvibhute | 0:3982c0e9eda1 | 80 | }; |
guillaume6544 | 6:d46e3c0c8d35 | 81 | |
belkaous | 9:ec9bf9f7645f | 82 | void emmeteur(){ |
guillaume6544 | 10:09b236addeb5 | 83 | |
guillaume6544 | 6:d46e3c0c8d35 | 84 | pc.printf("Sending : %d...",indice+1); |
guillaume6544 | 6:d46e3c0c8d35 | 85 | payload_t payload_tx; |
guillaume6544 | 6:d46e3c0c8d35 | 86 | payload_tx.ms = t_packet.read_ms(); |
guillaume6544 | 6:d46e3c0c8d35 | 87 | payload_tx.counter = packets_sent++; |
guillaume6544 | 6:d46e3c0c8d35 | 88 | |
guillaume6544 | 6:d46e3c0c8d35 | 89 | RF24NetworkHeader header_tx(/*to node*/ addr[indice]); |
guillaume6544 | 6:d46e3c0c8d35 | 90 | bool ok = network.write(header_tx,&payload_tx,sizeof(payload_tx)); |
guillaume6544 | 6:d46e3c0c8d35 | 91 | if (ok){ |
guillaume6544 | 6:d46e3c0c8d35 | 92 | timer.start(); |
guillaume6544 | 6:d46e3c0c8d35 | 93 | pc.printf("ok : %d\r\n",indice+1); |
guillaume6544 | 6:d46e3c0c8d35 | 94 | // a=1; |
guillaume6544 | 6:d46e3c0c8d35 | 95 | } |
guillaume6544 | 6:d46e3c0c8d35 | 96 | else |
guillaume6544 | 6:d46e3c0c8d35 | 97 | pc.printf("failed : %d.\r\n",indice+1); |
guillaume6544 | 6:d46e3c0c8d35 | 98 | |
guillaume6544 | 6:d46e3c0c8d35 | 99 | |
guillaume6544 | 6:d46e3c0c8d35 | 100 | } |
guillaume6544 | 6:d46e3c0c8d35 | 101 | void emmeteur2(){ |
guillaume6544 | 10:09b236addeb5 | 102 | |
guillaume6544 | 6:d46e3c0c8d35 | 103 | pc.printf("Sending : 2..."); |
guillaume6544 | 6:d46e3c0c8d35 | 104 | payload_t payload_tx; |
guillaume6544 | 6:d46e3c0c8d35 | 105 | payload_tx.ms = t_packet.read_ms(); |
guillaume6544 | 6:d46e3c0c8d35 | 106 | payload_tx.counter = packets_sent++; |
guillaume6544 | 6:d46e3c0c8d35 | 107 | |
guillaume6544 | 6:d46e3c0c8d35 | 108 | RF24NetworkHeader header_tx(/*to node*/ other_node2); |
guillaume6544 | 6:d46e3c0c8d35 | 109 | bool ok = network.write(header_tx,&payload_tx,sizeof(payload_tx)); |
guillaume6544 | 6:d46e3c0c8d35 | 110 | if (ok){ |
guillaume6544 | 6:d46e3c0c8d35 | 111 | timer.start(); |
guillaume6544 | 6:d46e3c0c8d35 | 112 | pc.printf("ok : 2.\r\n"); |
guillaume6544 | 6:d46e3c0c8d35 | 113 | // a=1; |
guillaume6544 | 6:d46e3c0c8d35 | 114 | } |
guillaume6544 | 6:d46e3c0c8d35 | 115 | else |
belkaous | 9:ec9bf9f7645f | 116 | pc.printf("failed : 2.\r\n"); |
guillaume6544 | 6:d46e3c0c8d35 | 117 | } |
akashvibhute | 2:926b93a68399 | 118 | int main() |
akashvibhute | 0:3982c0e9eda1 | 119 | { |
guillaume6544 | 5:bfef4ea383be | 120 | // double cpt=0; |
guillaume6544 | 6:d46e3c0c8d35 | 121 | float d1,d2; |
wesleytiem | 4:f70f3b565af0 | 122 | pc.baud(115200); |
akashvibhute | 0:3982c0e9eda1 | 123 | wait_ms(1000); |
guillaume6544 | 6:d46e3c0c8d35 | 124 | |
guillaume6544 | 6:d46e3c0c8d35 | 125 | pc.printf("mBed RF24Network node: Tx\r\n"); |
akashvibhute | 0:3982c0e9eda1 | 126 | radio.begin(); |
guillaume6544 | 10:09b236addeb5 | 127 | network.begin(/*channel*/ 53, /*node address*/ this_node); |
akashvibhute | 0:3982c0e9eda1 | 128 | wait_ms(2000); |
akashvibhute | 1:5be48a9550c3 | 129 | t.start(); |
akashvibhute | 1:5be48a9550c3 | 130 | t_packet.start(); |
guillaume6544 | 7:1b47a2b6329a | 131 | // temperature = 25; |
guillaume6544 | 7:1b47a2b6329a | 132 | // for (int i=0;i<10;i++) { |
guillaume6544 | 7:1b47a2b6329a | 133 | // temperature += LM35.read(); |
guillaume6544 | 7:1b47a2b6329a | 134 | // wait_ms(10); |
belkaous | 9:ec9bf9f7645f | 135 | // } |
guillaume6544 | 7:1b47a2b6329a | 136 | // temperature = temperature*34; |
guillaume6544 | 6:d46e3c0c8d35 | 137 | |
guillaume6544 | 7:1b47a2b6329a | 138 | vitesse = 342.0;//331.5 + 0.595*temperature; |
guillaume6544 | 6:d46e3c0c8d35 | 139 | |
guillaume6544 | 5:bfef4ea383be | 140 | button.rise(&pressed); |
akashvibhute | 2:926b93a68399 | 141 | while(1) |
akashvibhute | 0:3982c0e9eda1 | 142 | { |
guillaume6544 | 5:bfef4ea383be | 143 | |
akashvibhute | 0:3982c0e9eda1 | 144 | // Pump the network regularly |
akashvibhute | 0:3982c0e9eda1 | 145 | network.update(); |
guillaume6544 | 5:bfef4ea383be | 146 | //if(a==0){ |
akashvibhute | 1:5be48a9550c3 | 147 | /* If it's time to send a message, send it! */ |
akashvibhute | 1:5be48a9550c3 | 148 | unsigned long now = t.read_ms(); |
akashvibhute | 2:926b93a68399 | 149 | if ( now >= interval ) |
akashvibhute | 1:5be48a9550c3 | 150 | { |
guillaume6544 | 7:1b47a2b6329a | 151 | if(indice > 2) { |
guillaume6544 | 6:d46e3c0c8d35 | 152 | for(int j=0; j<n; j++) |
belkaous | 9:ec9bf9f7645f | 153 | printf("distance %d = %f\r\n",j+1,d[j]); |
belkaous | 9:ec9bf9f7645f | 154 | position_abs(610.0,600.0,d[0]*1000,d[1]*1000,d[2]*1000); |
guillaume6544 | 6:d46e3c0c8d35 | 155 | indice = 0; |
belkaous | 9:ec9bf9f7645f | 156 | } |
akashvibhute | 1:5be48a9550c3 | 157 | t.reset(); |
guillaume6544 | 5:bfef4ea383be | 158 | |
guillaume6544 | 5:bfef4ea383be | 159 | button.enable_irq(); |
guillaume6544 | 6:d46e3c0c8d35 | 160 | emmeteur(); |
guillaume6544 | 6:d46e3c0c8d35 | 161 | button.enable_irq(); |
guillaume6544 | 10:09b236addeb5 | 162 | |
guillaume6544 | 6:d46e3c0c8d35 | 163 | indice++; |
akashvibhute | 2:926b93a68399 | 164 | } |
belkaous | 9:ec9bf9f7645f | 165 | } |
belkaous | 9:ec9bf9f7645f | 166 | } |
guillaume6544 | 10:09b236addeb5 | 167 | |
belkaous | 9:ec9bf9f7645f | 168 | void position_abs(double L, double l, double d1, double d2, double d3) |
belkaous | 9:ec9bf9f7645f | 169 | { |
belkaous | 9:ec9bf9f7645f | 170 | printf("Hellooooooo auj!! \r\n"); |
belkaous | 9:ec9bf9f7645f | 171 | |
belkaous | 9:ec9bf9f7645f | 172 | double w = l; /* dim terrain */ |
belkaous | 9:ec9bf9f7645f | 173 | double u = sqrt(pow((double) L,2)+pow((double) l/2,2)), v = u; |
belkaous | 9:ec9bf9f7645f | 174 | double q = acos((w/2)/v), r = 18.44, t=q; |
belkaous | 9:ec9bf9f7645f | 175 | |
belkaous | 9:ec9bf9f7645f | 176 | /* triangle (B1,R,B3) */ |
belkaous | 9:ec9bf9f7645f | 177 | |
belkaous | 9:ec9bf9f7645f | 178 | double k = acos((pow(d1,2)+pow(v,2)-pow(d3,2))/(2*d1*v)); |
belkaous | 9:ec9bf9f7645f | 179 | //printf("k = %lf \n\r",k); |
belkaous | 9:ec9bf9f7645f | 180 | double alpha = acos((pow(d1,2)+pow(d3,2)-pow(v,2))/(2*d1*d3)); |
belkaous | 9:ec9bf9f7645f | 181 | //printf("alpha = %lf \n\r",alpha); |
belkaous | 9:ec9bf9f7645f | 182 | double p = acos((pow(v,2)+pow(d3,2)-pow(d1,2))/(2*v*d3)); |
belkaous | 9:ec9bf9f7645f | 183 | //printf("p = %lf \n\r",p); |
belkaous | 9:ec9bf9f7645f | 184 | |
belkaous | 9:ec9bf9f7645f | 185 | /* triangle (B2,R,B3)*/ |
belkaous | 9:ec9bf9f7645f | 186 | |
belkaous | 9:ec9bf9f7645f | 187 | double O = acos((pow(d3,2)+pow(u,2)-pow(d2,2))/(2*d3*u)); |
belkaous | 9:ec9bf9f7645f | 188 | //double O = acos((pow(d3,2)+pow(d2,2)-pow(u,2))/(2*d3*d2)); |
belkaous | 9:ec9bf9f7645f | 189 | //printf("O = %lf \n\r",O); |
belkaous | 9:ec9bf9f7645f | 190 | double m = acos((pow(u,2)+pow(d2,2)-pow(d3,2))/(2*u*d2)); |
belkaous | 9:ec9bf9f7645f | 191 | //double m = acos((pow(d3,2)+pow(u,2)-pow(d2,2))/(2*u*d3)); |
belkaous | 9:ec9bf9f7645f | 192 | //printf("m = %lf \n\r",m); |
belkaous | 9:ec9bf9f7645f | 193 | /* triangle (B1,R,B2) */ |
guillaume6544 | 5:bfef4ea383be | 194 | |
belkaous | 9:ec9bf9f7645f | 195 | double i = acos(((double) pow((double) w,2)+(double) pow((double) d1,2)-(double) pow((double) d2,2))/(double) (2*w*d1)); |
belkaous | 9:ec9bf9f7645f | 196 | //printf(" i= %lf \n\r",i); |
belkaous | 9:ec9bf9f7645f | 197 | double j = acos(((double) pow((double) d2,2)+(double) pow((double) w,2)-(double) pow((double) d1,2))/(double) (2*w*d2)); |
belkaous | 9:ec9bf9f7645f | 198 | //printf(" j= %lf \n\r",j); |
belkaous | 9:ec9bf9f7645f | 199 | |
belkaous | 9:ec9bf9f7645f | 200 | /* triangle (B1,R,B3) */ |
belkaous | 9:ec9bf9f7645f | 201 | double x1 = w/2 + cos(3.14-(p+q))*d3; |
belkaous | 9:ec9bf9f7645f | 202 | //printf(" x1= %lf \n\r",x1); |
belkaous | 9:ec9bf9f7645f | 203 | double y1 = sin(3.14-(p+q))*d3; |
belkaous | 9:ec9bf9f7645f | 204 | //printf(" y1= %lf \n\r",y1); |
belkaous | 9:ec9bf9f7645f | 205 | |
belkaous | 9:ec9bf9f7645f | 206 | /* triangle (B1,R,B2) */ |
belkaous | 9:ec9bf9f7645f | 207 | |
belkaous | 9:ec9bf9f7645f | 208 | double x2 = cos(i) *d1; |
belkaous | 9:ec9bf9f7645f | 209 | //printf(" x2= %lf \n\r",x2); |
belkaous | 9:ec9bf9f7645f | 210 | double y2 = L - sin(i)*d1; |
belkaous | 9:ec9bf9f7645f | 211 | //printf(" y2= %lf \n\r",y2); |
belkaous | 9:ec9bf9f7645f | 212 | |
belkaous | 9:ec9bf9f7645f | 213 | /* triangle (B2,R,B3)*/ |
belkaous | 9:ec9bf9f7645f | 214 | |
belkaous | 9:ec9bf9f7645f | 215 | double x3 = w/2 + cos(O+t)*d3; |
belkaous | 9:ec9bf9f7645f | 216 | //printf(" x3= %lf \n\r",x3); |
belkaous | 9:ec9bf9f7645f | 217 | double y3 = sin(O+t)*d3; |
belkaous | 9:ec9bf9f7645f | 218 | //printf(" y3= %lf \n\r",y3); |
belkaous | 9:ec9bf9f7645f | 219 | |
belkaous | 9:ec9bf9f7645f | 220 | double xt = (x1+x2+x3)/3; |
belkaous | 9:ec9bf9f7645f | 221 | printf(" xt= %lf \r\n",xt); |
belkaous | 9:ec9bf9f7645f | 222 | double yt = (y1+y2+y3)/3; |
belkaous | 9:ec9bf9f7645f | 223 | printf(" yt= %lf \r\n",yt); |
belkaous | 9:ec9bf9f7645f | 224 | } |
guillaume6544 | 10:09b236addeb5 | 225 |