Diff: main.cpp

Revision:

18:c1cd11db47ed

Parent:

16:a9e0eb97557f

Child:

20:7138ab2f93f7

--- a/main.cpp	Mon May 02 00:58:20 2016 +0000
+++ b/main.cpp	Sun May 15 19:13:53 2016 +0000
@@ -99,7 +99,7 @@
 Ticker controller_ticker;
 
 // Magnetometer variables and functions
-float max_x, max_y, min_x, min_y,x,y;
+float max_x=0, max_y=0, min_x=0, min_y=0,x,y;
 MotionSensorDataUnits mag_data;
 MotionSensorDataCounts mag_raw;
 float processMagAngle();
@@ -118,18 +118,18 @@
     motor.setVelocity(0);
     
     // Protocol parameters
+    printf("Initializing Ethernet!\r\n");
     set_leds_color(RED, red_led, green_led, blue_led);
     eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
     eth.connect();
+    printf("Protocol initialized! \r\n");
     set_leds_color(BLUE, red_led, green_led, blue_led);
     rcv.set_socket();
-    
     gyro.start_measure(GYRO_PERIOD);
     controller_ticker.attach(&control,Ts);
     //main loop
     while(1){
         readProtocol();
-        wait(0.01);
     }
 }
 void control(){
@@ -139,75 +139,64 @@
     if(!rcv.receive())
         return;
     char msg = rcv.get_msg();
+    printf("Message received!");
     switch(msg)
     {
         case NONE:
-            //ser.printf("sending red signal to led\r\n");
-             red_led = LED_ON;
-
+            //ser.printf("sending red signal to led\r\n");  
+            set_leds_color(BLACK,red_led,green_led,blue_led);          
             printf("NONE\r\n");
-
             wait(1);
-            red_led = LED_OFF;
             break;          
         case BRAKE:
             //ser.printf("sending green signal to led\r\n");
-            green_led = LED_ON;
+            set_leds_color(YELLOW,red_led,green_led,blue_led);
             motor.stopJogging();
             printf("BRAKE\r\n");
             motor.brakeMotor();
-            green_led = LED_OFF;
             break;
         case ANG_RST:
             //ser.printf("sending blue signal to led\r\n");
-            blue_led = LED_ON;
-
+            set_leds_color(PURPLE,red_led,green_led,blue_led);
             printf("ANG_RST\r\n");
             gyro.stop_measure();
+            printf("Stopped gyro\r\n");
             gyro.start_measure(GYRO_PERIOD);
-            blue_led = LED_OFF;
+            printf("Starting Gyro\r\n");
             initializeController();
             break;
         case ANG_REF:
-            red_led = LED_ON;
-            green_led = LED_ON;
-
-            reference = rcv.get_ang_ref();// - processMagAngle();
+            set_leds_color(GREEN,red_led,green_led,blue_led);
+            reference = rcv.get_ang_ref() - processMagAngle();
             printf("New reference: %f \n\r",reference*180/PI);
             if(reference > PI)
                 reference = reference - 2*PI;
-            if(reference < -PI)
+            else if(reference < -PI)
                 reference = reference + 2*PI;  
-            red_led = LED_OFF;
-            green_led = LED_OFF;
+            break;
+        case CAM_ANG_REF:
+            set_leds_color(RED,red_led,green_led,blue_led);
+            reference = rcv.get_cam_ang_ref();
+            printf("New reference: %f \n\r",reference*180/PI); 
             break;
         case GND_VEL:
-            red_led = LED_ON;
-            blue_led = LED_ON;
+            set_leds_color(AQUA,red_led,green_led,blue_led);
             float vel = rcv.get_gnd_vel();
             motor.setVelocity(vel);      
             printf("GND_VEL = %f\r\n", vel);
-
-            red_led = LED_OFF;
-            blue_led = LED_OFF;
             break;
         case JOG_VEL:
-            red_led = LED_ON;
-            blue_led = LED_ON;
-
+            set_leds_color(WHITE,red_led,green_led,blue_led);
             float p, r;
             rcv.get_jog_vel(&p,&r);
+            printf("Joggin with period %f and duty cycle %f\r\n",p,r);
             if(p == 0 || r == 0)
                 motor.stopJogging();
             else
                 motor.startJogging(r,p);
-            red_led = LED_OFF;
-            blue_led = LED_OFF;
             break;
         case PID_PARAMS:
-            blue_led = LED_ON;
-            green_led = LED_ON;
-
+            set_leds_color(RED,red_led,green_led,blue_led);
             float ar[4];
             rcv.get_pid_params(ar);
             P = ar[0];
@@ -218,26 +207,20 @@
                 ar[0], ar[1], ar[2], ar[3]);
 
             wait(1);
-            blue_led = LED_OFF;
-            green_led = LED_OFF;
             break;
         case MAG_CALIB:
+            set_leds_color(BLUE,red_led,green_led,blue_led);
+            printf("MAG_CALIB\r\n");
             float mag[4];
             rcv.get_mag_calib(mag);
             max_x=mag[1];
             max_y=mag[3];
             min_x=mag[0];
             min_y=mag[2];
+            printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
             break;
         default:
-            blue_led = LED_ON;
-            green_led = LED_ON;
-            red_led = LED_ON;
             printf("nothing understood\r\n");
-            wait(1);
-            blue_led = LED_OFF;
-            green_led = LED_OFF;
-            red_led = LED_OFF;
             //ser.printf("unknown command!\r\n");
     }
 }
@@ -312,8 +295,23 @@
 
 /* Function to transform the magnetometer reading in angle(rad/s).*/
 float processMagAngle(){
-    mag.getAxis(mag_data);
-    x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
-    y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
-    return -atan2(y,x);
+    printf("starting ProcessMagAngle()\n\r");
+    float mag_lpf = 0;
+    Timer t1;
+    for(int i = 0; i<20; i++){
+        //printf("\r\n");
+        //wait(0.1);
+        t1.start();
+        __disable_irq();
+        mag.getAxis(mag_data);
+        __enable_irq();
+        t1.stop();
+        x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
+        y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
+        mag_lpf = mag_lpf + (-atan2(y,x));
+        wait(0.015);
+        }
+   // wait(20*0.01);
+    printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
+    return mag_lpf/20;
 }
\ No newline at end of file