Diff: source/main.cpp

Revision:

0:fb6e494a7656

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/main.cpp	Wed Apr 20 10:03:58 2016 +0000
@@ -0,0 +1,94 @@
+#include "mbed.h"
+#include "params.h"
+#include "matrix.h"
+#include "kinematic.h"
+#include "pathPlanning.h"
+
+DigitalIn sw2(PC_13);
+Serial pc(USBTX, USBRX);
+//DigitalOut led_green(LED_GREEN);
+
+matrix path;
+matrix p0;
+matrix pf;
+
+matrix p;
+matrix q;
+
+Timer t;
+
+void check_sw2(void)
+{
+    if (sw2 == 0) {
+        //pc.printf("SW2 button pressed. \n");
+        //led_green = 0;
+        t.start();
+        for(int k = 0; k < 1 ; k++) {
+            for(int j = 0; j < path.col; j++) {
+                for(int i = 0; i < 6; i++) {
+                    p.element[i][0] = path.element[i][j];
+                    //pc.printf("%.4f    ", p.element[i][0]);
+                }
+
+                //pc.printf("\n");
+                IPKF(&p, &q);
+                
+                pc.printf("%f    ", q.element[0][0]);
+                pc.printf("\n");
+                
+                //for(int i = 0; i < 6; i++) {
+                //    pc.printf("%.4f    ", q.element[i][0]);
+                //}
+                //pc.printf("\n");
+
+                //DPKF(&q,&p);
+
+                //for(int i = 0; i < 6; i++) {
+                //    pc.printf("%.4f    ", p.element[i][0]);
+                //}
+                //pc.printf("\nj=%d\n", j);
+            }
+        }
+        wait(2);
+        pc.printf("%f", 0.0);
+        wait(2);
+        t.stop();
+        //pc.printf("%f s\n", t.read());
+        t.reset();
+    }
+    //led_green = 1;
+    return;
+}
+
+int main()
+{
+    //led_green = 1;
+    pc.baud(115200);
+
+    InitMatrix(&path, 6, 100);
+    InitMatrix(&p0, 6, 1);
+    InitMatrix(&pf, 6, 1);
+
+    InitMatrix(&p, 6, 1);
+    InitMatrix(&q, 6, 1);
+
+    p0.element[0][0]= 0.4;
+    p0.element[1][0]= 0;
+    p0.element[2][0]= 0.4;
+    p0.element[3][0]= 0;
+    p0.element[4][0]= 0;
+    p0.element[5][0]= 0;
+
+    pf.element[0][0]= 0.7;
+    pf.element[1][0]= 0.4;
+    pf.element[2][0]= -0.1;
+    pf.element[3][0]= PI/4;
+    pf.element[4][0]= PI/6;
+    pf.element[5][0]= -PI/3;
+
+    GeneratePath(&p0,&pf,&path);
+    while(true) {
+        check_sw2();
+        wait(0.1);
+    }
+}
\ No newline at end of file