Diff: main.cpp

Revision:

24:50805ef8c499

Parent:

20:70d651156779

Child:

25:b16f1045108f

--- a/main.cpp	Tue Apr 09 15:33:36 2013 +0000
+++ b/main.cpp	Tue Apr 09 20:37:59 2013 +0000
@@ -10,6 +10,7 @@
 #include "Processes/Printing/Printing.h"
 #include "coprocserial.h"
 #include <algorithm>
+#include "motion.h"
 
 pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
 
@@ -56,7 +57,13 @@
     Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
     Thread::wait(osWaitForever);
     */
-    
+    using AI::current_waypoint;
+    current_waypoint = new Waypoint;
+    current_waypoint->x = 0.5;
+    current_waypoint->y = 0.7;
+    current_waypoint->theta = 0.0;
+    current_waypoint->pos_threshold = 0.02;
+    current_waypoint->angle_threshold = 0.09;
     
     InitSerial();
     //while(1)
@@ -67,8 +74,13 @@
     Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
     
     Kalman::start_predict_ticker(&predictthread);
-    //Thread::wait(osWaitForever);
-    feedbacktest();
+    
+    // motion layer periodic callback
+    RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
+    motion_timer.start(50);
+    
+    Thread::wait(osWaitForever);
+    //feedbacktest();
     
 }
 
@@ -101,7 +113,7 @@
     }
 }
 
-
+/*
 void feedbacktest(){
     //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
     MainMotor mright(P_MOT_RIGHT_A, P_MOT_RIGHT_B), mleft(P_MOT_LEFT_A, P_MOT_LEFT_B);
@@ -123,7 +135,7 @@
         mright(max(min(errright*Pgain, 0.4f), -0.4f));
     }
 }
-
+*/
 void cakesensortest(){
     wait(1);
     printf("cakesensortest");