Polybot Grenoble / Mbed 2 deprecated RF24Network_Send_FN

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?

UserRevisionLine numberNew 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