Diff: main.cpp

Revision:

9:377560539b74

Parent:

7:f9c59a3e4155

Child:

10:294b9adbc9d3

--- a/main.cpp	Fri Apr 27 18:36:54 2012 +0000
+++ b/main.cpp	Sat Apr 28 17:21:24 2012 +0000
@@ -40,10 +40,10 @@
     //Init kalman
     kalman.KalmanInit();
 
-    Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
+    //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256);
     //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024);
     
-    //Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
+    Thread thr_motion(motion_thread,NULL,osPriorityNormal,1024);
     //Motion_Thread_Ptr = &thr_motion;
 
     //measure cpu usage. output updated once per second to symbol cpupercent
@@ -53,11 +53,80 @@
 
     //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
     while (1) {
+        
+        osThreadSetPriority (osThreadGetId(), osPriorityIdle);
+        
+        Timer timer;
+        ui.regid(10, 1);
+        
+        while(1) {
+            timer.reset();
+            timer.start();
+            nopwait(1000);
+            
+            ui.updateval(10, timer.read_us());
+        }
+        
         // do nothing
         Thread::wait(osWaitForever);
     }
 }
 
+void AI::ai_thread () {
+    /*
+    //printf("aithreadstart\r\n");
+    Thread::signal_wait(0x01);
+    settarget(660, 400, PI/2, true);
+
+    Thread::signal_wait(0x01);
+    settarget(660, 570, PI, true);
+
+    Thread::signal_wait(0x01);
+    settarget(400, 870, PI, true);
+
+    Thread::signal_wait(0x01);
+    settarget(660, 870, PI, false);
+
+    flag_terminate = true;
+    */
+
+    while (1) {
+
+        // goes to the mid
+        Thread::signal_wait(0x01);
+        settarget(1500, 1000, PI/2, true);
+
+        // left roll
+        Thread::signal_wait(0x01);
+        settarget(500, 1500, PI/2, true);
+
+        // mid
+        Thread::signal_wait(0x01);
+        settarget(1500, 1000, PI/2, true);
+
+        // map
+        Thread::signal_wait(0x01);
+        settarget(1500, 1500, PI/2, true);
+
+        // mid
+        Thread::signal_wait(0x01);
+        settarget(1500, 1000, -PI/2, true);
+
+        // home
+        Thread::signal_wait(0x01);
+        settarget(500, 500, 0, true);
+
+    }
+
+    Thread::signal_wait(0x01);
+    flag_terminate = true;
+    //OLED3 = true;
+
+    while (true) {
+        Thread::wait(osWaitForever);
+    }
+}
+
 
 void vMotorThread(void const *argument) {
     motors.resetEncoders();
@@ -105,7 +174,6 @@
         pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
         pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
         pc.printf("IR   : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
-        pc.printf("Angle_Offset: %0.4f \r\n",kalman.ir.angleOffset*180/PI);
         Thread::wait(100);
     }
 }