Team DIANA / Mbed 2 deprecated IPKF_Test_nucleo

test_IPKF

Dependencies:   mbed

source/main.cpp@0:fb6e494a7656, 2016-04-20 (annotated)

Committer:
LudovicoDani
Date:
Wed Apr 20 10:03:58 2016 +0000
Revision:
0:fb6e494a7656
IPKF_Test_nucleo

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LudovicoDani 0:fb6e494a7656 1 #include "mbed.h"
LudovicoDani 0:fb6e494a7656 2 #include "params.h"
LudovicoDani 0:fb6e494a7656 3 #include "matrix.h"
LudovicoDani 0:fb6e494a7656 4 #include "kinematic.h"
LudovicoDani 0:fb6e494a7656 5 #include "pathPlanning.h"
LudovicoDani 0:fb6e494a7656 6
LudovicoDani 0:fb6e494a7656 7 DigitalIn sw2(PC_13);
LudovicoDani 0:fb6e494a7656 8 Serial pc(USBTX, USBRX);
LudovicoDani 0:fb6e494a7656 9 //DigitalOut led_green(LED_GREEN);
LudovicoDani 0:fb6e494a7656 10
LudovicoDani 0:fb6e494a7656 11 matrix path;
LudovicoDani 0:fb6e494a7656 12 matrix p0;
LudovicoDani 0:fb6e494a7656 13 matrix pf;
LudovicoDani 0:fb6e494a7656 14
LudovicoDani 0:fb6e494a7656 15 matrix p;
LudovicoDani 0:fb6e494a7656 16 matrix q;
LudovicoDani 0:fb6e494a7656 17
LudovicoDani 0:fb6e494a7656 18 Timer t;
LudovicoDani 0:fb6e494a7656 19
LudovicoDani 0:fb6e494a7656 20 void check_sw2(void)
LudovicoDani 0:fb6e494a7656 21 {
LudovicoDani 0:fb6e494a7656 22 if (sw2 == 0) {
LudovicoDani 0:fb6e494a7656 23 //pc.printf("SW2 button pressed. \n");
LudovicoDani 0:fb6e494a7656 24 //led_green = 0;
LudovicoDani 0:fb6e494a7656 25 t.start();
LudovicoDani 0:fb6e494a7656 26 for(int k = 0; k < 1 ; k++) {
LudovicoDani 0:fb6e494a7656 27 for(int j = 0; j < path.col; j++) {
LudovicoDani 0:fb6e494a7656 28 for(int i = 0; i < 6; i++) {
LudovicoDani 0:fb6e494a7656 29 p.element[i][0] = path.element[i][j];
LudovicoDani 0:fb6e494a7656 30 //pc.printf("%.4f ", p.element[i][0]);
LudovicoDani 0:fb6e494a7656 31 }
LudovicoDani 0:fb6e494a7656 32
LudovicoDani 0:fb6e494a7656 33 //pc.printf("\n");
LudovicoDani 0:fb6e494a7656 34 IPKF(&p, &q);
LudovicoDani 0:fb6e494a7656 35
LudovicoDani 0:fb6e494a7656 36 pc.printf("%f ", q.element[0][0]);
LudovicoDani 0:fb6e494a7656 37 pc.printf("\n");
LudovicoDani 0:fb6e494a7656 38
LudovicoDani 0:fb6e494a7656 39 //for(int i = 0; i < 6; i++) {
LudovicoDani 0:fb6e494a7656 40 // pc.printf("%.4f ", q.element[i][0]);
LudovicoDani 0:fb6e494a7656 41 //}
LudovicoDani 0:fb6e494a7656 42 //pc.printf("\n");
LudovicoDani 0:fb6e494a7656 43
LudovicoDani 0:fb6e494a7656 44 //DPKF(&q,&p);
LudovicoDani 0:fb6e494a7656 45
LudovicoDani 0:fb6e494a7656 46 //for(int i = 0; i < 6; i++) {
LudovicoDani 0:fb6e494a7656 47 // pc.printf("%.4f ", p.element[i][0]);
LudovicoDani 0:fb6e494a7656 48 //}
LudovicoDani 0:fb6e494a7656 49 //pc.printf("\nj=%d\n", j);
LudovicoDani 0:fb6e494a7656 50 }
LudovicoDani 0:fb6e494a7656 51 }
LudovicoDani 0:fb6e494a7656 52 wait(2);
LudovicoDani 0:fb6e494a7656 53 pc.printf("%f", 0.0);
LudovicoDani 0:fb6e494a7656 54 wait(2);
LudovicoDani 0:fb6e494a7656 55 t.stop();
LudovicoDani 0:fb6e494a7656 56 //pc.printf("%f s\n", t.read());
LudovicoDani 0:fb6e494a7656 57 t.reset();
LudovicoDani 0:fb6e494a7656 58 }
LudovicoDani 0:fb6e494a7656 59 //led_green = 1;
LudovicoDani 0:fb6e494a7656 60 return;
LudovicoDani 0:fb6e494a7656 61 }
LudovicoDani 0:fb6e494a7656 62
LudovicoDani 0:fb6e494a7656 63 int main()
LudovicoDani 0:fb6e494a7656 64 {
LudovicoDani 0:fb6e494a7656 65 //led_green = 1;
LudovicoDani 0:fb6e494a7656 66 pc.baud(115200);
LudovicoDani 0:fb6e494a7656 67
LudovicoDani 0:fb6e494a7656 68 InitMatrix(&path, 6, 100);
LudovicoDani 0:fb6e494a7656 69 InitMatrix(&p0, 6, 1);
LudovicoDani 0:fb6e494a7656 70 InitMatrix(&pf, 6, 1);
LudovicoDani 0:fb6e494a7656 71
LudovicoDani 0:fb6e494a7656 72 InitMatrix(&p, 6, 1);
LudovicoDani 0:fb6e494a7656 73 InitMatrix(&q, 6, 1);
LudovicoDani 0:fb6e494a7656 74
LudovicoDani 0:fb6e494a7656 75 p0.element[0][0]= 0.4;
LudovicoDani 0:fb6e494a7656 76 p0.element[1][0]= 0;
LudovicoDani 0:fb6e494a7656 77 p0.element[2][0]= 0.4;
LudovicoDani 0:fb6e494a7656 78 p0.element[3][0]= 0;
LudovicoDani 0:fb6e494a7656 79 p0.element[4][0]= 0;
LudovicoDani 0:fb6e494a7656 80 p0.element[5][0]= 0;
LudovicoDani 0:fb6e494a7656 81
LudovicoDani 0:fb6e494a7656 82 pf.element[0][0]= 0.7;
LudovicoDani 0:fb6e494a7656 83 pf.element[1][0]= 0.4;
LudovicoDani 0:fb6e494a7656 84 pf.element[2][0]= -0.1;
LudovicoDani 0:fb6e494a7656 85 pf.element[3][0]= PI/4;
LudovicoDani 0:fb6e494a7656 86 pf.element[4][0]= PI/6;
LudovicoDani 0:fb6e494a7656 87 pf.element[5][0]= -PI/3;
LudovicoDani 0:fb6e494a7656 88
LudovicoDani 0:fb6e494a7656 89 GeneratePath(&p0,&pf,&path);
LudovicoDani 0:fb6e494a7656 90 while(true) {
LudovicoDani 0:fb6e494a7656 91 check_sw2();
LudovicoDani 0:fb6e494a7656 92 wait(0.1);
LudovicoDani 0:fb6e494a7656 93 }
LudovicoDani 0:fb6e494a7656 94 }