Diff: main.cpp

Revision:

20:70d651156779

Parent:

19:4b993a9a156e

diff -r 4b993a9a156e -r 70d651156779 main.cpp
--- a/main.cpp	Sun Apr 07 19:26:07 2013 +0000
+++ b/main.cpp	Tue Apr 09 15:33:36 2013 +0000
@@ -1,77 +1,18 @@
-//#pragma Otime // Compiler Optimisations
-
-// Eurobot13 main.cpp
-
-
-
-/*
-PINOUT Sensors
-5:  RF:SDI
-6 SDO
-7 SCK
-8 NCS
-9 NIRQ
-10-15 6 echo pins
-16 trig
-17 IRin
-18-20 unused
-21 stepper step
-22-27 unused
-28 Serial TX
-29-30 unused
-
-
-PINOUT Main
-5: Lower arm servo
-6: Upper arm servo
-
-14: Serial RX
-15: Distance sensor
-
-20: color sensor in
-21-24: Motors PWM IN 1-4
-25-26: Encoders 
-27-28: Encoders 
-29: Color sensor RED LED
-30: Color sensor BLUE LED
-
-*/
+#include "globals.h"
+#include "Kalman.h"
 #include "mbed.h"
 #include "rtos.h"
-#include "globals.h"
-Serial pc(USBTX, USBRX);
-
-const PinName P_SERVO_LOWER_ARM = p5;
-const PinName P_SERVO_UPPER_ARM = p6;
-
-const PinName P_SERIAL_RX       = p14;
-const PinName P_DISTANCE_SENSOR = p15;
-
-const PinName P_COLOR_SENSOR_IN = p20;
-
-const PinName P_MOT_RIGHT_A     = p21;
-const PinName P_MOT_RIGHT_B     = p22;
-const PinName P_MOT_LEFT_A      = p23;
-const PinName P_MOT_LEFT_B      = p24;
-
-const PinName P_ENC_RIGHT_A     = p28;
-const PinName P_ENC_RIGHT_B     = p27;
-const PinName P_ENC_LEFT_A      = p25;
-const PinName P_ENC_LEFT_B      = p26;
-
-const PinName P_COLOR_SENSOR_RED = p29;
-const PinName P_COLOR_SENSOR_BLUE = p30;
-
-pos beaconpos[] = {{0,0}, {0,2}, {3,1}};
-
 #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 "coprocserial.h"
 #include <algorithm>
 
+pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+
 void motortest();
 void encodertest();
 void motorencodetest();
@@ -105,9 +46,9 @@
     //ledphototransistortest();
     //colourtest(); // Red SnR too low
     //cakesensortest();
-    feedbacktest();
+    //feedbacktest();
     
-    /*
+     /*
     DigitalOut l1(LED1);
     Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
     l1=1;
@@ -116,11 +57,19 @@
     Thread::wait(osWaitForever);
     */
     
-    //Kalman test threads
-    //Ticker predictticker;
-    //predictthread(predictloopwrapper, this, osPriorityNormal, 512)
-    //updatethread(updateloopwrapper, this, osPriorityNormal, 512)
-    //predictticker( SIGTICKARGS(predictthread, 0x1) ),
+    
+    InitSerial();
+    //while(1)
+    //    printbuff();
+    wait(1);
+    Kalman::KalmanInit();
+    
+    Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
+    
+    Kalman::start_predict_ticker(&predictthread);
+    //Thread::wait(osWaitForever);
+    feedbacktest();
+    
 }
 
 #include <cstdlib>
@@ -132,7 +81,7 @@
     registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
     while (true){
         for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
-            buffer[i] =ID ;
+            buffer[i] = ID ;
         }
         updateval(ID, buffer, sizeof(buffer)/sizeof(buffer[0]));
         Thread::wait(200);
@@ -154,18 +103,21 @@
 
 
 void feedbacktest(){
-    Encoder Eright(P_ENC_RIGHT_A, P_ENC_RIGHT_B), Eleft(P_ENC_LEFT_A, P_ENC_LEFT_B);
+    //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);
     
-    float Pgain = -0.005;
+    Kalman::State state;
+    
+    float Pgain = -0.01;
     float fwdspeed = -400/3.0f;
     Timer timer;
     timer.start();
     
     while(true){
         float expecdist = fwdspeed * timer.read();
-        float errleft = Eleft.getPoint() - (expecdist*1.05);
-        float errright = Eright.getPoint() - expecdist;
+        state = Kalman::getState();
+        float errleft = left_encoder.getTicks() - (expecdist);
+        float errright = right_encoder.getTicks() - expecdist;
         
         mleft(max(min(errleft*Pgain, 0.4f), -0.4f));
         mright(max(min(errright*Pgain, 0.4f), -0.4f));
@@ -174,12 +126,12 @@
 
 void cakesensortest(){
     wait(1);
-    pc.printf("cakesensortest");
+    printf("cakesensortest");
     
     CakeSensor cs(P_COLOR_SENSOR_IN);
     while(true){
         wait(0.1);
-        pc.printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
+        printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
         }
 }
 
@@ -191,19 +143,19 @@
         ColourEnum ce = c.getColour();
         switch(ce){
             case BLUE :
-                pc.printf("BLUE\n\r");
+                printf("BLUE\n\r");
                 break;
             case RED:
-                pc.printf("RED\n\r");
+                printf("RED\n\r");
                 break;
             case WHITE:
-                pc.printf("WHITE\n\r");
+                printf("WHITE\n\r");
                 break;
             case INCONCLUSIVE:
-                pc.printf("INCONCLUSIVE\n\r");
+                printf("INCONCLUSIVE\n\r");
                 break;
             default:
-                pc.printf("BUG\n\r");
+                printf("BUG\n\r");
         }
     }
 
@@ -219,23 +171,23 @@
         blue = 0; red = 0;
         for(int i = 0; i != 5; i++){
             wait(0.1);
-            pc.printf("Phototransistor Analog is (none): %f \n\r", pt.read());
+            printf("Phototransistor Analog is (none): %f \n\r", pt.read());
         }
     
         blue = 1; red = 0;
         for(int i = 0; i != 5; i++){
             wait(0.1);
-            pc.printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
+            printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
         }
         blue = 0; red = 1;
         for(int i = 0; i != 5; i++){
             wait(0.1);
-            pc.printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
+            printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
         }
         blue = 1; red = 1;
         for(int i = 0; i != 5; i++){
             wait(0.1);
-            pc.printf("Phototransistor Analog is (both): %f \n\r", pt.read());
+            printf("Phototransistor Analog is (both): %f \n\r", pt.read());
         }
     } 
 }
@@ -244,7 +196,7 @@
     AnalogIn pt(P_COLOR_SENSOR_IN); 
     while(true){
         wait(0.1);
-        pc.printf("Phototransistor Analog is: %f \n\r", pt.read());
+        printf("Phototransistor Analog is: %f \n\r", pt.read());
     }
 
 }
@@ -285,8 +237,8 @@
     mleft(speed); mright(speed);
     servoTimer.start();
     while (true){
-        pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
-        if (Eleft.getPoint() < Eright.getPoint()){
+        printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
+        if (Eleft.getTicks() < Eright.getTicks()){
             mleft(speed);
             mright(speed - dspeed);
         } else {
@@ -330,8 +282,8 @@
     while (true){
     //left 27 cm = 113 -> 0.239 cm/pulse
     //right 27 cm = 72 -> 0.375 cm/pulse
-        pc.printf("Position is: %i \t %i \n\r", (int)(Eleft.getPoint()*0.239), (int)(Eright.getPoint()*0.375));
-        if (Eleft.getPoint()*0.239 < Eright.getPoint()*0.375){
+        printf("Position is: %i \t %i \n\r", (int)(Eleft.getTicks()*0.239), (int)(Eright.getTicks()*0.375));
+        if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375){
             mright(speed - dspeed);
         } else {
             mright(speed + dspeed);
@@ -349,13 +301,13 @@
     const int enc = -38;
     while(true){
         mleft(speed); mright(0);
-        while(Eleft.getPoint()>enc){
-            pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
+        while(Eleft.getTicks()>enc){
+            printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
         Eleft.reset(); Eright.reset();
         mleft(0); mright(speed);
-        while(Eright.getPoint()>enc){
-            pc.printf("Position is: %i \t %i \n\r", Eleft.getPoint(), Eright.getPoint());
+        while(Eright.getTicks()>enc){
+            printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
         Eleft.reset(); Eright.reset();
     }
@@ -367,7 +319,7 @@
     Serial pc(USBTX, USBRX);
     while(true){
         wait(0.1);
-        pc.printf("Position is: %i \t %i \n\r", E1.getPoint(), 0);//E2.getPoint());
+        printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
     }
 
 }