Diff: main.cpp

Revision:

50:937e860f4621

Parent:

49:665bdca0f2cd

Parent:

46:adcd57a5e402

Child:

51:bc261eae004b

--- a/main.cpp	Fri Apr 12 21:07:00 2013 +0000
+++ b/main.cpp	Fri Apr 12 21:26:02 2013 +0000
@@ -22,20 +22,32 @@
 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;
+
+Serial pc(USBTX, USBRX);
+
+int main()
+{
+
+    /*****************
+     *   Test Code   *
+     *****************/
     //motortest();
     //encodertest();
     //motorencodetest();
@@ -49,8 +61,8 @@
     //colourtest(); // Red SnR too low
     //cakesensortest();
     //feedbacktest();
-    
-     /*
+
+    /*
     DigitalOut l1(LED1);
     Thread p(Printing::printingloop,        NULL,   osPriorityNormal,   2048);
     l1=1;
@@ -61,13 +73,11 @@
     
     SystemTime.start();
     
-    Serial pc(USBTX, USBRX);
+    
     pc.baud(115200);
-    
+    wait(2);
     InitSerial();
-    //while(1)
-    //    printbuff();
-    wait(1);
+    wait(3);
     Kalman::KalmanInit();
     
     Thread predictthread(Kalman::predictloop, NULL, osPriorityNormal, 2084);//512); //temp 2k
@@ -77,7 +87,6 @@
     
     Ticker motorcontroltestticker;
     motorcontroltestticker.attach(MotorControl::motor_control_isr, 0.05);
-
     // ai layer thread
     Thread aithread(AI::ailayer, NULL, osPriorityNormal, 2048);
     
@@ -88,14 +97,18 @@
     Thread printingThread(Printing::printingloop, NULL, osPriorityLow, 2048);
 
     measureCPUidle(); //repurpose thread for idle measurement
+    //colourtest();
+    //pt_test();
+    while(true) {
     Thread::wait(osWaitForever);
-
+    }
 }
 
 #include <cstdlib>
 using namespace std;
 
-void printingtestthread(void const*){
+void printingtestthread(void const*)
+{
     const char ID = 1;
     float buffer[3] = {ID};
     Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
@@ -108,7 +121,8 @@
     }
 }
 
-void printingtestthread2(void const*){
+void printingtestthread2(void const*)
+{
     const char ID = 2;
     float buffer[5] = {ID};
     Printing::registerID(ID,sizeof(buffer)/sizeof(buffer[0]));
@@ -126,117 +140,131 @@
 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_upper(P_COLOR_SENSOR_BLUE_UPPER, P_COLOR_SENSOR_RED_UPPER, P_COLOR_SENSOR_IN_UPPER,UPPER);
+    Colour c_lower(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(c_lower.getColour()) {
             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(p18);
+    
+    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);
@@ -247,27 +275,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();
@@ -279,33 +309,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);
@@ -314,45 +348,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