Polybot Grenoble / Mbed 2 deprecated RF24Network_Send_FN

FN

Dependencies:   mbed RF24Network RF24

main.cpp@9:ec9bf9f7645f, 2019-04-08 (annotated)

Committer:
belkaous
Date:
Mon Apr 08 12:51:46 2019 +0000
Revision:
9:ec9bf9f7645f
Parent:
8:b0775f476afc
Child:
10:09b236addeb5
FN

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);
akashvibhute 0:3982c0e9eda1 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 6:d46e3c0c8d35 16
guillaume6544 7:1b47a2b6329a 17 const int n = 3;
guillaume6544 7:1b47a2b6329a 18 int addr[n] = {01,02,03};
guillaume6544 6:d46e3c0c8d35 19 float d[n];
guillaume6544 6:d46e3c0c8d35 20 int indice=0;
guillaume6544 5:bfef4ea383be 21
guillaume6544 5:bfef4ea383be 22 void pressed()
guillaume6544 5:bfef4ea383be 23 {
guillaume6544 5:bfef4ea383be 24 button.disable_irq();
guillaume6544 5:bfef4ea383be 25 timer.stop();
guillaume6544 5:bfef4ea383be 26 temps = timer.read_us();
wesleytiem 8:b0775f476afc 27 d[indice-1]=(temps-370.0)*(vitesse/1000000.0);
guillaume6544 6:d46e3c0c8d35 28 //printf("distance=%f\r\n",(temps-70.0)*(vitesse/1000000.0);
guillaume6544 6:d46e3c0c8d35 29 //wait_ms(3);
guillaume6544 5:bfef4ea383be 30 timer.reset();
guillaume6544 6:d46e3c0c8d35 31 //a=1;
guillaume6544 5:bfef4ea383be 32 }
guillaume6544 6:d46e3c0c8d35 33
akashvibhute 0:3982c0e9eda1 34 // Network uses that radio
akashvibhute 0:3982c0e9eda1 35 RF24Network network(radio);
guillaume6544 6:d46e3c0c8d35 36
akashvibhute 0:3982c0e9eda1 37 // Address of our node
guillaume6544 7:1b47a2b6329a 38 const uint16_t this_node = 00;
guillaume6544 6:d46e3c0c8d35 39
akashvibhute 0:3982c0e9eda1 40 // Address of the other node
guillaume6544 6:d46e3c0c8d35 41 const uint16_t other_node1 = 00;
guillaume6544 6:d46e3c0c8d35 42 const uint16_t other_node2 = 001;
guillaume6544 6:d46e3c0c8d35 43
akashvibhute 2:926b93a68399 44 // How often to send payload packet to the other unit
guillaume6544 7:1b47a2b6329a 45 const unsigned long interval = 1000; //ms
guillaume6544 6:d46e3c0c8d35 46
akashvibhute 0:3982c0e9eda1 47 // When did we last send?
akashvibhute 0:3982c0e9eda1 48 unsigned long last_sent;
akashvibhute 1:5be48a9550c3 49 Timer t;
guillaume6544 6:d46e3c0c8d35 50
akashvibhute 0:3982c0e9eda1 51 // How many have we sent already
akashvibhute 0:3982c0e9eda1 52 unsigned long packets_sent;
akashvibhute 1:5be48a9550c3 53 Timer t_packet;
guillaume6544 6:d46e3c0c8d35 54
akashvibhute 0:3982c0e9eda1 55 // Structure of our payload
akashvibhute 2:926b93a68399 56 struct payload_t
akashvibhute 0:3982c0e9eda1 57 {
akashvibhute 2:926b93a68399 58 unsigned long ms;
akashvibhute 2:926b93a68399 59 unsigned long counter;
akashvibhute 0:3982c0e9eda1 60 };
guillaume6544 6:d46e3c0c8d35 61
belkaous 9:ec9bf9f7645f 62 void emmeteur(){
akashvibhute 0:3982c0e9eda1 63
guillaume6544 6:d46e3c0c8d35 64 pc.printf("Sending : %d...",indice+1);
guillaume6544 6:d46e3c0c8d35 65 payload_t payload_tx;
guillaume6544 6:d46e3c0c8d35 66 payload_tx.ms = t_packet.read_ms();
guillaume6544 6:d46e3c0c8d35 67 payload_tx.counter = packets_sent++;
guillaume6544 6:d46e3c0c8d35 68
guillaume6544 6:d46e3c0c8d35 69 RF24NetworkHeader header_tx(/*to node*/ addr[indice]);
guillaume6544 6:d46e3c0c8d35 70 bool ok = network.write(header_tx,&payload_tx,sizeof(payload_tx));
guillaume6544 6:d46e3c0c8d35 71 if (ok){
guillaume6544 6:d46e3c0c8d35 72 timer.start();
guillaume6544 6:d46e3c0c8d35 73 pc.printf("ok : %d\r\n",indice+1);
guillaume6544 6:d46e3c0c8d35 74 // a=1;
guillaume6544 6:d46e3c0c8d35 75 }
guillaume6544 6:d46e3c0c8d35 76 else
guillaume6544 6:d46e3c0c8d35 77 pc.printf("failed : %d.\r\n",indice+1);
guillaume6544 6:d46e3c0c8d35 78
guillaume6544 6:d46e3c0c8d35 79
guillaume6544 6:d46e3c0c8d35 80 }
guillaume6544 6:d46e3c0c8d35 81 void emmeteur2(){
akashvibhute 0:3982c0e9eda1 82
guillaume6544 6:d46e3c0c8d35 83 pc.printf("Sending : 2...");
guillaume6544 6:d46e3c0c8d35 84 payload_t payload_tx;
guillaume6544 6:d46e3c0c8d35 85 payload_tx.ms = t_packet.read_ms();
guillaume6544 6:d46e3c0c8d35 86 payload_tx.counter = packets_sent++;
guillaume6544 6:d46e3c0c8d35 87
guillaume6544 6:d46e3c0c8d35 88 RF24NetworkHeader header_tx(/*to node*/ other_node2);
guillaume6544 6:d46e3c0c8d35 89 bool ok = network.write(header_tx,&payload_tx,sizeof(payload_tx));
guillaume6544 6:d46e3c0c8d35 90 if (ok){
guillaume6544 6:d46e3c0c8d35 91 timer.start();
guillaume6544 6:d46e3c0c8d35 92 pc.printf("ok : 2.\r\n");
guillaume6544 6:d46e3c0c8d35 93 // a=1;
guillaume6544 6:d46e3c0c8d35 94 }
guillaume6544 6:d46e3c0c8d35 95 else
belkaous 9:ec9bf9f7645f 96 pc.printf("failed : 2.\r\n");
guillaume6544 6:d46e3c0c8d35 97 }
akashvibhute 2:926b93a68399 98 int main()
akashvibhute 0:3982c0e9eda1 99 {
guillaume6544 5:bfef4ea383be 100 // double cpt=0;
guillaume6544 6:d46e3c0c8d35 101 float d1,d2;
wesleytiem 4:f70f3b565af0 102 pc.baud(115200);
akashvibhute 0:3982c0e9eda1 103 wait_ms(1000);
guillaume6544 6:d46e3c0c8d35 104
guillaume6544 6:d46e3c0c8d35 105 pc.printf("mBed RF24Network node: Tx\r\n");
akashvibhute 0:3982c0e9eda1 106 radio.begin();
akashvibhute 0:3982c0e9eda1 107 network.begin(/*channel*/ 90, /*node address*/ this_node);
akashvibhute 0:3982c0e9eda1 108 wait_ms(2000);
akashvibhute 1:5be48a9550c3 109 t.start();
akashvibhute 1:5be48a9550c3 110 t_packet.start();
guillaume6544 7:1b47a2b6329a 111 // temperature = 25;
guillaume6544 7:1b47a2b6329a 112 // for (int i=0;i<10;i++) {
guillaume6544 7:1b47a2b6329a 113 // temperature += LM35.read();
guillaume6544 7:1b47a2b6329a 114 // wait_ms(10);
belkaous 9:ec9bf9f7645f 115 // }
guillaume6544 7:1b47a2b6329a 116 // temperature = temperature*34;
guillaume6544 6:d46e3c0c8d35 117
guillaume6544 7:1b47a2b6329a 118 vitesse = 342.0;//331.5 + 0.595*temperature;
guillaume6544 6:d46e3c0c8d35 119
guillaume6544 5:bfef4ea383be 120 button.rise(&pressed);
akashvibhute 2:926b93a68399 121 while(1)
akashvibhute 0:3982c0e9eda1 122 {
guillaume6544 5:bfef4ea383be 123
akashvibhute 0:3982c0e9eda1 124 // Pump the network regularly
akashvibhute 0:3982c0e9eda1 125 network.update();
guillaume6544 5:bfef4ea383be 126 //if(a==0){
akashvibhute 1:5be48a9550c3 127 /* If it's time to send a message, send it! */
akashvibhute 1:5be48a9550c3 128 unsigned long now = t.read_ms();
akashvibhute 2:926b93a68399 129 if ( now >= interval )
akashvibhute 1:5be48a9550c3 130 {
guillaume6544 7:1b47a2b6329a 131 if(indice > 2) {
guillaume6544 6:d46e3c0c8d35 132 for(int j=0; j<n; j++)
belkaous 9:ec9bf9f7645f 133 printf("distance %d = %f\r\n",j+1,d[j]);
belkaous 9:ec9bf9f7645f 134 position_abs(610.0,600.0,d[0]*1000,d[1]*1000,d[2]*1000);
guillaume6544 6:d46e3c0c8d35 135 indice = 0;
belkaous 9:ec9bf9f7645f 136 }
akashvibhute 1:5be48a9550c3 137 t.reset();
guillaume6544 5:bfef4ea383be 138
guillaume6544 5:bfef4ea383be 139 button.enable_irq();
guillaume6544 6:d46e3c0c8d35 140 emmeteur();
guillaume6544 6:d46e3c0c8d35 141 button.enable_irq();
akashvibhute 2:926b93a68399 142
guillaume6544 6:d46e3c0c8d35 143 indice++;
akashvibhute 2:926b93a68399 144 }
belkaous 9:ec9bf9f7645f 145 }
belkaous 9:ec9bf9f7645f 146 }
belkaous 9:ec9bf9f7645f 147
belkaous 9:ec9bf9f7645f 148 void position_abs(double L, double l, double d1, double d2, double d3)
belkaous 9:ec9bf9f7645f 149 {
belkaous 9:ec9bf9f7645f 150 printf("Hellooooooo auj!! \r\n");
belkaous 9:ec9bf9f7645f 151
belkaous 9:ec9bf9f7645f 152 double w = l; /* dim terrain */
belkaous 9:ec9bf9f7645f 153 double u = sqrt(pow((double) L,2)+pow((double) l/2,2)), v = u;
belkaous 9:ec9bf9f7645f 154 double q = acos((w/2)/v), r = 18.44, t=q;
belkaous 9:ec9bf9f7645f 155
belkaous 9:ec9bf9f7645f 156 /* triangle (B1,R,B3) */
belkaous 9:ec9bf9f7645f 157
belkaous 9:ec9bf9f7645f 158 double k = acos((pow(d1,2)+pow(v,2)-pow(d3,2))/(2*d1*v));
belkaous 9:ec9bf9f7645f 159 //printf("k = %lf \n\r",k);
belkaous 9:ec9bf9f7645f 160 double alpha = acos((pow(d1,2)+pow(d3,2)-pow(v,2))/(2*d1*d3));
belkaous 9:ec9bf9f7645f 161 //printf("alpha = %lf \n\r",alpha);
belkaous 9:ec9bf9f7645f 162 double p = acos((pow(v,2)+pow(d3,2)-pow(d1,2))/(2*v*d3));
belkaous 9:ec9bf9f7645f 163 //printf("p = %lf \n\r",p);
belkaous 9:ec9bf9f7645f 164
belkaous 9:ec9bf9f7645f 165 /* triangle (B2,R,B3)*/
belkaous 9:ec9bf9f7645f 166
belkaous 9:ec9bf9f7645f 167 double O = acos((pow(d3,2)+pow(u,2)-pow(d2,2))/(2*d3*u));
belkaous 9:ec9bf9f7645f 168 //double O = acos((pow(d3,2)+pow(d2,2)-pow(u,2))/(2*d3*d2));
belkaous 9:ec9bf9f7645f 169 //printf("O = %lf \n\r",O);
belkaous 9:ec9bf9f7645f 170 double m = acos((pow(u,2)+pow(d2,2)-pow(d3,2))/(2*u*d2));
belkaous 9:ec9bf9f7645f 171 //double m = acos((pow(d3,2)+pow(u,2)-pow(d2,2))/(2*u*d3));
belkaous 9:ec9bf9f7645f 172 //printf("m = %lf \n\r",m);
belkaous 9:ec9bf9f7645f 173 /* triangle (B1,R,B2) */
guillaume6544 5:bfef4ea383be 174
belkaous 9:ec9bf9f7645f 175 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 176 //printf(" i= %lf \n\r",i);
belkaous 9:ec9bf9f7645f 177 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 178 //printf(" j= %lf \n\r",j);
belkaous 9:ec9bf9f7645f 179
belkaous 9:ec9bf9f7645f 180 /* triangle (B1,R,B3) */
belkaous 9:ec9bf9f7645f 181 double x1 = w/2 + cos(3.14-(p+q))*d3;
belkaous 9:ec9bf9f7645f 182 //printf(" x1= %lf \n\r",x1);
belkaous 9:ec9bf9f7645f 183 double y1 = sin(3.14-(p+q))*d3;
belkaous 9:ec9bf9f7645f 184 //printf(" y1= %lf \n\r",y1);
belkaous 9:ec9bf9f7645f 185
belkaous 9:ec9bf9f7645f 186 /* triangle (B1,R,B2) */
belkaous 9:ec9bf9f7645f 187
belkaous 9:ec9bf9f7645f 188 double x2 = cos(i) *d1;
belkaous 9:ec9bf9f7645f 189 //printf(" x2= %lf \n\r",x2);
belkaous 9:ec9bf9f7645f 190 double y2 = L - sin(i)*d1;
belkaous 9:ec9bf9f7645f 191 //printf(" y2= %lf \n\r",y2);
belkaous 9:ec9bf9f7645f 192
belkaous 9:ec9bf9f7645f 193 /* triangle (B2,R,B3)*/
belkaous 9:ec9bf9f7645f 194
belkaous 9:ec9bf9f7645f 195 double x3 = w/2 + cos(O+t)*d3;
belkaous 9:ec9bf9f7645f 196 //printf(" x3= %lf \n\r",x3);
belkaous 9:ec9bf9f7645f 197 double y3 = sin(O+t)*d3;
belkaous 9:ec9bf9f7645f 198 //printf(" y3= %lf \n\r",y3);
belkaous 9:ec9bf9f7645f 199
belkaous 9:ec9bf9f7645f 200 double xt = (x1+x2+x3)/3;
belkaous 9:ec9bf9f7645f 201 printf(" xt= %lf \r\n",xt);
belkaous 9:ec9bf9f7645f 202 double yt = (y1+y2+y3)/3;
belkaous 9:ec9bf9f7645f 203 printf(" yt= %lf \r\n",yt);
belkaous 9:ec9bf9f7645f 204 }