Diff: main.cpp

Revision:

43:c592bf6a6a2d

Parent:

20:70d651156779

Child:

44:555136de33e4

--- a/main.cpp	Tue Apr 09 15:33:36 2013 +0000
+++ b/main.cpp	Fri Apr 12 16:24:25 2013 +0000
@@ -12,6 +12,7 @@
 #include <algorithm>
 
 pos beaconpos[] = {{0,1}, {3,0}, {3,2}};
+Serial pc(USBTX, USBRX); // tx, rx
 
 void motortest();
 void encodertest();
@@ -20,20 +21,30 @@
 void motorsandservostest();
 void armtest();
 void motortestline();
-void ledtest();
-void phototransistortest();
-void ledphototransistortest();
 void colourtest();
+void pt_test();
 void cakesensortest();
 void printingtestthread(void const*);
 void printingtestthread2(void const*);
 void feedbacktest();
 
-int main() {
-    
-/*****************
- *   Test Code   *
- *****************/
+// bytes packing for peer to peer communication
+typedef union {
+    struct _data {
+        unsigned char header[3];
+        unsigned char ID;
+        float value;
+        float aux;
+    }  data;
+    unsigned char type_char[sizeof(_data)];
+} bytepack_t;
+
+int main()
+{
+
+    /*****************
+     *   Test Code   *
+     *****************/
     //motortest();
     //encodertest();
     //motorencodetest();
@@ -47,8 +58,8 @@
     //colourtest(); // Red SnR too low
     //cakesensortest();
     //feedbacktest();
-    
-     /*
+
+    /*
     DigitalOut l1(LED1);
     Thread p(printingThread,        NULL,   osPriorityNormal,   2048);
     l1=1;
@@ -56,31 +67,37 @@
     Thread b(printingtestthread2,   NULL,   osPriorityNormal,   1024);
     Thread::wait(osWaitForever);
     */
-    
-    
-    InitSerial();
+
+
+    //InitSerial();
     //while(1)
     //    printbuff();
-    wait(1);
-    Kalman::KalmanInit();
-    
-    Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
-    
-    Kalman::start_predict_ticker(&predictthread);
+    //wait(1);
+    // Kalman::KalmanInit();
+
+    //Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
+
+    //Kalman::start_predict_ticker(&predictthread);
     //Thread::wait(osWaitForever);
-    feedbacktest();
-    
+    //feedbacktest();
+    pc.baud(115200);
+    pc.printf("Hello from mbed\n");
+    colourtest();
+    //pt_test();
+    while(true) {
+    }
 }
 
 #include <cstdlib>
 using namespace std;
 
-void printingtestthread(void const*){
+void printingtestthread(void const*)
+{
     const char ID = 1;
     float buffer[3] = {ID};
     registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
-    while (true){
-        for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
+    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]));
@@ -88,12 +105,13 @@
     }
 }
 
-void printingtestthread2(void const*){
+void printingtestthread2(void const*)
+{
     const char ID = 2;
     float buffer[5] = {ID};
     registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
-    while (true){
-        for(size_t i = 1; i != sizeof(buffer)/sizeof(buffer[0]); ++i){
+    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]));
@@ -102,119 +120,135 @@
 }
 
 
-void feedbacktest(){
+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);
-    
+
     Kalman::State state;
-    
+
     float Pgain = -0.01;
     float fwdspeed = -400/3.0f;
     Timer timer;
     timer.start();
-    
-    while(true){
+
+    while(true) {
         float expecdist = fwdspeed * timer.read();
         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));
     }
 }
 
-void cakesensortest(){
+void cakesensortest()
+{
     wait(1);
     printf("cakesensortest");
-    
-    CakeSensor cs(P_COLOR_SENSOR_IN);
-    while(true){
+
+    CakeSensor cs(P_DISTANCE_SENSOR);
+    while(true) {
         wait(0.1);
         printf("distance: %f\t %f\r\n", cs.Distance(),cs.Distanceincm());
-        }
+    }
 }
 
-void colourtest(){
-    Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN);
-    c.Calibrate();
-    while(true){
+void colourtest()
+{
+    //Colour c(P_COLOR_SENSOR_BLUE, P_COLOR_SENSOR_RED, P_COLOR_SENSOR_IN,UPPER);
+    Colour c(P_COLOR_SENSOR_BLUE_LOWER, P_COLOR_SENSOR_RED_LOWER, P_COLOR_SENSOR_IN_LOWER,LOWER);
+
+    while(true) {
         wait(0.1);
+
         ColourEnum ce = c.getColour();
-        switch(ce){
+        switch(ce) {
             case BLUE :
-                printf("BLUE\n\r");
+                printf("BLUE\n");
                 break;
             case RED:
-                printf("RED\n\r");
+                printf("RED\n");
                 break;
             case WHITE:
-                printf("WHITE\n\r");
+                printf("WHITE\n");
                 break;
-            case INCONCLUSIVE:
-                printf("INCONCLUSIVE\n\r");
+            case BLACK:
+                printf("BLACK\n");
                 break;
             default:
-                printf("BUG\n\r");
+                printf("BUG\n");
         }
+
     }
 
 }
 
-
-void ledphototransistortest(){
-    DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
-    AnalogIn pt(P_COLOR_SENSOR_IN); 
-    Serial pc(USBTX, USBRX);
+void pt_test()
+{
+    DigitalOut _blue_led(p10);
+    DigitalOut _red_led(p11);
+    AnalogIn _pt(p19);
+    
+    bytepack_t datapackage;
+    // first 3 bytes of header is used for alignment
+    datapackage.data.header[0] = 0xFF;
+    datapackage.data.header[1] = 0xFF;
+    datapackage.data.header[2] = 0xFF;
+    while(true) {
+        //toggles leds for the next state
+        _blue_led = 1;
+        _red_led = 0;
+        wait(0.01);
+        volatile float blue_temp = _pt.read();
 
-    while(true){
-        blue = 0; red = 0;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (none): %f \n\r", pt.read());
-        }
-    
-        blue = 1; red = 0;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (blue): %f \n\r", pt.read());
-        }
-        blue = 0; red = 1;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (red ): %f \n\r", pt.read());
+
+        datapackage.data.ID = (unsigned char)0;
+        datapackage.data.value = blue_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
         }
-        blue = 1; red = 1;
-        for(int i = 0; i != 5; i++){
-            wait(0.1);
-            printf("Phototransistor Analog is (both): %f \n\r", pt.read());
+
+        _blue_led = 0;
+        _red_led = 1;
+        wait(0.01);
+        volatile float red_temp = _pt.read();
+
+
+        datapackage.data.ID = (unsigned char)1;
+        datapackage.data.value = red_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
         }
-    } 
-}
 
-void phototransistortest(){
-    AnalogIn pt(P_COLOR_SENSOR_IN); 
-    while(true){
-        wait(0.1);
-        printf("Phototransistor Analog is: %f \n\r", pt.read());
-    }
+        _blue_led = 0;
+        _red_led = 0;
+        wait(0.01);
+        volatile float noise_temp = _pt.read();
 
-}
 
-void ledtest(){
-    DigitalOut blue(P_COLOR_SENSOR_BLUE), red(P_COLOR_SENSOR_RED);
-    while(true){
-        blue = 1; red = 0;
-        wait(0.2);
-        blue = 0; red = 1;
-        wait(0.2);
-    
+        datapackage.data.ID = (unsigned char)2;
+        datapackage.data.value = noise_temp;
+        datapackage.data.aux = 0;
+        for (int i = 0; i < sizeof(datapackage); i++) {
+            //mbed_serial.putc(datapackage.type_char[i]);
+            pc.putc(datapackage.type_char[i]);
+        }
+
     }
 }
 
-void armtest(){
+
+
+void armtest()
+{
     Arm white(p26), black(p25, false, 0.0005, 180);
-    while(true){
+    while(true) {
         white(0);
         black(0);
         wait(1);
@@ -225,27 +259,29 @@
 }
 
 
-void motorsandservostest(){
+void motorsandservostest()
+{
     Encoder Eleft(p27, p28), Eright(p30, p29);
     MainMotor mleft(p24,p23), mright(p21,p22);
     Arm sTop(p25), sBottom(p26);
     //Serial pc(USBTX, USBRX);
     const float speed = 0.0;
     const float dspeed = 0.0;
-    
+
     Timer servoTimer;
-    mleft(speed); mright(speed);
+    mleft(speed);
+    mright(speed);
     servoTimer.start();
-    while (true){
+    while (true) {
         printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
-        if (Eleft.getTicks() < Eright.getTicks()){
+        if (Eleft.getTicks() < Eright.getTicks()) {
             mleft(speed);
             mright(speed - dspeed);
         } else {
             mright(speed);
             mleft(speed - dspeed);
         }
-        if (servoTimer.read() < 1){
+        if (servoTimer.read() < 1) {
             sTop.clockwise();
         } else if (servoTimer.read() < 4) {
             sTop.halt();
@@ -257,33 +293,37 @@
             //Led=0;
         } else if (servoTimer.read() < 7) {
             sBottom.halt();
-        }else {
+        } else {
             sTop.anticlockwise();
         }
         if (servoTimer.read() >= 9) servoTimer.reset();
     }
 }
 
-void motortestline(){
+void motortestline()
+{
     MainMotor mleft(p24,p23), mright(p21,p22);
     const float speed = 0.2;
-    mleft(speed); mright(speed);
+    mleft(speed);
+    mright(speed);
     while(true) wait(1);
 }
 
-void motorencodetestline(){
+void motorencodetestline()
+{
     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);
     //Serial pc(USBTX, USBRX);
     const float speed = 0.2;
     const float dspeed = 0.1;
 
-    mleft(speed); mright(speed);
-    while (true){
-    //left 27 cm = 113 -> 0.239 cm/pulse
-    //right 27 cm = 72 -> 0.375 cm/pulse
+    mleft(speed);
+    mright(speed);
+    while (true) {
+        //left 27 cm = 113 -> 0.239 cm/pulse
+        //right 27 cm = 72 -> 0.375 cm/pulse
         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){
+        if (Eleft.getTicks()*0.239 < Eright.getTicks()*0.375) {
             mright(speed - dspeed);
         } else {
             mright(speed + dspeed);
@@ -292,45 +332,55 @@
 
 }
 
-void motorencodetest(){
+void motorencodetest()
+{
     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);
     Serial pc(USBTX, USBRX);
-    
+
     const float speed = -0.3;
     const int enc = -38;
-    while(true){
-        mleft(speed); mright(0);
-        while(Eleft.getTicks()>enc){
+    while(true) {
+        mleft(speed);
+        mright(0);
+        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.getTicks()>enc){
+        Eleft.reset();
+        Eright.reset();
+        mleft(0);
+        mright(speed);
+        while(Eright.getTicks()>enc) {
             printf("Position is: %i \t %i \n\r", Eleft.getTicks(), Eright.getTicks());
         }
-        Eleft.reset(); Eright.reset();
+        Eleft.reset();
+        Eright.reset();
     }
 }
 
-void encodertest(){
+void encodertest()
+{
     Encoder E1(P_ENC_LEFT_A, P_ENC_LEFT_B);
     //Encoder E2(P_ENC_RIGHT_A, P_ENC_RIGHT_B);
     Serial pc(USBTX, USBRX);
-    while(true){
+    while(true) {
         wait(0.1);
         printf("Position is: %i \t %i \n\r", E1.getTicks(), 0);//E2.getTicks());
     }
 
 }
-void motortest(){
+void motortest()
+{
     MainMotor mright(p22,p21), mleft(p23,p24);
     while(true) {
         wait(1);
-        mleft(0.8); mright(0.8);
+        mleft(0.8);
+        mright(0.8);
         wait(1);
-        mleft(-0.2); mright(0.2);
+        mleft(-0.2);
+        mright(0.2);
         wait(1);
-        mleft(0); mright(0);
+        mleft(0);
+        mright(0);
     }
 }
\ No newline at end of file