main.cpp

00001 #include "mbed.h"
00002 #include "params.h"
00003 #include "matrix.h"
00004 #include "kinematic.h"
00005 #include "pathPlanning.h"
00006 
00007 DigitalIn sw2(PC_13);
00008 Serial pc(USBTX, USBRX);
00009 //DigitalOut led_green(LED_GREEN);
00010 
00011 matrix path;
00012 matrix p0;
00013 matrix pf;
00014 
00015 matrix p;
00016 matrix q;
00017 
00018 Timer t;
00019 
00020 void check_sw2(void)
00021 {
00022     if (sw2 == 0) {
00023         //pc.printf("SW2 button pressed. \n");
00024         //led_green = 0;
00025         t.start();
00026         for(int k = 0; k < 1 ; k++) {
00027             for(int j = 0; j < path.col; j++) {
00028                 for(int i = 0; i < 6; i++) {
00029                     p.element[i][0] = path.element[i][j];
00030                     //pc.printf("%.4f    ", p.element[i][0]);
00031                 }
00032 
00033                 //pc.printf("\n");
00034                 IPKF(&p, &q);
00035                 
00036                 pc.printf("%f    ", q.element[0][0]);
00037                 pc.printf("\n");
00038                 
00039                 //for(int i = 0; i < 6; i++) {
00040                 //    pc.printf("%.4f    ", q.element[i][0]);
00041                 //}
00042                 //pc.printf("\n");
00043 
00044                 //DPKF(&q,&p);
00045 
00046                 //for(int i = 0; i < 6; i++) {
00047                 //    pc.printf("%.4f    ", p.element[i][0]);
00048                 //}
00049                 //pc.printf("\nj=%d\n", j);
00050             }
00051         }
00052         wait(2);
00053         pc.printf("%f", 0.0);
00054         wait(2);
00055         t.stop();
00056         //pc.printf("%f s\n", t.read());
00057         t.reset();
00058     }
00059     //led_green = 1;
00060     return;
00061 }
00062 
00063 int main()
00064 {
00065     //led_green = 1;
00066     pc.baud(115200);
00067 
00068     InitMatrix(&path, 6, 100);
00069     InitMatrix(&p0, 6, 1);
00070     InitMatrix(&pf, 6, 1);
00071 
00072     InitMatrix(&p, 6, 1);
00073     InitMatrix(&q, 6, 1);
00074 
00075     p0.element[0][0]= 0.4;
00076     p0.element[1][0]= 0;
00077     p0.element[2][0]= 0.4;
00078     p0.element[3][0]= 0;
00079     p0.element[4][0]= 0;
00080     p0.element[5][0]= 0;
00081 
00082     pf.element[0][0]= 0.7;
00083     pf.element[1][0]= 0.4;
00084     pf.element[2][0]= -0.1;
00085     pf.element[3][0]= PI/4;
00086     pf.element[4][0]= PI/6;
00087     pf.element[5][0]= -PI/3;
00088 
00089     GeneratePath(&p0,&pf,&path);
00090     while(true) {
00091         check_sw2();
00092         wait(0.1);
00093     }
00094 }