Diff: main.cpp

Revision:

25:b16f1045108f

Parent:

24:50805ef8c499

Parent:

23:5cfc4789e00b

Child:

28:4e20b44251c6

Child:

30:791739422122

--- a/main.cpp	Tue Apr 09 20:37:59 2013 +0000
+++ b/main.cpp	Wed Apr 10 02:01:51 2013 +0000
@@ -2,17 +2,16 @@
 #include "Kalman.h"
 #include "mbed.h"
 #include "rtos.h"
-#include "Actuators/Arms/Arm.h"
-#include "Actuators/MainMotors/MainMotor.h"
-#include "Sensors/Encoders/Encoder.h"
-#include "Sensors/Colour/Colour.h"
-#include "Sensors/CakeSensor/CakeSensor.h"
-#include "Processes/Printing/Printing.h"
+#include "Arm.h"
+#include "MainMotor.h"
+#include "Encoder.h"
+#include "Colour.h"
+#include "CakeSensor.h"
+#include "Printing.h"
 #include "coprocserial.h"
 #include <algorithm>
 #include "motion.h"
-
-pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+#include "MotorControl.h"
 
 void motortest();
 void encodertest();
@@ -51,13 +50,20 @@
     
      /*
     DigitalOut l1(LED1);
-    Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
+    Thread p(Printing::printingloop,        NULL,   osPriorityNormal,   2048);
     l1=1;
     Thread a(printingtestthread,    NULL,   osPriorityNormal,   1024);
     Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
     Thread::wait(osWaitForever);
     */
+    
+    SystemTime.start();
+    
+    Serial pc(USBTX, USBRX);
+    pc.baud(115200);
+    
     using AI::current_waypoint;
+    
     current_waypoint = new Waypoint;
     current_waypoint->x = 0.5;
     current_waypoint->y = 0.7;
@@ -75,13 +81,19 @@
     
     Kalman::start_predict_ticker(&predictthread);
     
+    Ticker motorcontroltestticker;
+    motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
+    
     // motion layer periodic callback
     RtosTimer motion_timer(motion::motionlayer, osTimerPeriodic);
     motion_timer.start(50);
-    
+
+   
+    Thread::wait(3500);
+    Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
+
     Thread::wait(osWaitForever);
-    //feedbacktest();
-    
+
 }
 
 #include <cstdlib>
@@ -90,12 +102,12 @@
 void printingtestthread(void const*){
     const char ID = 1;
     float buffer[3] = {ID};
-    registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
+    Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
     while (true){
         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
             buffer[i] = ID ;
         }
-        updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
+        Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
         Thread::wait(200);
     }
 }
@@ -103,16 +115,17 @@
 void printingtestthread2(void const*){
     const char ID = 2;
     float buffer[5] = {ID};
-    registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
+    Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
     while (true){
         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
             buffer[i] = ID;
         }
-        updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
+        Printing::updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
         Thread::wait(500);
     }
 }
 
+
 /*
 void feedbacktest(){
     //Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
@@ -136,6 +149,7 @@
     }
 }
 */
+
 void cakesensortest(){
     wait(1);
     printf("cakesensortest");